Track papers across labs, topics, and trends.
Search papers, labs, and topics across Lattice.
FusionCore, a ROS 2 package, fuses IMU, wheel encoder, GPS, and Visual SLAM data using a 23-state Unscented Kalman Filter (UKF) to produce a robust 100 Hz odometry stream. A key innovation is the online estimation of wheel encoder yaw rate bias using GPS heading cross-covariance, which mitigates heading drift during GPS outages. Evaluation on the NCLT dataset demonstrates that FusionCore achieves significantly lower Absolute Trajectory Error (ATE) compared to robot_localization, while also avoiding numerical divergence issues.
FusionCore's online estimation of wheel encoder yaw rate bias slashes heading drift during GPS blackouts, outperforming existing methods by up to 22x in trajectory accuracy.
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.