Summary: GNSS drifts, wheels slip, IMUs accumulate error, and visual SLAM can suddenly lose tracking. FusionCore attempts to fuse all these sensors into a continuous 100 Hz pose estimate using a 23-state Unscented Kalman Filter (UKF). It supports ROS 2, automatic bias estimation, handles latency and outliers — but it also has clear boundaries.
For reliable mobile robot navigation, you usually cannot trust a single sensor.
GNSS provides global position, but tall buildings, tree cover, and tunnels cause drift or dropouts; wheel odometry is smooth over short intervals but “confidently computes wrong answers” when wheels slip; IMUs update fast but keep integrating small biases; visual SLAM works well indoors but can jump suddenly due to weak texture, occlusion, or relocalization.
What engineers ultimately face is often not “which sensor is most accurate,” but a messier problem:
How do you combine these imperfect data streams into one continuous, trustworthy trajectory that can be handed directly to Nav2?
FusionCore was built to answer exactly that question.
What Is FusionCore?
FusionCore is an open-source multi-sensor fusion project for ROS 2, built around a 23-state Unscented Kalman Filter (UKF).
It can take in:
- Raw IMU data;
- Wheel odometry from differential-drive and Ackermann vehicles;
- Position and velocity from standard GNSS or RTK receivers;
- Dual-antenna GNSS heading;
- Magnetometer;
- Radar Doppler velocity;
- Visual odometry or SLAM poses from ORB-SLAM3, RTAB-Map, Kimera, and similar systems.
After fusion, it outputs a continuous odometry and TF at 100 Hz by default, ready to feed into Nav2, controllers, or higher-level task systems.
The project supports ROS 2 Jazzy and Humble, is developed in C++17, and is licensed under Apache 2.0. It also separates the core filtering library from the ROS 2 wrapper: fusioncore_core has no ROS dependency and can be embedded in other C++ programs; fusioncore_ros handles topics, TF, lifecycle, and diagnostics.
That is more flexible than a design that only works as a ROS node.
23 States: What Is the Filter Actually Estimating?
The 23-state vector reflects a realistic mobile robot pose problem. Roughly speaking, it covers:
- 3D position, orientation, and velocity (attitude, position, velocity);
- Accelerometer and gyroscope biases (IMU bias terms);
- Wheel scale factors or odometry-related parameters;
- Sensor-to-base transform corrections and timing-related states.
Instead of treating every sensor as “ground truth,” the filter maintains a unified state estimate and lets each measurement correct the states it can observe. Biases are estimated online rather than calibrated offline, which is one of the project’s practical highlights.
What Real Engineering Problems Does It Solve?
Beyond the math, the project addresses several problems that tutorials usually skip:
- Sensor bias estimation: accelerometer and gyroscope biases are estimated online, reducing drift without external calibration.
- Delayed and out-of-order measurements: GNSS and visual SLAM arrive late and irregularly; the filter handles measurement delay and buffers rather than assuming perfect synchronization.
- Outlier rejection: invalid or jumping measurements are detected and down-weighted, preventing one bad reading from corrupting the whole pose.
- Wheel slip handling: slip is modeled or detected instead of being silently integrated into the odometry.
- Stationary detection: the filter avoids false drift when the robot is stopped.
- Coordinate frame checks: TF and frame management are treated as first-class concerns, reducing the classic “axes swapped” debugging nightmare.
- Relocalization recovery: after GNSS reacquisition or SLAM relocalization, the estimate can re-converge instead of jumping.
FusionCore official GPS outlier injection test.
What Do the Official Test Results Show?
The project’s published test results show consistent improvement over using any single sensor alone. In typical outdoor scenarios, fusing GNSS + IMU + wheel odometry reduces drift compared to wheel odometry or IMU alone, and maintains usable pose during brief GNSS outages. The exact numbers depend on sensor quality and environment, but the trend is clear: multi-sensor fusion provides smoother, more robust localization than any single source.
FusionCore vs. robot_localization EKF error comparison on selected NCLT sequences.
Trajectory comparison across NCLT sequences in different seasons.
Want to Try It? Start With This Path
If you want to reproduce the results, a suggested route is:
- Start with
fusioncore_coreoffline: feed in a recorded rosbag to verify the filter behavior without hardware. - Set up the ROS 2 environment (
fusioncore_ros) with a simulated or real robot. - Start with IMU + wheel odometry only: verify straight-line and in-place rotation behavior matches the real direction of motion.
- Confirm the TF between
base_link,odom, GNSS antenna, and IMU. - Then add GNSS: check covariance, heading, and antenna lever arm.
- Finally add visual SLAM, magnetometer, or radar velocity.
- Record rosbags and compare offline against trajectory ground truth or repeated routes.
If you plug in all topics at once, it is very hard to tell whether drift comes from the filter, time synchronization, coordinate axes, or extrinsic calibration.
Which Projects Are Worth Trying?
FusionCore fits scenarios like:
- ROS 2 outdoor inspection robots, agricultural robots, and low-speed unmanned platforms;
- Differential or Ackermann robots that need RTK + IMU + wheel odometry fusion;
- Applications where GNSS is occasionally blocked and you need to coast on inertia and wheel odometry for short periods;
- Indoor use with visual SLAM or LiDAR ICP while needing smoother control poses;
- Teams already using
robot_localizationbut struggling with bias estimation, GNSS coordinate handling, or parameter tuning.
Know These Boundaries Before You Start
FusionCore is still a very new project. Do not put it on production hardware just by reading the feature list.
It will still drift without GNSS for long periods. The documentation clearly states that without absolute heading, GNSS outages longer than roughly 5–7 minutes accumulate significant heading error. Magnetometers, visual SLAM, or other absolute heading sources only mitigate this — the filter cannot create information out of nothing.
Mecanum wheels and omnidirectional chassis have no dedicated motion model yet. You can relax the lateral non-holonomic constraint, but that is not full modeling.
Inertial coasting mode comparison in GNSS-degraded scenarios.
FusionCore uses loosely coupled GNSS. It fuses the receiver’s output position, velocity, and covariance — not raw pseudoranges and carrier phase. Projects that need deep-coupled GNSS/INS or centimeter-level post-processing should look at dedicated tightly coupled navigation frameworks.
“Auto-tuning” does not replace time synchronization and extrinsic calibration. If sensor axes are wrong or timestamps are scrambled, even a smart UKF will stably output wrong results.
Final Thoughts
The most attractive thing about FusionCore is not that it implements a Kalman filter once more, but that it puts the engineering problems tutorials tend to ignore at the center: sensor bias, late GNSS, outliers, wheel slip, stationary drift, coordinate frame checks, and relocalization recovery.
It is young, independent hardware validation is still limited, and it does not suit every chassis or every high-precision navigation need. But if you are fusing IMU, wheel odometry, GNSS, or visual SLAM in ROS 2, it is already a solution worth seriously comparing with real rosbag data.
Do not ask first whether it can “replace” some older project.
A more valuable approach: feed the same sensor data, the same real route, and the same error metrics — and let the results speak for themselves.
Project & Resources
GitHub: https://github.com/manankharwar/fusioncore
Paper: https://arxiv.org/abs/2605.25239
If you enjoy navigation and localization projects that can run on real robots, follow along — we will keep sharing useful, reproducible open-source tools for GNSS, IMU, SLAM, and multi-sensor fusion.
If you have any questions about this topic, feel free to contact us at [email protected]
Have questions about this article? Feel free to contact us at [email protected] — we’re happy to help!
