Merge pull request #24 from MOLAorg/fix/bugs-in-simple-estimator-cov

Fix: Avoid double covariance increment

Author: jlblancoc

mola_state_estimation_simple/include/mola_state_estimation_simple/StateEstimationSimple.h

@@ -39,7 +39,7 @@
 
 namespace mola::state_estimation_simple
 {
-/** Fuse of odometry, IMU, and SE(3) pose/twist estimations.
+/** Simple motion-model state estimator fusing odometry, IMU, and SE(3) pose/twist.
  *
  * Usage:
  * - (1) Call initialize() or set the required parameters directly in params_.
@@ -50,13 +50,24 @@ namespace mola::state_estimation_simple
  * - (4) Read the estimation up to any nearby moment in time with
  *       estimated_navstate()
  *
- * Old observations are automatically removed.
+ * ## Prior covariance model (estimated_navstate)
  *
- * \note This implementation of mola::NavStateFilter ignores the passed
- *       "frame_id".
- * \note It also ignore GNSS sensor.
+ * Given `dt` seconds elapsed since the last `fuse_pose()` call, the returned
+ * prior pose covariance diagonal is:
+ *
+ *   cov_xyz = sigma_relative_pose_linear^2
+ *           + (sigma_random_walk_acceleration_linear * dt)^2
+ *
+ *   cov_rot = sigma_relative_pose_angular^2
+ *           + (sigma_random_walk_acceleration_angular * dt)^2
  *
- * \sa mola::IMUIntegrator
+ * `sigma_relative_pose_linear` [m] is a dt-independent floor on position
+ * uncertainty and is the primary knob for tightening the ICP prior.
+ * `sigma_random_walk_acceleration_linear` [m/s^2] adds time-growing
+ * uncertainty due to unmodeled accelerations.
+ *
+ * \note This implementation of mola::NavStateFilter ignores the passed
+ *       "frame_id" and GNSS observations.
  *
  * \ingroup mola_state_estimation_grp
  */

mola_state_estimation_simple/src/StateEstimationSimple.cpp

@@ -312,21 +312,14 @@ std::optional<NavState> StateEstimationSimple::estimated_navstate(
         cov(i, i) += varRot;
     }
 
-    if (state_.last_twist_cov.has_value())
-    {
-        auto twistCov = state_.last_twist_cov.value();
-        twistCov *= dt * dt;
-        cov += twistCov;
-
-        for (int i = 0; i < 3; i++)
-        {
-            (*state_.last_twist_cov)(i, i) += varXYZ;
-        }
-        for (int i = 3; i < 6; i++)
-        {
-            (*state_.last_twist_cov)(i, i) += varRot;
-        }
-    }
+    // sigma_rel is a position-domain quantity (meters): add it directly as
+    // position variance, independent of the fuse_pose/query dt ratio.
+    cov(0, 0) += mrpt::square(params.sigma_relative_pose_linear);
+    cov(1, 1) += mrpt::square(params.sigma_relative_pose_linear);
+    cov(2, 2) += mrpt::square(params.sigma_relative_pose_linear);
+    cov(3, 3) += mrpt::square(params.sigma_relative_pose_angular);
+    cov(4, 4) += mrpt::square(params.sigma_relative_pose_angular);
+    cov(5, 5) += mrpt::square(params.sigma_relative_pose_angular);
 
     ret.pose.cov_inv = cov.inverse_LLt();