⚡ electrical engineering

FusionCore: A 23-State Unscented Kalman Filter for IMU, Wheel Encoder, GPS, and Visual SLAM Fusion in ROS 2

FusionCore is an open-source ROS 2 package that employs a 23-state Unscented Kalman Filter to robustly fuse IMU, wheel encoder, GPS, and Visual SLAM data into a high-frequency odometry stream, achieving significantly lower trajectory errors and greater numerical stability than the standard robot_localization package across diverse NCLT dataset sequences.

Original authors: Manan Kharwar (Independent Researcher, Hamilton, ON, Canada)

Published 2026-05-26

📖 6 min read🧠 Deep dive

CC BY 4.0

Original authors: Manan Kharwar (Independent Researcher, Hamilton, ON, Canada)

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to navigate a robot through a city, but you are blindfolded, your ears are covered, and you can only guess where you are by feeling the wheels turn. That is essentially what robots face when they try to figure out their location. They have to combine data from different "senses" to build a clear picture of where they are.

This paper introduces FusionCore, a new software tool designed to be the "brain" that combines these senses for robots using the ROS 2 system. Here is how it works, explained through simple analogies.

The Problem: The "Noisy" Team

Robots usually have three main ways to guess their location:

Wheel Encoders (The Pedometer): Counts how many times the wheels spin. It's great for short distances but gets confused if the wheels slip on ice or mud.
GPS (The Satellite Map): Tells you exactly where you are on Earth. But it's slow, sometimes disappears (like when you drive under a bridge), and can occasionally give you a "glitch" that says you are in a different city.
IMU (The Inner Ear): A sensor that feels acceleration and spinning. It's very fast, but over time, it starts to "drift" (like a spinning top that slowly tilts), making the robot think it's moving even when it's standing still.

Existing software (called robot_localization) tries to mix these signals, but it has a few flaws:

It treats the "drift" in the sensors as a fixed error, not something that changes over time.
It gets confused when GPS signals jump across map boundaries.
It struggles to tell the difference between a real GPS update and a crazy glitch.

The Solution: FusionCore's "Smart Brain"

FusionCore is a new "brain" (a mathematical filter) that fuses all these signals into a single, smooth, high-speed stream of location data (100 times per second). It uses a 23-dimensional "state" to keep track of everything, including some things other systems ignore.

Here are the key tricks FusionCore uses, explained with metaphors:

1. The "Drift Detective" (Online Bias Estimation)

Imagine you are walking with a friend who has a slight limp. If you don't notice the limp, your map of where you are will slowly get wrong.

Old Way: The software assumes the limp is fixed and ignores it.
FusionCore Way: It has a "Drift Detective" inside. It constantly watches the sensors and asks, "Is the wheel slipping? Is the gyroscope getting tired?" It calculates the exact amount of "limp" (bias) in real-time and subtracts it. This keeps the robot from getting lost even when GPS is gone for a while.

2. The "Global Address" (ECEF-Native GPS)

GPS coordinates are usually converted into local maps (like UTM zones). If you drive across a zone boundary, the map can glitch and crash.

FusionCore Way: Instead of using local street addresses, it speaks the language of the whole Earth (ECEF). It treats the planet as one giant sphere, so it never gets confused by map boundaries. It's like using latitude and longitude for the whole world instead of switching to a new city map every 10 miles.

3. The "Lie Detector" (Adaptive Outlier Gating)

Sometimes GPS sends a message saying, "I am now in the ocean!" when the robot is clearly on a road.

Old Way: The software uses a rigid rule: "If the error is bigger than 5 meters, ignore it." But if the GPS is actually just a little noisy, it might ignore good data.
FusionCore Way: It uses a "Lie Detector" that learns. It looks at the history of errors. If the GPS is usually accurate but suddenly has a bad day, the detector loosens its grip. If the GPS is acting crazy, it slams the door shut. It adjusts its own sensitivity automatically, so it doesn't get fooled by glitches or reject good data.

4. The "Time Traveler" (Delay Compensation)

GPS data often arrives a split-second late because the satellite has to do math before sending the signal.

FusionCore Way: It keeps a "memory buffer" (a ring buffer) of the last second of IMU data. When the late GPS message finally arrives, the software doesn't just apply it to the current moment. It "rewinds" to the exact moment the GPS signal was taken, applies the correction, and then "fast-forwards" back to now. This ensures the robot's position is perfectly synchronized, even with slow sensors.

5. The "Visual Memory" (VSLAM Fusion)

If the robot loses GPS (like driving into a tunnel), it can use a camera (Visual SLAM) to track features in the room.

FusionCore Way: It can seamlessly switch to the camera's view. If the camera loses its way and has to "re-start" its map, FusionCore knows how to snap the robot's position back to reality without the robot panicking or crashing.

The Results: The Big Test

The author tested FusionCore against the standard software (robot_localization) using 12 long driving sequences (up to 92 minutes each) from a public dataset.

The Winner: FusionCore won in 10 out of 12 races.
The Margin: In some cases, it was 22 times more accurate than the standard software.
The Losers: The standard software's advanced version (UKF) crashed (diverged) on all 12 sequences, meaning it completely lost track of the robot's position.
Why FusionCore Lost 2 Races:
1. The Long Blackout: In one race, the GPS signal was gone for nearly 8 minutes. Even the best detective can't guess perfectly without a map for that long, and the robot drifted slightly.
2. The "Adversarial" Glitch: In another race, the GPS sent a cluster of lies that were so convincing they tricked the filter. The standard software happened to reject them by accident because its rules were too strict, while FusionCore tried to be too helpful and got tricked. The author notes these are specific, fixable issues.

Summary

FusionCore is a smarter, more adaptable way to help robots know where they are. It treats sensor errors as living things that change over time, learns to ignore lies, and remembers the past to correct the present. While not perfect, it significantly outperforms the current industry standard in most real-world scenarios.

Technical Summary: FusionCore

Problem Statement

Accurate pose estimation is fundamental to autonomous robot navigation, yet practical deployment of sensor fusion systems remains challenging. Existing solutions face several critical limitations:

Bias Drift: IMU gyroscope and accelerometer biases drift continuously, and wheel encoder yaw rate biases (systematic offsets in differential drive) are rarely treated as filter states, leading to unobservable heading error accumulation during GPS outages.
Architectural Fragility: Dominant ROS packages like robot_localization rely on separate nodes for GPS coordinate transformation (UTM), introducing zone boundary failures. They also use fixed noise covariances and scalar outlier rejection thresholds that do not account for measurement dimensionality.
Operational Gaps: Current systems struggle with GPS-denied operation, map reinitialization recovery, and the integration of diverse sensors like Visual SLAM (VSLAM) and radar without complex coordination.

Methodology

FusionCore is an open-source ROS 2 package implementing a 23-state Unscented Kalman Filter (UKF) that fuses IMU, wheel encoder odometry, GPS, and Visual SLAM pose into a single 100 Hz odometry stream.

Core Architecture

State Vector: The filter maintains a 23-dimensional state vector including position ( $p$ ), orientation ( $q$ ), linear/angular velocity ( $v, \omega$ ), linear acceleration ( $a$ ), gyroscope bias ( $b_g$ ), accelerometer bias ( $b_a$ ), and a critical wheel encoder yaw rate bias ( $b_{ewz}$ ).
Bias Estimation: Unlike previous approaches, gyroscope, accelerometer, and encoder yaw biases are first-class filter states with dedicated process noise. This allows the filter to converge on sensor-specific bias values online without a separate calibration phase.
GPS Fusion: The system fuses GPS data natively in the Earth-Centered, Earth-Fixed (ECEF) frame, converting to ENU only for the filter state. This eliminates the need for a separate navsat_transform node and avoids UTM zone boundary failures.
Measurement Models:
- IMU: Handles 6-DOF raw data and 2/3-DOF orientation updates, accounting for gravity and bias.
- Wheel Encoders: Subtracts the estimated $b_{ewz}$ from reported angular velocity before integration. Enforces non-holonomic constraints (zero vertical velocity and acceleration) with adaptive noise.
- VSLAM: Accepts 6-DOF pose measurements from any visual odometry/SLAM system, including automatic recovery from map reinitialization by re-anchoring the transform after consecutive outlier rejections.
- Velocity: Supports GPS Doppler velocity and Radar Doppler ego-velocity for slip detection.

Filter Design Innovations

Per-Sensor Chi-Squared Gating: Implements Mahalanobis distance outlier rejection with thresholds calibrated specifically for the degrees of freedom (DOF) of each sensor path (e.g., 3-DOF GPS vs. 1-DOF heading), rather than using a single scalar threshold.
Adaptive Noise Covariance: Automatically adjusts sensor noise covariance ( $R$ ) in real-time using a sliding window of innovation sequences and an exponential moving average, eliminating the need for manual retuning due to environmental changes.
Delay Compensation: Utilizes a 1-second IMU ring buffer to perform retrodiction, applying delayed GPS or VSLAM measurements to the correct historical state before re-propagating forward.
Zero-Velocity Updates (ZUPT): Detects stationary periods to suppress bias drift.
Coast Mode: During GPS blackouts, the estimated encoder yaw bias is subtracted from the encoder data to reduce heading drift.

Key Contributions

The paper presents ten specific contributions:

A 23-state UKF formulation with continuous online estimation of gyroscope, accelerometer, and wheel encoder yaw rate biases.
ECEF-native GPS fusion, removing UTM projection dependencies.
Per-sensor Mahalanobis chi-squared outlier gating with dimensionality-calibrated thresholds.
Automatic noise covariance adaptation from innovation sequences.
Automatic Zero-Velocity Updates (ZUPT) for stationary periods.
IMU ring-buffer retrodiction for delay compensation.
Visual SLAM pose fusion with automatic recovery from map reinitialization.
GPS velocity fusion for cross-sensor slip detection.
Radar Doppler ego-velocity fusion for 4D imaging radar platforms.
A quantitative evaluation against robot_localization on twelve full-length NCLT sequences.

Evaluation and Results

The system was evaluated on twelve full-length sequences (55–92 minutes each) from the NCLT public dataset, covering diverse seasonal conditions. The evaluation compared FusionCore against the robot_localization EKF and UKF using Absolute Trajectory Error (ATE) RMSE.

Performance: FusionCore achieved lower ATE than robot_localization EKF on 10 out of 12 sequences. Improvements on winning sequences ranged from 1.2× to 22.2×.
Stability: The robot_localization UKF diverged numerically (NaN) on all twelve sequences within the first 30 seconds, confirming known instability issues with high-rate inputs.
Failure Analysis:
- 2012-06-15 (FC Loss): FusionCore lost to robot_localization EKF (49.2m vs 18.2m) due to a 7.7-minute GPS blackout. The paper attributes this to the limitations of encoder-based dead reckoning over such long durations without an absolute heading reference (e.g., magnetometer).
- 2012-08-20 (FC Loss): FusionCore lost significantly (98.3m vs 10.6m) due to an "adversarial GPS cluster" of 105 fixes appearing 720–840m from the ground truth at a blackout boundary. The paper notes that robot_localization's miscalibrated gate happened to reject these outliers, whereas FusionCore's relaxed gate (necessary for recovery) accepted them. The authors identify a velocity-consistency pre-check as the solution.
Covariance Mismatch: The paper highlights that robot_localization's reliance on fixed, often inaccurate GPS covariance reports leads to valid data rejection and a collapse into dead reckoning. FusionCore's adaptive noise model mitigates this by inflating the noise model when innovation errors exceed reported covariances.

Significance and Claims

The paper claims that FusionCore addresses the practical limitations of existing ROS 2 sensor fusion stacks by providing a single lifecycle node that requires no external coordinators. Its significance lies in:

Robustness: Achieving superior trajectory accuracy in long-duration, real-world scenarios where existing UKF implementations fail numerically.
Adaptability: Handling dynamic sensor quality and GPS covariance mismatches through adaptive noise and dimensionality-aware gating.
Operational Continuity: Enabling GPS-denied operation and seamless recovery from VSLAM map reinitialization.
Reproducibility: Providing a fully open-source implementation with benchmark scripts and configurations for standard platforms (TurtleBot3, Husky) and integration with rtabmap_ros.

The authors conclude that while specific failure modes (long blackouts, adversarial GPS clusters) exist, they are diagnosable and have identified architectural fixes (magnetometer integration, velocity pre-checks) for future work.