FusionCore: A 23-State Unscented Kalman Filter for IMU, Wheel Encoder, GPS, and Visual SLAM Fusion in ROS 2
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.
Code (0)
등록된 구현이 없습니다.
Tasks
Visual OdometrySimilar Papers 제목 키워드 기반
Unscented Kalman Filter: Aspects and Adaptive Setting of Scaling Parameter
This technical note deals with the unscented Kalman filter for state estimation of nonlinear stochastic dynamic systems with a special focus on the scaling parameter of the filter. Its standard choice is analyzed and …
State EstimationReal-to-Sim: Predicting Residual Errors of Robotic Systems with Sparse Data using a Learning-based Unscented Kalman Filter
Achieving highly accurate dynamic or simulator models that are close to the real robot can facilitate model-based controls (e.g., model predictive control or linear-quadradic regulators), model-based trajectory planning …
Model Predictive ControlTrajectory PlanningThe Unscented Transform Controller: a new model predictive control law for highly nonlinear systems
The Unscented Transform which is the basis of the Unscented Kalman Filter, UKF, is used here to develop a novel predictive controller for non-linear plants, called the Unscented Transform Controller, UTC. The UTC can be …
Model Predictive ControlRobust Square Root Unscented Kalman filter of graph signals
Considering the problem of nonlinear and non-gaussian filtering of the graph signal, in this paper, a robust square root unscented Kalman filter based on graph signal processing is proposed. The algorithm uses a graph to…
State EstimationMaximum Correntropy Unscented Filter
The unscented transformation (UT) is an efficient method to solve the state estimation problem for a non-linear dynamic system, utilizing a derivative-free higher-order approximation by approximating a Gaussian distribut…
State Estimation