manankharwar/fusioncore

解决的问题

FusionCore 是一个为室外机器人设计的传感器融合库，旨在提供稳定、高频（100 Hz）的定位能力。它通过提供详细的诊断信息，解决了‘无声漂移’问题——这些信息能明确告诉用户是哪个传感器导致了估计失败以及原因，而不是在毫无解释的情况下发生漂移。

工作原理

它使用 23 状态的无迹卡尔曼滤波器（UKF）融合来自 IMU、轮式编码器、GPS（GNSS）和视觉 SLAM 的数据。关键技术特性包括：

• 回溯推断（Retrodiction）： 当延迟的 GPS 修正到达时，IMU 的环形缓冲区会重放更新，以重建精确时间戳下的状态。
• 自适应噪声（Adaptive Noise）： 持续估计陀螺仪和加速度计的偏置，并使用自适应噪声协方差更新来应对噪声大或打滑的车轮。
• ECEF 融合（ECEF Fusion）： GPS 数据直接在地心地固（ECEF）坐标系中融合，避免了 UTM 区域边界问题。
• 诊断功能（Diagnostics）： 发布实时健康状态和测量被丢弃的原因（例如 CHI2_FAILED、IMPLAUSIBLE_JUMP）。

适用对象

需要可靠定位系统、易于配置且兼容 ROS 2（Jazzy 和 Humble）或纯 C++ 环境的室外自主车辆（如农业机器人或自动驾驶厢式车）的机器人工程师和开发者。

主要亮点

• 高性能： 在 Raspberry Pi 4 上每周期运行时间低于 1ms。
• ROS 2 无关： 核心滤波器是无 ROS 依赖的纯 C++17 库。
• 鲁棒性： 包含零速度更新（ZUPT）以防止静止时漂移，以及卡方门限以拒绝异常值。
• 部署友好： 通过使用自适应估计器，最大限度减少手动参数调优。