State estimators

1. Theory

State Estimation (SE) comprises finding the vehicle kinematic state(s) that best explain the imperfect, noisy sensor readings.

Write me!

What? Why? How?

2. Selecting the S.E. method in launch files

2.1. Launching the state estimator standalone

Next we show different possible use cases.

Fusing two nav_msgs/Odometry sources (e.g. wheel + visual odometry)

This demo subscribes to two nav_msgs/Odometry topics from ROS 2, fuses them in the sliding-window factor graph smoother alongside an optional IMU, and publishes the fused result back as nav_msgs/Odometry + /tf.

Each odometry topic is assigned a distinct output_sensor_label; the smoother treats them as independent odometry frames and estimates the optimal T_map_to_odom_X transform for each one.

Step 1 — Start fake sensor publishers (for testing without a real robot):

# Fake wheel odometry: 2% systematic scale drift in X, 50 Hz
python3 $(ros2 pkg prefix mola_demos)/share/mola_demos/demos/fake_wheel_odom_publisher.py

# Fake visual/lidar odometry: 3 mm/step lateral drift, 30 Hz
python3 $(ros2 pkg prefix mola_demos)/share/mola_demos/demos/fake_visual_odom_publisher.py

# Fake IMU: gravity-aligned accelerometer + gyroscope, 100 Hz
python3 $(ros2 pkg prefix mola_demos)/share/mola_demos/demos/fake_imu_publisher.py

Both fake odometry nodes share the same ground-truth motion (circle at vx=1 m/s, wz=0.2 rad/s) but have different noise and drift characteristics so that the smoother can demonstrate visible fusion benefit.

Step 2 — Launch the smoother:

ros2 launch mola_state_estimation_smoother ros2-fuse-two-odometries.launch.py \
  odom1_topic:=/wheel_odom \
  odom2_topic:=/visual_odom \
  imu_topic:=/imu

Or using mola-cli directly (set topic names via environment variables):

ODOM1_TOPIC=/wheel_odom \
ODOM2_TOPIC=/visual_odom \
IMU_TOPIC=/imu \
  mola-cli $(ros2 pkg prefix mola_state_estimation_smoother)/share/mola_state_estimation_smoother/mola-cli-launchs/state_estimator_ros2.yaml

Key launch arguments:

Argument	Default	Description
`odom1_topic`	`/wheel_odom`	First `nav_msgs/Odometry` topic (e.g. wheel encoders)
`odom2_topic`	`/visual_odom`	Second `nav_msgs/Odometry` topic (e.g. visual/LiDAR odometry)
`imu_topic`	`/imu`	IMU topic for gravity alignment
`gnss_topic`	`/gps`	Optional GNSS topic for geo-referencing
`enforce_planar_motion`	`False`	Constrain z=0, pitch=0, roll=0 for ground vehicles
`use_mola_gui`	`True`	Show MolaViz visualization

Step 3 — Verify fused output:

# Fused pose as nav_msgs/Odometry:
ros2 topic echo /state_estimation/pose

# Inspect all published topics:
ros2 topic list | grep state_estimation

# View /tf tree:
ros2 run tf2_tools view_frames

2.2. Launching the state estimator + LO/LIO

In the context of launching LiDAR odometry (LO) mapping or localization as explained here, note that default configurations include StateEstimationSimple as the method of choice, but it can be changed as follows:

3. API and supported inputs

Write me!

imgs/mola_state_estimation_api_overview.webp

4. Implementation: Simple estimator

Write me!

5. Implementation: Factor graph smoother

The package mola_state_estimation_smoother implements a sliding window optimization over the last few keyframes and sensor observations (odometry sources, IMU, GNNS) in order to being able to solve for the optimal kinematic state (pose + velocity) at any desired time point, interpolating or extrapolating into the past or future.

When run as a MOLA module (e.g. within a ROS 2 node), it also publishes the estimated fused pose information in a timely manner, for use as the high-quality, robust localization source.

This package follows this frame convention (see other /tf configurations when using MOLA LiDAR-odometry without state estimation):

https://mrpt.github.io/imgs/mola_mrpt_ros_frames_fusion.png

This is who is responsible of publishing each transformation:

odom_{i} → base_link: One or more odometry sources.
map → base_link: Published by this state estimation package (mola_state_estimation_smoother).
enu → {map, utm}: Published by either:
- mola_lidar_odometry map loading service if fed with a geo-referenced metric map (.mm) file; or
- mola_state_estimation_smoother (this package) if geo-referencing is to be estimated at run-time; or
- mrpt_map_server (github) if set to publish a geo-referenced map.

Add me: Pictures of factor graph model.

Write me: concept of adding temporary keyframes for querying the pose at a given time.

5.1. Kinematic factors

Between two consecutive keyframes close enough in time, a “kinematic factor” is added. Two options are implemented:

A. Free motion kinematic factor

This is actually implemented as the combination of distinct GTSAM factors:

mola::state_estimation_smoother::FactorConstLocalVelocityPose: between linear and the angular velocity components of both keyframes to favor smooth velocities. See line 3 of eq (4) in the MOLA RSS2019 paper.
mola::state_estimation_smoother::FactorTrapezoidalIntegrator: enforces fulfillment of numerical integration on the translational part of SE(3). See line 2 of eq (1) in the MOLA RSS2019 paper.
mola::state_estimation_smoother::FactorAngularVelocityIntegration: enforces the fulfillment of numerical integration on the rotational part of SE(3). See line 1 of eq (4) in the MOLA RSS2019 paper.

B. Tricycle model kinematic factor

This is actually implemented as the combination of distinct GTSAM factors:

mola::state_estimation_smoother::FactorConstLocalVelocityPose: between linear and the angular velocity components of both keyframes to favor smooth velocities. See line 3 of eq (4) in the MOLA RSS2019 paper.
mola::state_estimation_smoother::FactorTricycleModelIntegrator: enforces fulfillment of numerical integration assuming the robot moves following the part of SE(3). TODO: Write equations!
gtsam::PriorFactor: to (gently) favor null components of the local velocity components vy, vz, wx, wy. Parameters can be used to tune how much these soft constraints are allowed to be broken, i.e. depending on how much wheel slippage exists.