Kalman Filter Localization

Real UrbanNav Odaiba data (MIT) · actual ROS 2 output · reference-seeded initialization. Reproduce the real-data demo · Full measured results and assumptions · Synthetic 6DoF demo

A 6DoF error-state EKF for ROS 2, fusing IMU, absolute pose and velocity observations, with GNSS and odometry input modes. It estimates position, velocity, attitude, and IMU biases.

Development target: a general 6DoF localizer for Autoware and Nav2. See the integration contracts and roadmap for current capabilities, TF ownership and remaining replacement requirements. Autoware/Nav2 launch and activation provides runnable entry points with topic/service remappings and startup instructions.

Features

• GNSS pose / NavSatFix / Doppler velocity
• Gyroscope and accelerometer bias estimation
• GNSS NIS gating and Huber/Cauchy robust losses
• GNSS antenna lever arm and delayed-measurement rewind/replay
• Wheel speed, NHC, ZUPT, and ZIHR
• Continuous-time process noise and second-order discretization
• CSV evaluation and UrbanNav Tokyo ablation tools

Build and run

source /opt/ros/$ROS_DISTRO/setup.bash
colcon build --symlink-install
source install/setup.bash
ros2 launch kalman_filter_localization ekf.launch.py

Main topics:

Health and typed diagnostics are always published:

The legacy Float64MultiArray debug topics remain available when publish_debug_topics is enabled. The typed interfaces are defined in the separate kalman_filter_localization_msgs package and are intended for monitoring tools and long-term API use. Detailed field definitions and health-state transitions are in docs/diagnostics.md.

An optional joint 6DoF pose input preserves correlated position/attitude covariance, transforms reference frames at the sensor timestamp, and supports delayed replay and six-dimensional NIS gating. The linear Twist input adds correlated 3D velocity observations with sensor-origin transport and replay. Pose freshness is monitored independently of IMU arrival. Twist axes can be selected for forward-speed-only inputs. See initialization and reset for full correlated initial covariance and the reset service.

See ekf.yaml for the default parameters and param/profiles for dataset-specific configurations.

Configuration diagnosis

Run kf_doctor before tuning a new bag. It discovers common GNSS/IMU/wheel/odometry topics, checks sensor timestamps, frame IDs, covariance validity, IMU rate and stationary-window statistics, then writes a machine-readable report and a conservative profile skeleton:

ros2 run kalman_filter_localization kf_doctor \
  --bag input_bag \
  --output-report .data/doctor/report.json \
  --output-profile .data/doctor/profile.yaml

Fixed-lag smoothing oracle

File truncated at 100 lines see the full file

Any contribution that you make to this repository will be under the BSD license 2.0, as dictated by that license.