ROS 2 Multisensor State Estimation

A reproducible ROS 2 Jazzy project that fuses wheel odometry, yaw-rate IMU, and simulated GNSS in a custom C++ extended Kalman filter. It includes deterministic fault injection, covariance and health diagnostics, ground-truth-aligned evaluation, an optional robot_localization baseline, Gazebo Harmonic visualization, automated tests, CI, and committed benchmark evidence.

What it delivers

• Custom Eigen-based 6-state EKF: [x, y, yaw, v, yaw_rate, gyro_bias]
• Continuous-white-noise process model, nonlinear planar kinematics, Joseph covariance update, NIS gating, and bounded robust GNSS reweighting
• nav_msgs/Odometry fused state with pose/twist covariance and map -> base_link TF
• Sensor-age, accept/reject, and NIS values on /diagnostics
• Configurable Gaussian noise, gyro bias/random walk, wheel slip, and GNSS dropout
• Deterministic seeds and five 60-second scenarios
• CSV trajectory logs, JSON metrics, PNG plots, and ground-truth comparison
• Position/heading RMSE, final drift, dropout RMSE/max error, 3-DOF NEES, and 95% marginal coverage
• C++ unit tests, Python unit/regression tests, and a real ROS graph launch test

Benchmark results

Representative deterministic 60-second results:

Scenario	Position RMSE	Heading RMSE
Nominal	0.148 m	0.025 rad
GNSS dropout	0.150 m	0.023 rad
Noisy IMU	0.153 m	0.026 rad
Wheel slip	1.442 m	0.237 rad
Combined degradation	1.814 m	0.231 rad

The wheel-slip and combined-degradation scenarios intentionally retain degraded performance. They demonstrate estimator inconsistency when systematic wheel-velocity error is not represented by the assumed measurement model.

Quick start

Requirements are Ubuntu 24.04, ROS 2 Jazzy, Python 3.12, Eigen 3, NumPy, Matplotlib, and optionally robot_localization, Gazebo Harmonic (ros_gz_sim), and RViz.

From the repository root:

rosdep install --from-paths src --ignore-src -r -y
./scripts/build_and_test.sh
./scripts/demo.sh nominal 60

The demo exits when simulation completes and writes metrics.json, trajectory.csv, and trajectory.png under results/live_nominal/. Replace nominal with noisy_imu, wheel_slip, gnss_dropout, or combined_degradation.

Fast headless benchmark of every scenario:

./scripts/run_benchmarks.sh

Live C++ EKF benchmark without RViz:

./scripts/run_ros_benchmark.sh gnss_dropout 60

Optional comparison and Gazebo front end:

make comparison  # custom EKF and robot_localization consume identical samples
make gazebo      # Gazebo Harmonic robot follows the deterministic command profile

ROS interfaces

Direction	Topic/service	Type	Purpose
Input	/sensors/wheel_odometry	nav_msgs/msg/Odometry	forward velocity and yaw rate
Input	/sensors/imu	sensor_msgs/msg/Imu	biased/noisy yaw rate
Input	/sensors/gnss	sensor_msgs/msg/NavSatFix	global position converted to local ENU
Output	/fused/odometry	nav_msgs/msg/Odometry	fused state and covariance
Output	/tf	tf2_msgs/msg/TFMessage	map -> base_link
Output	/diagnostics	diagnostic_msgs/msg/DiagnosticArray	staleness, NIS, accepted/rejected updates
Output	/evaluation/results	std_msgs/msg/String	final metrics JSON
Service	/reset	std_srvs/srv/Trigger	reset estimator state

The fixed ENU anchor is configured in config/ekf.yaml. The first valid GNSS fix initializes position; initial map-aligned yaw and its uncertainty are explicit parameters.

Repository map

src/multisensor_state_estimation/
├── include/, src/       C++ EKF library and ROS node
├── python/              deterministic simulator, metrics, evaluator, QA transcription
├── config/scenarios/    five versioned fault profiles
├── launch/              demo, comparison, and Gazebo launch files
├── test/                GTest, pytest, and launch_testing
├── urdf/, worlds/       Gazebo Harmonic assets
└── rviz/                estimator/truth visualization
results/committed/        deterministic, versioned benchmark evidence
docs/                     architecture, mathematics, tuning, validation
scripts/                  one-command build, demo, and benchmark workflows

Validation

The final validated repository state includes:

• 17 tests with zero failures/errors/skips
• Python lint passing
• shell syntax validation passing
• headless Gazebo integration passing
• RViz nominal demo passing
• robot_localization comparison passing
• clean ROS process shutdown
• byte-for-byte reproducible committed benchmark artifacts

The committed benchmark summary SHA-256 is:

0c9a39772a6c6681e98367f581c6b48304f8124f26fa2b04c305d4ea6f660609

Evidence and limitations

The committed results are generated from the deterministic NumPy transcription of the production equations, not hand-authored values. The C++ implementation is independently exercised by convergence/covariance tests, ROS launch tests, and the live ROS benchmark.

Wheel slip and combined degradation intentionally expose inconsistency from an unmodelled systematic velocity scale error. The evidence is retained rather than sanitized.

Exact commands, environment, limitations, and the full result table are in validation.md.

Documentation

• Architecture
• Estimator mathematics
• Tuning
• Validation

License

Apache-2.0.