State estimators

1. Theory

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

What? In MOLA, the kinematic state of the vehicle at time \(t\) is

\[\mathbf{x}(t) = \left( \mathbf{T}(t),\ \mathbf{v}(t),\ \boldsymbol{\omega}(t) \right)\]

where \(\mathbf{T}(t) \in SE(3)\) is the pose of the vehicle (base_link) in a reference frame (typically map), and \(\mathbf{v}(t), \boldsymbol{\omega}(t)\) are its linear and angular velocity expressed in the vehicle’s own frame (the body twist). A state estimator answers “what is \(\mathbf{x}(t)\) and how uncertain is it?” for any time \(t\) close to the latest measurements, in any of the frames it knows about.

Why?

No single sensor is enough: wheel, visual and LiDAR odometry drift over time; GNSS is absolute but noisy, slow, and sometimes unavailable; an IMU gives attitude and rates, but positions integrated from it drift within seconds.
Sensors run at different rates and with different latencies, while consumers need the state at their own timestamps: LiDAR odometry needs a motion prior at each scan time (ICP initial guess, scan de-skewing), a controller needs a smooth, high-rate pose.
Each estimate comes with a covariance, so it can be used as a properly weighted prior by other modules.

How? By default, both estimators share a constant velocity motion model between measurements (StateEstimationSimple can optionally integrate the IMU instead, see imu_propagation in section 4):

\[\begin{split}\mathbf{T}(t + \Delta t) = \mathbf{T}(t) \oplus \exp\!\left( \begin{bmatrix} \mathbf{v}(t) \\ \boldsymbol{\omega}(t) \end{bmatrix} \Delta t \right)\end{split}\]

where unmodeled accelerations are treated as zero-mean white noise (parameters sigma_random_walk_acceleration_linear and _angular), so uncertainty grows with \(\Delta t\). This model is used to interpolate or extrapolate the state to any requested time. Requests farther than max_time_to_use_velocity_model from the latest data return no estimate.

The two implementations differ in how measurements are combined:

StateEstimationSimple (section 4) keeps only the latest state: each new pose replaces the previous one, and velocities are low-pass filtered. Cheap and robust, but it does not really fuse redundant sources.
StateEstimationSmoother (section 5) solves a maximum a posteriori (MAP) problem over a sliding window of keyframes:

\[\mathcal{X}^\star = \arg\min_{\mathcal{X}} \sum_j \rho_j\!\left( \left\| \mathbf{r}_j(\mathcal{X}_j) \right\|^2_{\boldsymbol{\Sigma}_j} \right), \qquad \|\mathbf{r}\|^2_{\boldsymbol{\Sigma}} = \mathbf{r}^\top \boldsymbol{\Sigma}^{-1} \mathbf{r}\]

The variables \(\mathcal{X}\) are \(\mathbf{T}_k, \mathbf{v}_k, \boldsymbol{\omega}_k\) for every keyframe \(k\) in the last sliding_window_length seconds, plus the pose of each odometry frame in map (T_map_to_odom_i) and the geo-reference (T_enu_to_map). Each factor \(\mathbf{r}_j\) is either a sensor measurement or a kinematic constraint between consecutive keyframes, with covariance \(\boldsymbol{\Sigma}_j\); \(\rho_j\) is the identity (Gaussian noise) or a Huber kernel for robust factors. The problem is solved incrementally with iSAM2 (GTSAM), and keyframes leaving the window are marginalized out, so the cost per update stays bounded. Since all states within the window are re-estimated as new evidence arrives, the smoother can also estimate quantities only observable over time, such as the drift of each odometry frame with respect to map, or the geo-reference.

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

2.1. Try it yourself: simulated sensors + RViz

No robot or dataset is needed for a first look at the smoother. This demo runs a simulated robot driving a circle of 5 m radius at 1 m/s, publishing noisy wheel odometry, drifting visual odometry, IMU and GNSS, plus its ground truth. The smoother fuses the sensor combination selected with mode, and RViz shows the result:

ros2 launch mola_state_estimation_smoother ros2-demo-simulated-sensors.launch.py \
  mode:=wheels_imu

_images/state_estimation_demo_rviz.webp — `mode:=wheels_imu` after one lap: ground truth (white), raw wheel odometry (red), raw visual odometry (orange, not fused in this mode), fused trail (green), and the current fused pose with its position covariance (blue).

`mode`	Fused sensors, and what to look at
`wheels_imu` (default)	Wheel odometry + IMU. The wheel odometry heading drifts; the IMU attitude keeps the fused heading, so the fused trail stays on the ground truth while the raw odometry spirals away. Position uncertainty grows, since nothing observes absolute position.
`wheels_imu_gnss`	Wheel odometry + IMU + GNSS, estimating the geo-reference (`enu -> map`) online. The ground truth, given in `enu`, is only drawn once the geo-reference converges, and through that estimate, so it also shows its error.
`two_odometries`	Wheel odometry + drifting visual odometry + IMU: two odometry chains, each with its own `T_map_to_odom_i`.
`imu_gnss`	IMU + GNSS only. Without odometry, the trajectory between GNSS fixes relies on the IMU and the constant velocity model alone; compare it with the other modes.

Other launch arguments: use_rviz (default True), use_mola_gui (MolaViz console, default False), and the simulated noise wheel_odom_ang_sigma (yaw rate noise, default 0.15 rad/s) and visual_odom_drift (lateral drift, default 0.08 m/s). Smoother parameters can be overridden as explained in the next section, e.g. prefix the command with MOLA_REL_POSE_INCR_SIGMA_LIN=0.05 to see the effect of trusting each odometry increment less.

Under the hood, this launch file runs the synthetic sensor publisher from mola_demos and includes ros2-state-estimator.launch.py, the same one used with real sensors below.

2.2. Launching the state estimator standalone

Next we show different possible use cases. All of them run StateEstimationSmoother inside a mola-cli process, with mola::BridgeROS2 subscribing to the sensor topics and publishing the fused result back to ROS 2. See section 3 for how each message type is fused.

Smoother parameters are read from params/state-estimation-smoother.yaml. Most of them can be overridden via the environment variables named in that file, or a whole custom copy of the file can be used instead:

MOLA_STATE_ESTIMATOR_YAML=/path/to/my-smoother-params.yaml \
  ros2 launch mola_state_estimation_smoother ros2-state-estimator.launch.py [...]

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 the fake sensor publisher (for testing without a real robot):

# Wheel odom (50 Hz) + visual odom (30 Hz, Y drift) + IMU (100 Hz),
# all from a single script, circular motion at vx=1 m/s, wz=0.2 rad/s:
python3 $(ros2 pkg prefix mola_demos)/share/mola_demos/demos/fake_sensor_publisher.py \
  --ros-args \
  -p scenario:=circle \
  -p odom2_topic:=/visual_odom \
  -p imu_topic:=/imu

The two odometry streams share the same ground-truth circular motion but have different noise and drift characteristics so the smoother can demonstrate visible fusion benefit. The script supports three scenarios via scenario:= (circle, moving, static) and can also publish GNSS by setting gnss_topic:=/gps.

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_name:=/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

Tip

Odometry sources are fused as chains of increments, whose uncertainty is set by relative_pose_increment_sigma_lin / _ang (env vars MOLA_REL_POSE_INCR_SIGMA_LIN / _ANG), not by the covariance in the messages. Tune them for each platform. See section 3.

Key launch arguments:

Argument	Default	Description
`odom1_topic`	`/wheel_odom`	First `nav_msgs/Odometry` topic (e.g. wheel encoders)
`odom1_label`	`wheel_odom`	Sensor label (and odometry frame name) of the first source
`odom2_topic`	`/visual_odom`	Second `nav_msgs/Odometry` topic (e.g. visual/LiDAR odometry)
`odom2_label`	`visual_odom`	Sensor label (and odometry frame name) of the second source
`imu_topic_name`	`/imu`	IMU topic (empty to disable)
`gnss_topic_name`	(empty)	Optional GNSS `NavSatFix` topic (empty to disable)
`enforce_planar_motion`	`False`	Constrain z=0, pitch=0, roll=0 for ground vehicles
`use_mola_gui`	`True`	Show MolaViz visualization
`use_rviz`	`False`	Also launch RViz2

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.3. 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

_images/mola_state_estimation_api_overview.webp

3.1. C++ API

Both estimators implement the virtual interface mola::NavStateFilter:

Method	Purpose
`fuse_pose(t, pose, frame_id)`	SE(3) pose with covariance of the vehicle at time `t`, relative to frame `frame_id` (`map`, or a source’s own odometry frame).
`fuse_odometry(obs, name)`	Planar wheel odometry (`CObservationOdometry`); increment uncertainty comes from a motion model.
`fuse_imu(obs)`	IMU reading (`CObservationIMU`): orientation, acceleration and/or angular velocity.
`fuse_gnss(obs)`	GNSS fix (`CObservationGPS`).
`fuse_twist(t, twist, cov)`	Body velocity measurement.
`estimated_navstate(t, frame_id)`	Returns a mola::NavState: pose with its information matrix in `frame_id`, plus body twist with its information matrix. Empty if there is not enough data yet, or `t` is too far from the latest data.

Optional methods: estimated_trajectory(), has_converged_localization(), set_geo_reference() / get_geo_reference().

When running as a MOLA module (e.g. inside mola-cli), an estimator:

receives raw observations from the module(s) named in its raw_data_source (e.g. BridgeROS2, or a dataset source), and dispatches them by type as in the table below. Sensor labels are filtered with the regular expressions do_process_imu_labels_re, do_process_odometry_labels_re and do_process_gnss_labels_re. Observations labeled ground_truth are ignored unless fuse_ground_truth_label: true.
receives fuse_pose() calls and estimated_navstate() queries from other modules. For example, mola::LidarOdometry finds the estimator module in the system, feeds it its ICP poses, and uses its predictions as motion prior.

3.2. Inputs from ROS 2

mola::BridgeROS2 converts subscribed topics into MOLA observations, labeled with each topic’s output_sensor_label. IMU and GNSS observations carry the sensor pose on the vehicle, taken from /tf (or from fixed_sensor_pose if use_fixed_sensor_pose: true).

ROS 2 message	`StateEstimationSmoother`	`StateEstimationSimple`
`nav_msgs/Odometry` (default: becomes a `CObservationRobotPose`)	`fuse_pose()` in the source’s own frame (named after its label). Odometry drifts, so only the increments between consecutive keyframes are fused, with the uncertainty set by the required parameters `relative_pose_increment_sigma_lin` / `_ang` (plus optional growth with the increment size, `..._per_sqrt_meter` / `_rad`). The first reading adds one absolute factor, with the message covariance, to resolve `T_map_to_odom_<label>`. The twist part is not used.	Increments between consecutive readings dead-reckon the pose between primary pose updates.
`nav_msgs/Odometry` (with `BridgeROS2` param `odometry_as_robot_pose_observation: false`: becomes a planar `CObservationOdometry`)	`fuse_odometry()`: relative increments, with covariance from the motion model `odom_motion_model_a1` .. `a4`.	Increments as above, plus wheel velocities (`sigma_wheel_odom_*`).
`sensor_msgs/Imu`	Orientation as an absolute attitude factor (`imu_attitude_sigma_deg`, 0 disables; `imu_attitude_azimuth_offset_deg`); acceleration as gravity direction (`imu_normalized_gravity_alignment_sigma`, 0 disables); angular velocity as a prior on \(\boldsymbol{\omega}_k\) (`imu_angular_velocity_sigma`, 0 disables). Readings are averaged over `imu_min_sample_period`.	Angular velocity (`sigma_imu_angular_velocity`). Optionally, inertial propagation between pose updates (`imu_propagation`).
`sensor_msgs/NavSatFix`, `gps_msgs/GPSFix`	ENU position factor with a Huber kernel (`gnss_huber_threshold`). Requires a geo-reference: either estimated (`estimate_geo_reference: true`) or fixed (`fixed_geo_reference`, or given by a geo-referenced map).	Ignored unless `gnss_enabled: true` and a geo-reference is set; then low-sigma fixes nudge the position.
(none: C++ or other MOLA modules, e.g. LiDAR odometry)	`fuse_pose()` with `frame_id` equal to `reference_frame_name` (`map`): a prior factor on the keyframe pose. Other frames: as for `nav_msgs/Odometry` above.	Each pose becomes the new state; velocity is estimated from consecutive poses.

Note

IMU orientation. sensor_msgs/Imu orientation is used unless orientation_covariance[0] is negative (the ROS convention for “no orientation”). An IMU without magnetometer, whose yaw is arbitrary, must publish -1 there, otherwise that yaw is taken as the absolute heading. By default (offset 0), yaw zero is assumed to point North; IMUs whose yaw zero points East (ENU, as in REP-103) need imu_attitude_azimuth_offset_deg: -90.

3.3. Outputs

estimated_navstate() from C++, for any time and frame (see above).
StateEstimationSmoother also publishes, at its module execution_rate, the fused pose of vehicle_frame_name (base_link) in reference_frame_name (map). BridgeROS2 forwards it as nav_msgs/Odometry on <module name>/pose (e.g. /state_estimation/pose) and as /tf, if its publish_*_from_slam_source parameters select this module. Optional extra outputs: map -> odom (publish_map_to_odom_tf) and the fused pose in a separate child frame (publish_fused_vehicle_tf). An estimated geo-reference is published once converged (publish_estimated_georef_on_convergence).
StateEstimationSimple does not publish anything by itself: it serves predictions to other modules, e.g. LiDAR odometry, which publishes its own pose.

4. Implementation: Simple estimator

The package mola_state_estimation_simple implements StateEstimationSimple, a lightweight constant-velocity estimator. It is the default in MOLA LiDAR odometry, since for LO/LIO the LiDAR poses dominate and all it needs is a good motion prior for the next scan.

Algorithm:

State: the latest pose (with covariance) at its timestamp, plus the body twist \((\mathbf{v}, \boldsymbol{\omega})\).
Pose updates (fuse_pose(), e.g. LiDAR odometry) replace the pose. The twist is derived from consecutive poses of the same source (pose increment divided by \(\Delta t\)).
Odometry readings dead-reckon the pose forward between pose updates. Planar wheel odometry is applied in the yaw-only frame, and its velocities (if present) update the twist.
IMU angular velocity updates \(\boldsymbol{\omega}\). IMU and odometry readings are buffered and applied in timestamp order, so results do not depend on delivery order.
Velocity filter (velocity_filter_enabled, default on): each twist component is smoothed by a scalar Kalman filter, with process noise from sigma_random_walk_acceleration_* and measurement noise from each input.
Prediction: estimated_navstate(t) applies the constant velocity model from the last pose (or integrates the IMU, if imu_propagation is enabled), with a diagonal covariance growing with \(\Delta t\) since the last pose update:

\[\sigma^2_{xyz} = \sigma^2_{v}\,\Delta t^2 + \left(\tfrac{1}{2}\sigma_{a,\text{lin}}\,\Delta t^2\right)^2 + \sigma^2_{\text{rel,lin}}, \qquad \sigma^2_{rot} = \sigma^2_{\omega}\,\Delta t^2 + \left(\tfrac{1}{2}\sigma_{a,\text{ang}}\,\Delta t^2\right)^2 + \sigma^2_{\text{rel,ang}}\]

(added to the last pose covariance), where \(\sigma_v, \sigma_\omega\) are the uncertainties of the filtered twist, sigma_random_walk_acceleration_linear/angular (\(\sigma_a\)) model unmodeled accelerations, and sigma_relative_pose_linear/angular (\(\sigma_{\text{rel}}\)) is a constant floor. The linear velocity covariance is rotated from the vehicle frame into the map frame.
The frame_id argument is ignored: all poses are assumed to be in the same frame.

Main parameters (default values from state-estimation-simple.yaml; the full list is in Parameters):

Parameter	Default	Meaning
`max_time_to_use_velocity_model`	0.75 s	Maximum extrapolation time since the last observation.
`sigma_random_walk_acceleration_linear` / `_angular`	1.0 m/s², 10.0 rad/s²	Unmodeled acceleration; velocity filter process noise.
`sigma_relative_pose_linear` / `_angular`	0.5 m, 0.1 rad	Constant floor of the predicted pose uncertainty (tightness of the ICP prior).
`sigma_imu_angular_velocity`	0.05 rad/s	Gyroscope noise.
`sigma_wheel_odom_linear_vel` / `_angular_vel`	0.1 m/s, 0.05 rad/s	Wheel odometry velocity noise.
`initial_twist` (+ `initial_twist_sigma_lin/ang`)	zero (20 m/s, 3 rad/s)	Initial velocity guess, e.g. for datasets starting in motion.
`imu_propagation`	false	Propagate the pose between updates by integrating the IMU (gyro + accelerometer) instead of a constant twist.
`gnss_enabled`	false	Use low-sigma GNSS fixes (e.g. RTK) to correct position drift, given a geo-reference.
`enforce_planar_motion`	false	Force z, pitch and roll to zero.

Use the smoother instead when several sources must be truly fused, or when GNSS-based geo-referencing must be estimated.

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.