mola_state_estimation_simple

Simple kinematic state vector extrapolation algorithm to fuse odometry sources.

This repository provides:

• NavStateFuse: C++ class to integrate odometry, IMU, and pose/twist estimations.

See package documentation.

Build and install

Refer to the root MOLA repository.

License

This package is released under the GNU GPL v3 license. Other options available upon request.

Changelog for package mola_state_estimation_simple

3.0.3 (2026-10-06)

• Simple estimator: dimensionally consistent constant-velocity prediction covariance
• Fix deprecated CPose3D::getYawPitchRoll() and unchecked nodiscard load results
• Contributors: Jose Luis Blanco-Claraco

3.0.2 (2026-10-01)

3.0.1 (2026-09-28)

3.0.0 (2026-09-28)

• StateEstimationSimple: optional inertial propagation between pose updates
• Port to MRPT 3.x
• Never fuse a dataset's ground truth; add relative-pose factors to fuse_pose()
• Apply a planar odometry increment in the yaw-only frame
• Fuse odometry in timestamp order, not on arrival
• Don't let the twist seed get clobbered on first fuse
• Apply params.initial_twist (MOLA_INITIAL_VX)
• Make wheel-odometry motion-model sigmas tunable; fix pre-anchor odometry baseline bug
• StateEstimationSimple: implement transform_frame()
• Keep buffered IMU readings across reset()
• Fuse IMU readings by timestamp, not on arrival
• Declare mola_yaml as an explicit dependency
• Optional georeferencing ENU custom origin; tunable GNSS Huber kernel threshold
• Contributors: Jose Luis Blanco-Claraco

2.4.2 (2026-06-04)

• feat(#33): velocity filter now handles multi-rate interleaved sources via per-component clocks, fixing linear velocity starvation when LiDAR pose stamps lag IMU stamps
• feat: velocity filter enabled by default (set velocity_filter_enabled: false to restore legacy behavior)
• feat: opt-in CSV instrumentation via MOLA_VEL_FILTER_DUMP and MOLA_NAVSTATE_DUMP env vars
• fix: missing vel_filter_P reset
• test: multi-rate interleaved velocity regression test
• Contributors: Jose Luis Blanco-Claraco

2.4.1 (2026-06-02)

• Merge pull request #32 from MOLAorg/feat/1d-kalman-velocities feat: new 1D kalman velocity filter
• feat: new 1D kalman velocity filter
• Contributors: Jose Luis Blanco-Claraco

2.4.0 (2026-05-11)

• Merge pull request #30 from MOLAorg/bump-cmake-version bump min req cmake version to 3.22

• bump min req cmake version to 3.22

• Merge pull request #29 from MOLAorg/fix/odometry-fuse-pose-twist-corruption fix(state_estimation_simple): correct twist and initial-guess corruption when wheel odometry is active

• Add more unit test cases

• fix: odom twist does not overwrite IMU wx/wy

• fix(state_estimation_simple): correct twist and initial-guess corruption when wheel odometry is active Three inter-related bugs caused the LiDAR adaptive threshold sigma to grow toward maximum_sigma whenever wheel odometry was fused:

  1. Wrong twist from fuse_pose() when odometry is active. fuse_pose() computed incrPose = new_ICP - last_pose, but last_pose had been modified by fuse_odometry() / fuse_odometry_3d_pose() between scans. The result was the odometry residual / dt instead of the true robot velocity, corrupting de-skewing and the rot_error term in the adaptive threshold. Fix: per-source SourceState in State tracks each source's own last absolute pose and timestamp. fuse_pose() now computes incrPose = new_ICP - src.last_pose, which is always the true ICP-to-ICP motion regardless of intervening odom updates.

  2. CObservationRobotPose (3D odom path) overwrote last_pose_obs_tim. When BridgeROS2 forwards wheel odometry as CObservationRobotPose (odometry_as_robot_pose_observation=true), onNewObservation() routed it to fuse_pose(), which updated last_pose_obs_tim to the odom timestamp. If the next LiDAR ICP fuse_pose() arrived with a slightly earlier timestamp, dt < 0 and the call was silently rejected. last_pose was then the absolute wheel odometry pose (in the odom frame), producing ~90-degree

File truncated at 100 lines see the full file