diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
index ca06c7c010bc153756650f35c55e01114fdf4e34..0859f15bcee22d3be1c1857365d03d50f0315962 100644
--- a/include/core/sensor/sensor_odom_2d.h
+++ b/include/core/sensor/sensor_odom_2d.h
@@ -78,7 +78,7 @@ class SensorOdom2d : public SensorBase
          *
          * See implementation for details.
          */
-        Matrix3d computeCovFromMotion (const VectorXd& _data) const;
+        Matrix2d computeCovFromMotion (const VectorXd& _data) const;
 
 
 };
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
index a3fe8ee9aa2a81849fb0caa2789d785cdafcf22a..c853426d8592809cbd2f94ec65202762d153e753 100644
--- a/src/sensor/sensor_odom_2d.cpp
+++ b/src/sensor/sensor_odom_2d.cpp
@@ -34,7 +34,7 @@ double SensorOdom2d::getRotVarToRotNoiseFactor() const
     return k_rot_to_rot_;
 }
 
-Eigen::Matrix3d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
+Eigen::Matrix2d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
 {
     double d = fabs(_data(0));
     double r = fabs(_data(1));
@@ -42,7 +42,7 @@ Eigen::Matrix3d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
     double dvar = k_disp_to_disp_ * d;
     double rvar = k_rot_to_rot_   * r;
 
-    return (Matrix3d() << dvar, dvar, rvar).finished();
+    return (Vector2d() << dvar, rvar).finished().asDiagonal();
 }
 
 }
diff --git a/src/sensor/sensor_odom_3d.cpp b/src/sensor/sensor_odom_3d.cpp
index 5954f505c0daf3e37588e0b78bf2b3b440b1052f..b261029d677a40c7c8b821b1e4708251d4bd38ca 100644
--- a/src/sensor/sensor_odom_3d.cpp
+++ b/src/sensor/sensor_odom_3d.cpp
@@ -50,7 +50,7 @@ Eigen::Matrix6d SensorOdom3d::computeCovFromMotion (const VectorXd& _data) const
     double dvar = min_disp_var_ + k_disp_to_disp_ * d;
     double rvar = min_rot_var_  + k_disp_to_rot_  * d + k_rot_to_rot_ * r;
 
-    return (Matrix6d() << dvar, dvar, dvar, rvar, rvar, rvar).finished();
+    return (Vector6d() << dvar, dvar, dvar, rvar, rvar, rvar).finished().asDiagonal();
 }
 
 } // namespace wolf