在移动机器人导航中，精确的位姿估计是实现自主定位与路径规划的基础。单一传感器各有缺陷：IMU 存在漂移、轮式编码器易受打滑影响、GPS 信号会跳跃。FusionCore 作为 ROS 2 社区新兴的传感器融合库，采用无迹卡尔曼滤波（UKF）将 IMU、轮式编码器和 GPS 融合为统一的高可信度位置估计，其在 NCLT 数据集上 5/6 序列优于传统 robot_localization，且具备自适应噪声协方差、IMU 偏置估计、ECEF 原生 GPS 融合等特性。本文聚焦工程化配置与滤波器参数调优，为开发者提供可落地的参数清单与监控要点。
融合架构与状态向量
FusionCore 的核心是 22 维状态向量的 UKF 滤波器，状态包括：三轴位置 (x, y, z)、四元数方向 (qw, qx, qy, qz)、三轴线速度、三轴角速度、三轴线加速度、以及三轴陀螺仪偏置和三轴加速度计偏置。四元数表示避免了欧拉角的奇异性问题，IMU 偏置的在线估计则消除了启动时的校准步骤。滤波器输出 100Hz 的融合里程计 /fusion/odom、兼容 AMCL 与 slam_toolbox 的 /fusion/pose 以及 TF 变换 odom → base_link，可直接作为 Nav2 的里程计来源而无需额外节点或坐标变换。
滤波器的 sigma 点采用 45 点配置，数值稳定性通过 P 矩阵对称化、identity-shift Cholesky 分解修复和角速度方差上限约束保障。延迟补偿机制保存最近 1 秒的 100 条 IMU 消息，当延迟的 GPS 修正到达时，恢复对应时刻的状态快照进行重融合，再将缓冲区中的 IMU 消息回放至当前时刻，从而消除运动模型近似误差对延迟测量的影响。
IMU 参数配置
IMU 是融合系统的主观参考，其噪声参数直接影响滤波器的预测精度。FusionCore 从 IMU 数据手册获取陀螺仪噪声密度和加速度计噪声密度作为 imu.gyro_noise 和 imu.accel_noise 的初始值。典型 6 轴 IMU（如 BMI088）陀螺仪噪声约为 0.005 rad/s，加速度计噪声约为 0.1 m/s²；9 轴 IMU（如 BNO085）因包含磁力计可设置 imu.has_magnetometer: true 以利用 AHRS 提供的绝对航向信息。
imu.remove_gravitational_acceleration 参数控制是否在融合前去除重力分量。大多数 IMU 报告的是原始比力（包含重力），设置为 true 时 FusionCore 使用当前滤波器姿态估计移除重力分量，避免机器人在静止时 Z 轴漂移。对于 6 轴 IMU，必须设置 imu.has_magnetometer: false，因为纯陀螺仪积分的航向会随时间发散，FusionCore 会自动跟踪航向有效性标记，仅在获得独立航向源（双天线、9 轴 AHRS 或车辆行进 5 米以上）后才启用天线杆臂校正。
IMU 帧变换是工程中的常见陷阱。FusionCore 读取每条 IMU 消息的 frame_id 并通过 TF 查询到 base_link 的旋转，将角速度和线性加速度变换后再融合。若 TF 缺失，节点会在启动时输出精确的修复命令：ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id imu_link。这避免了因 IMU 安装位置与机器人中心不一致导致的姿态估计偏差。
GPS 参数配置
FusionCore 原生支持 ECEF（地心地固坐标系）GPS 融合，这与需要 navsat_transform 节点进行局部坐标转换的 robot_localization 截然不同。gnss.base_noise_xy 和 gnss.base_noise_z 分别设置水平和高程方向的基准噪声（米），默认 1.0m 和 2.0m，实际值会根据 HDOP/VDOP 或消息中的完整 3×3 协方差矩阵自动缩放。
GPS 固定质量门控是保证融合可靠性的关键。gnss.min_fix_type 参数指定最低可接受的 GPS 质量：1 为普通 GPS，2 为 DGPS，3 对应 RTK_FLOAT（但 NavSatFix 状态字段无法表达此级别，实际映射为 RTK_FIXED），4 为 RTK_FIXED。默认接受任何有效修复，设置 gnss.min_fix_type: 4 可在 RTK 可用的户外环境中获得厘米级精度。相关参数 gnss.max_hdop: 4.0 和 gnss.min_satellites: 4 提供额外的质量筛选。
天线杆臂校正解决 GPS 天线与机器人基准点不重合的问题。参数 gnss.lever_arm_x/y/z（单位：米）描述天线在机体坐标系中的偏移量，前向为 x，左侧为 y，向上为 z。FusionCore 使用当前姿态旋转矩阵计算杆臂校正：p_antenna = p_base + R × lever_arm。关键的是，杆臂校正仅在航向已通过独立源验证后才会激活，包括：双天线航向消息、9 轴 IMU 发布的完整姿态、或车辆以不低于 0.2 m/s 速度行进超过 5 米且角速度低于 0.3 rad/s。航向未验证时，即使偏置参数非零也不会应用校正，防止因航向错误导致位置估计恶化。
多天线场景下，第二 GPS 接收机使用独立的杆臂参数 gnss.lever_arm2_x/y/z 和独立的话题 gnss.fix2_topic，每个接收机均可配置完整的天线杆臂和噪声模型。
轮式编码器参数配置
轮式编码器提供里程计速度信息，参数 encoder.vel_noise 和 encoder.yaw_noise 分别控制线速度和角速度的过程噪声，默认 0.05 m/s 和 0.02 rad/s。对于差速驱动机器人，这些值反映了轮子直径误差、轮距测量误差和地面不平整引入的累积不确定性。mecanum 驱动或全向移动机器人的横向速度无法被当前运动模型预测，FusionCore 会在诊断中报告此项局限。
ZUPT（零速更新）机制是静止场景下抑制 IMU 漂移的核心功能。当编码器速度低于 0.05 m/s 且角速度低于 0.05 rad/s 时，FusionCore 自动融合零速度伪测量并使用极紧的噪声参数，从而防止静止机器人的速度估计发散。启动阶段的偏置估计通过 init.stationary_window: 2.0 参数实现：滤波器在前 2 秒收集静止 IMU 数据直接估计偏置，将启动瞬态从约 10cm 降至 1cm 以下。
UKF 过程噪声与自适应噪声
UKF 的过程噪声协方差矩阵 Q 通过 ukf.q_position、ukf.q_orientation、ukf.q_velocity、ukf.q_angular_vel、ukf.q_acceleration、ukf.q_gyro_bias 和 ukf.q_accel_bias 参数配置。四元数正则化项 ukf.q_orientation 必须保持极小值（1.0e-9），过大的值会破坏四元数数学运算并在典型 IMU 速率下导致航向漂移和 Z 轴上升。陀螺仪和加速度计偏置的过程噪声（分别为 1.0e-5）极小，因为偏置在实际运行中变化缓慢。
自适应噪声协方差是 FusionCore 区别于传统配置的关键特性。启用 adaptive.imu、adaptive.encoder 和 adaptive.gnss 后，滤波器维护每个传感器最近 50 个 innovation 样本的滑动窗口，估计实际噪声协方差并使用指数移动平均（alpha=0.01）逐步更新 R 矩阵。运行数分钟后，R 收敛至真实传感器特性，无需手动调优。adaptive.window 控制窗口长度，adaptive.alpha 控制更新速度。
异常值剔除与诊断
Mahalanobis 距离检验在每个传感器融合前执行。outlier_threshold_gnss: 16.27、outlier_threshold_enc: 11.34 和 outlier_threshold_imu: 15.09 分别对应三维位置、三维编码器和六维 IMU 在 99.9% 置信度下的卡方分布阈值。GPS 跳跃 500m 的注入测试验证了零位置变化，说明剔除机制有效。协方差下界在门限检验前应用，防止 RTK 级接收机（典型 σxy ~3mm）在滤波器尚未收敛至 RTK 精度时触发自剔除。
/diagnostics 话题以 1Hz 频率发布每个传感器的健康状态，包括：IMU、编码器、GNSS 和滤波器。每个状态项显示 OK 或 WARN 级别，附带异常值计数、航向来源、行驶距离、位置不确定度和更新次数。传感器超时检测（默认 1.0 秒）将静默传感器标记为 STALE，滤波器继续基于其余传感器运行并在传感器恢复后自动重连。
工程落地清单
部署 FusionCore 时，建议按以下顺序验证：首先确保 TF 树完整，启动节点后检查配置阶段的 TF 验证输出；然后在静止状态下观察 ZUPT 是否生效，验证速度保持在 1e-10 量级；接着在已知轨迹上运行，对比融合结果与 RTK 真值或视觉里程计；最后观察诊断话题的异常值计数和航向状态标志。环境预设文件（env_open.yaml、env_urban.yaml、env_canopy.yaml）提供针对不同 GPS 信号条件的噪声缩放，可直接与硬件配置文件组合使用而无需修改硬件参数。
FusionCore 的设计目标是为 ROS 2 提供一个开箱即用的高精度传感器融合方案，填补 robot_localization 在 ECEF GPS 融合、IMU 偏置估计和自适应噪声方面的空白。对于需要快速集成 IMU、GPS 和轮式编码器的移动机器人项目，FusionCore 的完整配置参数体系和诊断能力提供了从实验室验证到现场部署的完整工程路径。
资料来源：FusionCore GitHub 仓库（manankharwar/FusionCore）