manankharwar/fusioncore

What it solves

FusionCore is a sensor fusion library designed for outdoor robots to provide stable, high-frequency (100 Hz) localization. It solves the problem of "silent drift" by providing detailed diagnostics that tell the user exactly which sensor is causing an estimate to fail and why, rather than just drifting without explanation.

How it works

It uses a 23-state Unscented Kalman Filter (UKF) to fuse data from IMUs, wheel encoders, GPS (GNSS), and visual SLAM. Key technical features include:

• Retrodiction: An IMU ring buffer replays updates when delayed GPS fixes arrive to reconstruct the state at the exact timestamp.
• Adaptive Noise: It continuously estimates gyro and accel bias and uses adaptive noise covariance updates to handle noisy or slipping wheels.
• ECEF Fusion: GPS is fused directly in Earth-Centered, Earth-Fixed (ECEF) coordinates to avoid UTM zone boundary issues.
• Diagnostics: It publishes real-time health status and rejection reasons (e.g., CHI2_FAILED, IMPLAUSIBLE_JUMP) for dropped measurements.

Who it’s for

Robotics engineers and developers building outdoor autonomous vehicles (such as agricultural robots or autonomous vans) who need a reliable localization stack that is easy to configure and compatible with ROS 2 (Jazzy and Humble) or plain C++ environments.

Highlights

• High Performance: Runs in under 1ms per cycle on a Raspberry Pi 4.
• ROS 2 Independent: The core filter is a plain C++17 library with no ROS dependencies.
• Robustness: Includes a zero-velocity update (ZUPT) to prevent drift while idle and a chi-squared gate to reject outliers.
• Deployment-Oriented: Designed to minimize manual parameter tuning by using adaptive estimators.