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

Abstract: We present FusionCore, an open-source ROS 2 sensor fusion package that fuses IMU, wheel encoder odometry, GPS, and Visual SLAM pose into a single 100 Hz odometry stream using a 23-state Unscented Kalman Filter (UKF). The 23rd state is an online estimate of the wheel encoder's systematic yaw rate bias, identified through GPS heading cross-covariance and subtracted during GPS blackouts to reduce heading drift in coast mode. FusionCore also estimates gyroscope and accelerometer biases as explicit filter states, handles GPS natively in ECEF without a separate coordinate projection node, applies per-sensor Mahalanobis chi-squared outlier gating calibrated to measurement degrees of freedom, and adapts sensor noise covariance automatically from the innovation sequence. VSLAM pose fusion enables GPS-denied operation with any visual odometry or SLAM system, including automatic recovery from map reinitialization. We evaluate against robot_localization on twelve full-length sequences (55-92 min each) from the NCLT public dataset. FusionCore achieves lower Absolute Trajectory Error (ATE) on ten of twelve sequences, with improvements ranging from 1.2x to 22.2x on winning sequences. The robot_localization UKF diverges numerically on all twelve sequences. FusionCore is available at https://github.com/manankharwar/fusioncore under the Apache 2.0 license.

• The paper introduces a 23-state UKF that fuses IMU, wheel encoder, GPS, and VSLAM data for robust odometry in ROS 2.
• It employs explicit bias estimation and adaptive noise covariance, significantly enhancing performance over conventional methods.
• Quantitative evaluations on the NCLT dataset demonstrate 1.2× to 22.2× improvements in ATE RMSE compared to traditional EKF/UKF frameworks.

FusionCore: A Comprehensive UKF Framework for Integrated Robotic Odometry in ROS 2

Overview and Motivation

The paper "FusionCore: A 23-State Unscented Kalman Filter for IMU, Wheel Encoder, GPS, and Visual SLAM Fusion in ROS 2" (2605.25239) introduces FusionCore, an open-source sensor fusion package designed for robust, high-frequency odometry output in mobile robots using ROS 2. Unlike traditional fusion approaches—particularly those adopted by robot_localization—FusionCore integrates explicit bias states for gyroscopes, accelerometers, and wheel encoders, adapts to sensor noise online, and supports both GPS and Visual SLAM (VSLAM) pose fusion. The package addresses key deficiencies in prior solutions with architectural choices tuned for long-term, large-scale deployment in real-world environments.

Filter Design and State Representation

FusionCore's UKF operates over a 23-dimensional state vector, capturing position, orientation (unit quaternion), velocities, accelerations, biases for gyroscope and accelerometer, and a dedicated wheel encoder yaw rate bias. Explicit bias modeling enables continuous online estimation, circumventing the drift and calibration issues typical of conventional strategies.

The process model leverages high-rate IMU updates with bias-driven random walks under zero-mean Gaussian noise. Sigma point generation is adapted for quaternion states, resolving ambiguity and numeric instabilities via sign consistency and eigenvalue shifting for covariance preservation. Angular velocity variance is capped to maintain numerical stability.

Figure 1: Online gyroscope bias estimation; UKF bias states bound heading drift, converging to real bias within 40 seconds.

Sensor Fusion Methodology

FusionCore uniquely handles sensor inputs:

• IMU: Two sequential updates per message, fusing gyroscope, accelerometer, and (for 9-axis IMUs) magnetometer-derived orientation, applying gravity compensation. Roll, pitch, and yaw observability is adjusted per available sensor axes.
• Wheel Encoders: Encoder velocity updates subtract the estimated yaw bias, enforcing ground constraints on vertical velocity and acceleration using adaptive noise covariances.
• GPS: Fused natively in ECEF, sidestepping UTM projection zone boundary errors. Position noise covariance is constructed dynamically using HDOP/VDOP or receiver error statistics, with lever arm corrections where possible.
• VSLAM: Pose measurements from any visual odometry/SLAM system are accepted, with automatic recovery from map reinitialization events via transform anchoring post chi-squared gate rejection.

Per-sensor chi-squared gating is calibrated to measurement dimensionality—a significant advancement over scalar thresholds in robot_localization. Adaptive noise covariance is derived online from innovations, maintaining filter consistency as sensor characteristics evolve.

Figure 2: Chi-squared gating (GPS position); gates reject extreme GPS spikes, preserving filter integrity.

Figure 3: Adaptive noise convergence; the filter rapidly matches true sensor noise, maintaining innovation consistency.

Delay Compensation and Zero Velocity Updates

FusionCore implements IMU ring buffer retrodiction, compensating for delays in GPS and VSLAM messages by applying updates retrospectively at the correct measurement epoch. Zero velocity updates (ZUPT) during stationary periods suppress bias drift, securing accurate state estimation across operational phases.

Quantitative Evaluation

FusionCore was benchmarked extensively on the NCLT dataset—twelve full-length sequences spanning 940 minutes—against robot_localization's EKF and UKF solutions. Results demonstrate FusionCore's superiority in ATE RMSE on ten out of twelve sequences, yielding performance improvements from 1.2× up to 22.2×. Notably, robot_localization's UKF diverged numerically on all sequences due to known instabilities.

Figure 4: FusionCore trajectory vs RTK ground truth, showing bounded error across a 92-minute outdoor run.

Analysis of Losses and Limitations

FusionCore's two losses are attributable to:

• Extended GPS Blackouts: Encoder bias estimation is insufficient for heading drift mitigation during blackouts exceeding several minutes. Integration of magnetometer data is posited as the theoretical fix.
• Adversarial GPS Clusters: Unphysically large clusters of erroneous GPS fixes during blackout recovery can transiently dominate the state estimate. Velocity-consistency checks upstream of chi-squared gating are identified as practical mitigation.

In both cases, the root cause is diagnosable, and future revisions aim to address these modes without tuning penalties for normal operation.

ROS 2 Integration and Practical Adoption

FusionCore is distributed as a ROS 2 lifecycle node, with configuration and checkpointing interfaces supporting rapid deployment and session persistence. Official binary packages and simulation environments facilitate integration across platforms (e.g., TurtleBot3, Husky), with upstream demo inclusion in rtabmap_ros confirming adoption in the community.

Theoretical and Practical Implications

FusionCore demonstrates the necessity of explicit bias modeling and adaptive noise for robust odometry in multi-sensor robotic systems. Its approach—especially per-sensor chi-squared gating aligned with measurement DOF and retrodiction—offers a blueprint for future filtering architectures, improving resilience to real-world sensor failures, blackouts, and adversarial noise. The findings imply that sensor fusion frameworks should proactively model sensor idiosyncrasies rather than relying on static assumptions or batch post-processing, particularly as robots move toward deployment in unstructured environments.

Future Directions

Improvements are outlined for magnetometer integration during GPS blackouts, dynamic gate modulation based on rejection statistics, and RTK fix-type-aware noise scaling. Filter health diagnostics via innovation whiteness testing are also proposed. These advances would further strengthen the filter's adaptability and diagnostic capabilities, supporting long-term autonomy.

Conclusion

FusionCore constitutes a robust, high-frequency odometry fusion framework for ROS 2 robots integrating IMU, wheel encoder, GPS, and VSLAM inputs. Empirical evaluation substantiates its performance advantage over conventional packages, largely attributable to its architectural innovations: continuous bias estimation, adaptive noise, per-sensor outlier gating, and delay compensation. Future enhancements are poised to further extend its resilience and accuracy, offering a practical and theoretically sound foundation for mobile robot navigation and state estimation (2605.25239).