在移动机器人定位系统中，传感器融合是决定导航精度的核心技术。ROS 生态中广泛使用的 robot_localization 包基于扩展卡尔曼滤波（EKF）实现多传感器融合，但在面对强非线性系统和传感器噪声时，其一阶线性化近似会带来估计误差。针对这一痛点，开源项目 FusionCore 提出了一套基于无迹卡尔曼滤波（UKF）的完整解决方案，通过 22 状态滤波器、自适应噪声协方差估计和逐传感器异常值剔除等工程手段，在 NCLT 数据集上实现了优于 robot_localization 的定位精度。
22 状态 UKF 滤波器架构
FusionCore 采用 22 状态向量的 UKF 滤波器，状态空间包含载体的三维位置、三维速度、三维姿态（欧拉角）、三轴陀螺仪偏置以及三轴加速度计偏置。这种设计将 IMU 的系统性误差 —— 即陀螺仪零偏和加速度计零偏 —— 作为滤波器内部状态进行实时估计，而非依赖事先标定或静态补偿。在机器人长时间运行过程中，IMU 零偏会随温度漂移和机械应力变化，将偏置纳入状态向量使得滤波器能够自动追踪这些变化，从而避免因零偏估计不准确导致的姿态和位置误差累积。
UKF 相比 EKF 的核心优势在于通过无迹变换（Unscented Transform）处理非线性模型，无需对系统方程进行一阶线性化近似。对于机器人运动模型而言，当载体进行大角度机动或高速转向时，EKF 的线性化假设会产生显著的系统误差。FusionCore 在 NCLT 数据集的 2012-03-31 序列测试中取得了 4.2 米的平均轨迹误差（ATE RMSE），而 robot_localization 的同序列误差达到 54.3 米 —— 这一数量级的差距主要归因于 UKF 对非线性姿态演化的更准确建模。滤波器在 100 赫兹运行频率下完成状态更新，满足实时性要求。
自适应噪声协方差估计
传统滤波器配置中，过程噪声协方差矩阵和测量噪声协方差矩阵通常由工程师手工设定，这要求开发者深入理解各传感器的噪声特性。然而在实际部署中，传感器的噪声特性往往随工作状态和环境条件动态变化 —— 例如 GPS 在开阔天空下精度较高，进入隧道或高楼遮挡区域后测量噪声急剧增大。FusionCore 实现了从创新序列（Innovation Sequence）自动估计测量噪声协方差的机制，滤波器在运行过程中持续分析实际观测值与预测值之间的残差，动态调整各传感器的噪声权重。
这种自适应策略使得 FusionCore 能够在无需人工调参的情况下自动适应不同传感器组合和运行环境。开发者只需提供各传感器的数据接口和基本配置，滤波器会自动学习并匹配实际的噪声水平。官方声称实现 “零手动调参”（Zero Manual Tuning），这在机器人定位领域是相当激进的目标，意味着系统在部署后能够快速适配而无需反复调整滤波器参数。
卡方检验异常值剔除
机器人系统中各类传感器均可能产生突发性故障或异常测量：GPS 信号可能因多径效应产生米级跳变，车轮编码器可能因打滑而产生虚假位移，IMU 可能因冲击而出现尖峰噪声。如果不加筛选地将这些异常值输入滤波器，短期内即可导致估计结果严重偏离真值。FusionCore 在每一帧传感器数据进入滤波器前都执行卡方检验（Chi-squared Test），通过比较测量残差与预设置信阈值判断数据是否有效。
具体实现上，滤波器对每个传感器的更新通道独立计算卡方统计量。假设创新序列服从零均值高斯分布，其协方差矩阵已知，则创新向量与协方差逆的乘积构成卡方统计量，在给定置信水平（如 99.9%）下对应明确的阈值。当统计量超过阈值时，该帧传感器数据被判定为异常并被拒绝参与状态更新。值得注意的是，robot_localization 仅对 GPS 实现类似机制，而 FusionCore 将此防护扩展至 IMU、车轮编码器和 GPS 的全部三个传感器通道，提供了更全面的鲁棒性保障。
ECEF 坐标系下的 GPS 处理
FusionCore 对 GPS 数据的处理方式也区别于常规方案。大多数机器人定位系统将 GPS 坐标投影至局部切平面坐标系（如 UTM 或局部 ENU 帧）后再进行融合，这种处理虽然简化了计算但引入了投影误差，尤其在高纬度地区或大范围移动场景中误差不可忽视。FusionCore 直接在地球中心地球固定（ECEF）坐标系中处理 GPS 测量，将滤波器状态也维护在 ECEF 框架下，从而彻底消除坐标转换带来的系统性偏差。
这种设计对支持高精度 GNSS 接收机（如 RTK GPS）尤为重要。RTK GPS 能够提供厘米级相对定位精度，但前提是系统能够正确处理原始观测数据而不引入额外误差源。FusionCore 的 ECEF 原生处理架构使其在配合 RTK 定位模块时能够充分发挥设备精度潜能，这在需要精确轨迹建图或高精度自主导航的应用中具有实际价值。
NCLT 数据集基准测试
FusionCore 在密歇根大学 North Campus Long-Term（NCLT）数据集上进行了系统性评估，该数据集是机器人定位领域广泛使用的公开基准，包含多种传感器配置和复杂城市场景。测试使用与 robot_localization 相同的输入数据 ——IMU、车轮编码器和 GPS—— 未进行任何手动参数调优。对比结果如下：FusionCore 在六个测试序列中的五个取得更低的平均轨迹误差（ATE RMSE），其中 2012-01-08 序列误差为 5.6 米对比 robot_localization 的 13.0 米，2012-02-04 序列误差为 9.7 米对比 19.1 米，2012-08-20 序列误差为 7.5 米对比 24.1 米。特别值得注意的是，robot_localization 的 UKF 模式在所有序列中均出现 NaN 发散，而 FusionCore 的 UKF 实现保持数值稳定。
唯一出现性能倒退的是 2012-11-04 序列，FusionCore 误差为 28.6 米而 robot_localization 为 9.6 米。这表明在特定场景下 —— 可能涉及特殊的传感器噪声特性或运动模式 —— 自适应噪声估计算法可能产生次优结果。实际部署时建议针对特定任务进行验证测试，必要时可手动调整异常值检验的置信阈值或噪声自适应机制的参数。
工程落地的关键参数
对于计划采用 FusionCore 的开发团队，以下参数值得在部署时重点关注。滤波器状态维度固定为 22 维，无需也不建议手动修改；UKF 的 sigma 点参数采用默认配置即可满足大多数场景。异常值剔除的卡方阈值对应 99.9% 置信度，这一数值在实际测试中表现稳健，但如果环境中传感器噪声显著偏大，可适当放宽阈值以避免频繁拒绝有效测量。GPS 处理模块原生支持 NMEA 0183 entence 输出，配合 u-blox 或类似 GNSS 接收机可直接使用。系统支持 ROS 2 Jazzy（Ubuntu 24.04）和 Humble（Ubuntu 22.04）两个长期维护版本，在树莓派等嵌入式平台可通过忽略 Gazebo 仿真包实现轻量化部署。
综合来看，FusionCore 通过将 IMU 偏置估计、噪声自适应和全传感器异常值剔除等工程优化集成到统一框架中，为 ROS 2 环境下的传感器融合提供了一种开箱即用的高精度方案。尽管在极端场景下可能需要手动干预，但其整体性能表现已经证明了 UKF 路线在机器人定位中的实用价值。
资料来源：FusionCore 项目 GitHub 仓库（https://github.com/manankharwar/FusionCore）