FusionCore

A 23-state UKF for outdoor robots: IMU, wheel encoders, GPS and visual SLAM at 100 Hz. It fuses the sensors you already have, and when the estimate goes wrong it tells you which sensor and why instead of drifting silently. Apache 2.0, ROS 2 Jazzy and Humble, and the filter itself is a plain C++ library with no ROS dependency.

Quick start

sudo apt install ros-jazzy-fusioncore     # or ros-humble-fusioncore

Or from source:

mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src
git clone https://github.com/manankharwar/fusioncore.git
cd ~/ros2_ws
rosdep install --from-paths src --ignore-src -r -y
colcon build --packages-up-to fusioncore_ros
source install/setup.bash

Check it works before wiring it to a robot. This starts the filter with fake sensors and verifies every output, in about 15 seconds:

bash tools/quick_test.sh

Then point it at your robot:

ros2 launch fusioncore_ros fusioncore.launch.py \
  fusioncore_config:=/path/to/your_robot.yaml

The launch file brings the lifecycle node all the way up to active on its own. Pass autoconfigure:=false if a nav2_lifecycle_manager should own it instead.

Docker, if you would rather not install ROS 2: docs/docker.md

docker run --rm ghcr.io/manankharwar/fusioncore:latest bash tools/quick_test.sh

When it goes wrong, it tells you why

Most localization debugging is not a mathematics problem. The filter drifts, and the hard part is working out which of six sensors caused it. FusionCore publishes what it is thinking while it runs, on real hardware:

ros2 topic echo /fusion/debug/gnss_status     # one message per GPS fix
ros2 topic echo /fusion/debug/filter_health   # filter state at 1 Hz

gnss_status answers “why was that fix dropped?” for every fix. A rejection_reason (CHI2_FAILED, SIGMA_XY_HIGH, IMPLAUSIBLE_JUMP, DELAY_TOO_LARGE and the rest), the Mahalanobis distance printed next to the threshold it was actually tested against, and the filter’s own position sigma at that moment.

filter_health answers “does this filter even know which way it is pointing?” Per-sensor innovation norms, heading uncertainty in degrees, which source the heading came from (GPS_TRACK, MAGNETOMETER, DUAL_ANTENNA, NONE), and a separate count of measurements dropped because two drivers disagree about the clock rather than because the data was bad.

That last distinction matters more than it sounds. A sensor whose timestamps run behind the filter clock is not being fused at all, and from the outside that looks exactly like a badly tuned filter.

You can also ask, after the fact, whether the covariance the filter reported was honest. This needs no ground truth and works on any recorded bag:

python3 tools/nis_from_bag.py /path/to/your_bag

Details: Is your filter’s covariance honest?

What FusionCore does not do

Every project has these. Most do not write them down.

Yaw is not observable from a 6-axis IMU, wheel encoders and GPS position alone. The gyro measures wz + gyro_bias and the encoder measures wz + encoder_bias, which is two equations for three unknowns. GPS track heading only helps while the robot moves in a straight line fast enough for the displacement bearing to beat the position noise. Add a magnetometer or dual-antenna GNSS heading and the problem goes away. Without one, expect heading uncertainty to grow during slow or twisty driving, and read heading_sigma_deg in filter_health rather than assuming.

The chi-squared gate is less sensitive than its nominal threshold on a smoothing receiver. Many GNSS receivers report their absolute accuracy, several metres dominated by multipath, while emitting fixes that agree with each other to centimetres because they filter internally. A Kalman filter assumes white measurement noise, so it gets handed a covariance far larger than any innovation it will see, and the gate then sits much further above typical than its 99.9% design point suggests. Measure yours with nis_from_bag.py before relying on the gate.

Long GPS blackouts still accumulate heading error. Beyond roughly five to seven minutes of dead reckoning, residual bias drift dominates. See known limitations.

Built around the problems real robots have

File truncated at 100 lines see the full file

Contributing to FusionCore

Thanks for your interest. Contributions are welcome: hardware configs, bug fixes, tests, and documentation all help.

The fastest way to contribute

The most impactful contributions right now are hardware configs. If you have FusionCore running on a robot, platform, or IMU that isn’t in the repo yet, open a PR adding a YAML under fusioncore_ros/config/. See the hardware config section below.

Before you start

• Check open issues: the bug may already be reported
• Check Discussions: the question may already be answered
• For anything bigger than a typo fix, open an issue or Discussion first so we can align before you write code

Development setup

# Clone and build
git clone https://github.com/manankharwar/fusioncore.git
cd fusioncore

source /opt/ros/jazzy/setup.sh  # replace jazzy with humble on Ubuntu 22.04
rosdep install -r --from-paths . --ignore-src --rosdistro jazzy -y  # replace jazzy with humble on Ubuntu 22.04
colcon build --packages-up-to compass_msgs fusioncore_core fusioncore_ros --cmake-args -DBUILD_TESTING=ON

# Run all tests before and after your change
colcon test --packages-select compass_msgs fusioncore_core fusioncore_ros
colcon test-result --verbose

All 102 tests must pass. CI will catch it if they don’t.

Hardware configs

A hardware config is a YAML file under fusioncore_ros/config/ named after the platform (e.g. clearpath_husky.yaml, ublox_f9p.yaml).

Copy fusioncore_ros/config/fusioncore.yaml as the starting point and adjust:

• imu.gyro_noise / imu.accel_noise: pull from your IMU’s datasheet
• gnss.base_noise_xy: your GPS receiver’s CEP spec
• Any topic remaps specific to your platform

Add a comment at the top with: platform name, IMU model, GPS receiver model, and whether it was field-tested or tuned from datasheet only. Field-tested configs get merged faster.

Pull request checklist

• All 102 tests pass (colcon test-result --verbose shows 0 failures)
• For new features: tests added in fusioncore_core/tests/
• For hardware configs: YAML includes a comment with platform + sensor details
• Commit message describes why, not just what

Code style

C++17. Follow the style of the surrounding code: no reformatting unrelated lines. clang-format is not enforced but is appreciated.

Reporting bugs

Use the Bug Report issue template. Include the output of colcon test-result --verbose if tests are involved.

Questions

Open a Discussion rather than an issue. Issues are for bugs and tracked work; Discussions are for questions, configs, and ideas.

Response time: typically within 24 hours.