diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
index c853426d8592809cbd2f94ec65202762d153e753..574e4277e9181ee768b9bb08df1ba3f4d5082dd0 100644
--- a/src/sensor/sensor_odom_2d.cpp
+++ b/src/sensor/sensor_odom_2d.cpp
@@ -36,6 +36,7 @@ double SensorOdom2d::getRotVarToRotNoiseFactor() const
 
 Eigen::Matrix2d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
 {
+    assert(_data.size() == 2);
     double d = fabs(_data(0));
     double r = fabs(_data(1));