rsasaki0109/kalman_filter_localization_ros2

What it solves

This project provides a high-precision localization system for robots and vehicles, solving the problem of estimating a vehicle's position, velocity, and attitude (orientation) in real-time. It specifically addresses challenges like sensor noise, GNSS outages (where GPS signal is lost), and IMU bias drift, ensuring the vehicle knows its location even when individual sensors fail or are inaccurate.

How it works

The system uses an Error-State Extended Kalman Filter (ESEKF) to fuse data from multiple sensors: GNSS (position and velocity), IMU (acceleration and angular velocity), and wheel odometry.

Key technical mechanisms include:

• Measurement Replay: A rewind-and-replay engine that allows the filter to fuse measurements at their actual sensor timestamps, correcting for delays.
• Robust Loss Functions: Uses Huber and Cauchy losses to ignore "outlier" measurements that would otherwise skew the position estimate.
• Coping with Outages: Integrates Non-Holonomic Constraints (NHC) and Zero Velocity Updates (ZUPT) to maintain accuracy during GNSS outages.
• Propagation Models: Offers multiple integration methods (fast, exact, or legacy) to calculate the state between sensor updates.
• Smoothing: An optional fixed-lag RTS smoother can re-process past states using future data to create a highly accurate reference trajectory.

Who it’s for

• Robotics engineers developing autonomous vehicles or ground robots.
• Researchers focusing on sensor fusion and localization algorithms.
• Developers using ROS 2 for navigation stacks.

Highlights

• Multi-sensor Fusion: Combines GNSS, IMU, and wheel speed data.
• Fault Isolation: Automatically quarantines sensors that produce non-finite or consistently rejected samples.
• Bias Estimation: Continuously estimates and corrects for gyroscope and accelerometer biases.
• Diagnostic Tools: Includes kf_doctor for sensor configuration diagnosis and a comprehensive evaluation suite for calculating APE and RPE errors.
• UrbanNav Performance: Demonstrated ability to reduce 3D RMSE during long GNSS outages (up to 81.8 seconds) from over 600 meters to 3.15 meters.