FusionCore：基于 23 状态 Unscented Kalman Filter 的 IMU、轮速编码器、GPS 与 Visual SLAM 融合系统（ROS 2 实现）

我们提出FusionCore，这是一个开源的ROS 2传感器融合软件包，利用23状态Unscented Kalman Filter（UKF）将IMU、轮速编码器里程计、GPS和Visual SLAM姿态融合为单一的100 Hz里程计流。其中第23个状态为轮速编码器系统偏差的在线估计，通过GPS航向交叉协方差识别，并在GPS中断时进行补偿，以减少 coast 模式下的航向漂移。FusionCore还估计了陀螺仪和加速度计的偏差作为显式滤波状态，原生处理GPS的ECEF坐标，无需单独的坐标投影节点，应用每传感器的Mahalanobis卡方异常值门控并校准至测量自由度，自动从创新序列调整传感器噪声协方差。VSLAM姿态融合使系统能够在无GPS环境下与任何视觉里程计或SLAM系统协同工作，包括从地图重新初始化的自动恢复。我们在NCLT公开数据集的12个完整序列（每段55-92分钟）上进行评估。FusionCore在10个序列上实现了更低的绝对轨迹误差（ATE），获胜序列的改进范围为1.2倍至22.2倍。而robot_localization的UKF在所有12个序列上都出现了数值发散。FusionCore已在https://github.com/manankharwar/fusioncore 上提供，采用Apache 2.0许可证。