FusionCore: ROS 2 sensor fusion (IMU and GPS and encoders)

ROS 2 sensor fusion SDK. Combines IMU, wheel encoders, and GPS into one reliable position estimate. Self-tuning noise covariance. Apache 2.0.

Every mobile robot needs to know where it is. It gets this from multiple sensors: IMU, wheel encoders, GPS: each of which is imperfect in its own way. IMUs drift. Wheels slip. GPS jumps. You need software that intelligently combines all three into one trustworthy position estimate.

That software is called a sensor fusion package. The standard one for ROS, robot_localization, lacks native ECEF GPS fusion, IMU bias estimation, and adaptive noise covariance. Its designated replacement (fuse) has incomplete GPS support with no ECEF handling or RTK quality gating as of early 2026. No clear accessible replacement exists for either.

FusionCore is built to fill that gap.

FusionCore vs robot_localization on the NCLT dataset (University of Michigan): same IMU + wheel odometry + GPS, no manual tuning. Six sequences, same pipeline:

| Sequence | FC ATE RMSE | RL-EKF ATE RMSE | RL-UKF | |---|---|---|---| | 2012-01-08 | 5.6 m | 23.4 m | NaN divergence at t=31 s | | 2012-02-04 | 9.7 m | 20.6 m | NaN divergence at t=22 s | | 2012-03-31 | 4.2 m | 10.8 m | NaN divergence at t=18 s | | 2012-08-20 | 7.5 m | 9.4 m | NaN divergence | | 2012-11-04 | 28.7 m | 10.9 m | NaN divergence | | 2013-02-23 | 4.1 m | 5.8 m | NaN divergence |

FusionCore wins 5 of 6 sequences. On 2012-11-04 (fall, degraded GPS), FC's Mahalanobis outlier gate still loses despite inertial coast mode (Q inflation on consecutive rejections): GPS was sufficiently degraded for long enough that accumulated drift could not be fully recovered. RL-EKF has no rejection gate and self-corrects immediately. RL-UKF diverged with NaN on all six sequences. Full methodology, configs, and reproduce instructions in benchmarks/.

| Capability | robot_localization | Fuse | FusionCore | |---|---|---|---| | Core filter | EKF or UKF | Factor graph | UKF (22D quaternion state) | | 3D support | Yes | Yes | Full 3D, native | | IMU bias estimation | No built-in states | Plugin-dependent | Gyro + accel bias states | | GPS fusion | navsat_transform node | Plugin, no ECEF/RTK | ECEF-native, single node | | Dual antenna heading | No | No | Yes | | IMU frame transform | Manual (YAML) | Manual (YAML) | Automatic via TF | | Message covariances | Used | Partial | Full 3×3 GNSS + odometry | | GNSS antenna offset | Ignored | Ignored | Lever arm + observability guard | | Outlier rejection | mahalanobis_threshold | Robust loss functions | Chi-squared gating, all sensors | | GPS fix quality gating | No | No | GPS / DGPS / RTK_FLOAT / RTK_FIXED | | Adaptive noise | Manual | Manual | Auto from innovation sequence | | TF validation at startup | Basic | No | Startup check + fix commands | | Multiple GNSS receivers | Workaround | Workaround | Native, independent lever arms | | compass_msgs/Azimuth | No | No | Yes (ENU/NED, rad/deg) | | Delay compensation | history_length | Factor graph inherent | Full IMU replay, 500ms | | Ground constraint | Not built-in | Not built-in | VZ=0 pseudo-measurement | | ZUPT | Not built-in | Not built-in | Auto when stationary | | Sensor dropout detection | Basic | Basic | Per-sensor SensorHealth enum | | /diagnostics | Basic | Basic | Per-sensor health + outliers | | Published covariance | Yes | Yes | Full UKF P matrix | | Filter reset service | No | No | ~/reset (no restart needed) | | Maintenance | Reduced since 2023 | Active | Active, 24h response | | License | BSD-3 | BSD-3 | Apache 2.0 | | ROS 2 Jazzy | Ported from ROS 1 | Native | Native, from scratch |