FusionCore

A 23-state UKF for outdoor robots: IMU, wheel encoders, GPS and visual SLAM at 100 Hz. Two numbers from your IMU datasheet instead of days of tuning, and when the estimate goes wrong it names the sensor and the reason 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.

Start without installing anything

If you have a rosbag, you can get something useful out of this before you build it. Point bag_report.py at the bag and it tells you what is wrong with your estimation setup, in terms of your robot rather than a benchmark dataset:

python3 tools/bag_report.py /path/to/your/bag

No config, no ROS distro to match, and every check works with no ground truth, which is the point: almost nobody has a surveyed reference for their own robot. It reports things like whether your IMU and your wheels disagree about which way the robot turned, whether two sensors are on different clocks, and whether the bag even contains a manoeuvre that makes heading observable. Details in docs/bag-report.md.

The first bag it was ever run on had an IMU and wheel encoders reporting opposite yaw-rate signs on 100% of turns, over 127,419 samples. That bag was from this project’s own benchmark harness, and the bug had been there for every number it had ever published.

Answers to the things that stop people trying it

Yes, you can run it beside robot_localization rather than instead of it. Same bag, same inputs, both filters, compare the two trajectories. That is how the numbers here were produced, and fusioncore_ros/tests/test_rl_interface_conformance.py locks the interface so it stays a drop-in comparison rather than a rewrite.

Yes, your existing robot_localization config converts. tools/rl_to_fusioncore.py reads it and emits the FusionCore equivalent, because that config already encodes everything you learned about your robot. It refuses to guess: anything that does not map cleanly comes out as a commented TODO with the reason, repeated on stderr so it is visible when you redirect stdout to a file. A config that looks complete but quietly invented a number is worse than one that admits what it does not know.

No, you do not need ROS 2. The filter is a plain C++ library with Eigen and no ROS dependency. The ROS 2 wrapper is a separate package you can ignore.

No, you do not need ground truth to evaluate it. tools/bag_report.py works entirely on raw sensor topics, because almost nobody has a surveyed reference for their own robot. It will not give you an accuracy number, and it says so rather than inventing one.

Yes, it runs on a Raspberry Pi 4. Well under 1 ms per cycle in a Release build, same source on ARM and x86. Build unoptimised and it is drastically slower, so do not skip CMAKE_BUILD_TYPE.

No, you should not trust the benchmark table below right now. Five of its twelve rows have been re-measured at n=2 on corrected input, and the headline claim that stood here, that FusionCore beat robot_localization on ten of twelve, did not survive it: FusionCore wins two of the five and loses three, and one row reversed outright from a published 81% win to a measured 20% loss. The cause was a frame error in the dataset player (#169) that fed a wrong wheel-odometry yaw rate to both filters, so the old numbers describe an input that no longer exists in either column. The other seven rows have not been re-run and nothing should be concluded from them. If a benchmark table in a README has never been wrong, nobody has checked it.

What is defensible today. FusionCore rejects GPS fixes that are kinematically impossible, and that one is provable from physics without any reference: a 60 m jump in one second cannot happen to a 1.5 m/s robot. On NCLT, post-fix, it is 5.0x and 4.7x better than robot_localization on the two sequences where it wins, and 1.2x, 1.8x and 15x worse on the three where it loses. The spread is the open problem and it is being worked; the average is not the interesting number and neither is a win count.

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

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

Every test must pass. CI will catch it if they don’t. The count is not quoted here on purpose: it moves with nearly every change, and a stale number in a contributing guide tells you nothing useful. 0 failures is the bar.

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

• colcon test-result --verbose shows 0 errors and 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.