diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
index 5f2fc1a717b7c58f985ede8845ea2de01dc17fcb..ca06c7c010bc153756650f35c55e01114fdf4e34 100644
--- a/include/core/sensor/sensor_odom_2d.h
+++ b/include/core/sensor/sensor_odom_2d.h
@@ -62,6 +62,25 @@ class SensorOdom2d : public SensorBase
          **/
         double getRotVarToRotNoiseFactor() const;
 
+        /**
+         * Compute covariance of odometry given the motion data.
+         *
+         * NOTE: This is a helper function for the user, not called automatically anywhere.
+         *
+         * computeCovFromMotion() produces a diagonal covariance that depends on the amount of motion:
+         *  - a linear dependence with total displacement
+         *  - a linear dependence with total rotation
+         *
+         *  The formula for the variances is as follows:
+         *
+         *    - disp_var = k_disp_to_disp * displacement
+         *    - rot_var  = k_rot_to_rot   * rotation
+         *
+         * See implementation for details.
+         */
+        Matrix3d computeCovFromMotion (const VectorXd& _data) const;
+
+
 };
 
 } // namespace wolf
diff --git a/include/core/sensor/sensor_odom_3d.h b/include/core/sensor/sensor_odom_3d.h
index ab0e8b46872c21015b02ffcb9d521dbd77e2a606..d5f5b417eb36a0628a3425822d03b3fef494fb36 100644
--- a/include/core/sensor/sensor_odom_3d.h
+++ b/include/core/sensor/sensor_odom_3d.h
@@ -72,6 +72,26 @@ class SensorOdom3d : public SensorBase
         double getMinDispVar() const;
         double getMinRotVar() const;
 
+        /**
+         * Compute covariance of odometry given the motion data.
+         *
+         * NOTE: This is a helper function for the user, not called automatically anywhere.
+         *
+         * computeCovFromMotion() produces a diagonal covariance that depends on the amount of motion:
+         *  - a minimal value for displacement
+         *  - a minimal value for rotation
+         *  - a linear dependence with total displacement
+         *  - a linear dependence with total rotation
+         *
+         *  The formula for the variances is as follows:
+         *
+         *    - disp_var = disp_var_min + k_disp_to_disp * displacement
+         *    - rot_var  = rot_var_min  + k_disp_to_rot  * displacement + k_rot_to_rot * rotation
+         *
+         * See implementation for details.
+         */
+        Matrix6d computeCovFromMotion (const VectorXd& _data) const;
+
 };
 
 inline double SensorOdom3d::getDispVarToDispNoiseFactor() const
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
index ec7a3b54d3f8163f44276bf8120cb482a3a2fdd7..a3fe8ee9aa2a81849fb0caa2789d785cdafcf22a 100644
--- a/src/sensor/sensor_odom_2d.cpp
+++ b/src/sensor/sensor_odom_2d.cpp
@@ -34,6 +34,17 @@ double SensorOdom2d::getRotVarToRotNoiseFactor() const
     return k_rot_to_rot_;
 }
 
+Eigen::Matrix3d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
+{
+    double d = fabs(_data(0));
+    double r = fabs(_data(1));
+
+    double dvar = k_disp_to_disp_ * d;
+    double rvar = k_rot_to_rot_   * r;
+
+    return (Matrix3d() << dvar, dvar, rvar).finished();
+}
+
 }
 
 // Register in the FactorySensor
diff --git a/src/sensor/sensor_odom_3d.cpp b/src/sensor/sensor_odom_3d.cpp
index 58b9b62ee9e111ce84887ba16e728a3494e40b7a..5954f505c0daf3e37588e0b78bf2b3b440b1052f 100644
--- a/src/sensor/sensor_odom_3d.cpp
+++ b/src/sensor/sensor_odom_3d.cpp
@@ -9,6 +9,7 @@
 
 #include "core/state_block/state_block.h"
 #include "core/state_block/state_quaternion.h"
+#include "core/math/rotations.h"
 
 namespace wolf {
 
@@ -37,6 +38,21 @@ SensorOdom3d::~SensorOdom3d()
     //
 }
 
+Eigen::Matrix6d SensorOdom3d::computeCovFromMotion (const VectorXd& _data) const
+{
+    double d = _data.head<3>().norm();
+    double r;
+    if (_data.size() == 6)
+        r = _data.tail<3>().norm();
+    else
+        r = 2 * acos(_data(6)); // arc cos of real part of quaternion
+
+    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();
+}
+
 } // namespace wolf
 
 // Register in the FactorySensor