FusionCore tackles a persistent problem in robotics sensor fusion: how to reliably combine IMU, wheel encoder, and GPS data at high rates without succumbing to common failure modes that plague popular tools like robot_localization. Its standout feature is a robust Unscented Kalman Filter (UKF) implementation that operates at 100 Hz, designed to handle GPS outliers and delayed measurements gracefully, all while running efficiently on embedded platforms like the Raspberry Pi 4.
what fusioncore does: a ROS 2 sensor fusion SDK with unscented kalman filtering
FusionCore is a C++ SDK built for ROS 2 that implements an Unscented Kalman Filter tailored for fusing inertial measurement unit (IMU) data, wheel encoder odometry, and GPS signals. Unlike many other sensor fusion libraries, it fuses GPS data directly in Earth-Centered Earth-Fixed (ECEF) coordinates, avoiding common pitfalls associated with UTM projections, such as zone boundary discontinuities that can cause catastrophic failures.
The filter runs at a steady 100 Hz fusion rate, which is sufficient for most mobile robotics applications requiring smooth and responsive state estimation. It is designed as a drop-in replacement for the widely used robot_localization package’s Extended Kalman Filter (EKF), addressing some of the latter’s most notorious failure modes.
Key architectural components include:
- An Unscented Kalman Filter core that better handles nonlinearities compared to EKF.
- GPS fusion in native ECEF coordinates without converting to UTM, improving robustness.
- A chi-squared outlier gate that rejects erroneous GPS fixes, demonstrated by catching a 707 m corrupted fix that robot_localization’s EKF missed.
- Adaptive noise covariance estimation that auto-calibrates from just two IMU datasheet parameters, eliminating the need for tedious manual tuning.
- An IMU ring buffer that supports retrodiction, allowing the filter to handle GPS measurements delayed by up to 500 ms.
- Built-in non-holonomic constraints and zero-velocity updates (ZUPT) to prevent drift during idle periods.
FusionCore runs efficiently with under 0.2 ms per cycle on high-end i7 CPUs and under 1 ms per cycle on a Raspberry Pi 4 with NEON SIMD auto-detection, making it suitable for embedded ARM platforms without recompilation.
why fusioncore stands out: adaptive noise, outlier rejection, and real-time performance
What distinguishes FusionCore is its focus on robustness and real-world usability in challenging sensor fusion scenarios. Most sensor fusion packages require extensive manual tuning of noise parameters, which can be a huge pain point. FusionCore’s adaptive noise covariance estimation derives from just two IMU datasheet parameters, drastically reducing calibration overhead and improving out-of-the-box performance.
The chi-squared outlier gate is a pragmatic addition that improves safety and reliability by rejecting corrupted GPS fixes. The example given is telling: a 707-meter corrupted fix that robot_localization’s EKF accepted caused large deviations in position estimates, whereas FusionCore kept the position stable within 1 meter. This kind of resilience is critical in fielded robotics systems where GPS quality can fluctuate wildly.
Handling delayed GPS measurements via an IMU ring buffer and retrodiction is another practical design choice. Sensor fusion algorithms often struggle when measurement timestamps are not synchronized or when GPS data arrives late. FusionCore’s architecture accommodates up to a 500 ms delay, which is typical in real GPS receivers, by rolling back and correcting the filter state.
The embedded-friendly performance is notable. Running under 1 ms per update on a Raspberry Pi 4 means FusionCore can be deployed without specialized hardware or recompilation. The use of NEON SIMD instructions detected at runtime ensures that the filter is optimized for ARM CPUs, balancing speed with portability.
On benchmark datasets like NCLT, FusionCore wins 10 out of 12 sequences against robot_localization’s EKF, with the two failures traced to GPS data quality issues outside the filter’s control. The average trajectory error is reported at 5.6 m versus 13.0 m for the EKF on a 600 second campus drive, a tangible improvement.
The codebase is implemented in modern C++ within the ROS 2 ecosystem, making it accessible to many roboticists already invested in ROS 2. It’s designed to integrate smoothly as a drop-in replacement.
quick start: installation and testing on ROS 2
FusionCore provides two main installation options. The first is building from source on Ubuntu with ROS 2 Jazzy or Humble:
mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src
git clone https://github.com/manankharwar/fusioncore.git
cd ~/ros2_ws
source /opt/ros/jazzy/setup.bash # or /opt/ros/humble/setup.bash
rosdep install --from-paths src --ignore-src -r -y
colcon build --packages-up-to fusioncore_ros
source install/setup.bash
To verify the installation, the repo includes a quick automated test script that runs FusionCore with fake sensor data and checks outputs:
bash tools/quick_test.sh
This test runs for about 15 seconds and prints pass/fail status for each check.
Alternatively, users can run FusionCore inside a Docker container without installing ROS 2:
docker pull ghcr.io/manankharwar/fusioncore:latest
docker run --rm -it ghcr.io/manankharwar/fusioncore:latest bash
Inside the container, run the same quick test script to validate the setup.
verdict: a practical and robust UKF for ROS 2 sensor fusion
FusionCore fills a niche for robotics developers needing a robust, production-ready Unscented Kalman Filter that addresses real-world GPS fusion challenges. Its adaptive noise estimation and robust outlier rejection solve two of the biggest pain points in sensor fusion calibration and reliability.
While the filter excels in many scenarios, it still depends heavily on GPS data quality. The two NCLT benchmark sequences where it lost to robot_localization were due to GPS issues beyond the filter’s design. Also, the focus on a single UKF approach means users looking for multi-modal or particle filter solutions will need to look elsewhere.
For embedded developers and ROS 2 users, FusionCore offers a neatly packaged, performant, and well-documented option to upgrade localization pipelines. The drop-in replacement design means you can try it without rewriting your existing stack. If you’ve struggled with robot_localization’s quirks or want a more adaptive, resilient UKF, FusionCore is worth a close look.
Its combination of practical engineering, clear benchmarking, and thoughtful design make it a solid choice for real-world mobile robot state estimation.
Related Articles
- Super-LIO: A structured mapping LiDAR-Inertial Odometry system for faster real-time navigation — Super-LIO improves LiDAR-Inertial Odometry with a compact mapping strategy that speeds up correspondence search by 1.2-4
- A hands-on guide to classical autonomous vehicle control algorithms in Python — Explore a Python repo implementing classical autonomous vehicle algorithms as transparent simulations. Covers localizati
- MR.ScaleMaster: heterogeneous multi-robot monocular SLAM fusion via Sim(3) optimization — MR.ScaleMaster fuses scale-ambiguous monocular SLAM trajectories from multiple robots using Sim(3) graph optimization, e
- Inside the ZED SDK: GPU-accelerated spatial perception for stereo cameras — Explore the ZED SDK, a C++ library for real-time stereo vision, SLAM, and spatial mapping with GPU acceleration and zero
- Observing AI agents at scale with opsrobot: a Vector-based telemetry pipeline for OpenClaw workflows — opsrobot-ai/opsrobot offers a full-stack observability platform for AI agents, using Vector pipelines to transform OpenC
→ GitHub Repo: manankharwar/fusioncore ⭐ 229 · C++