3. PX4 导航与传感器体系
PX4 的 EKF2 并不是“直接读取传感器”的滤波器，而是一个多源融合估计器。它通过 uORB 订阅各传感器驱动发布的主题，把不同频率、不同噪声特性、不同参考系的观测统一到同一套状态向量中。理解每个传感器在 EKF2 中扮演的角色，是调试位置漂移、高度跳变、偏航抖动等问题的前提。
一、核心原理：EKF2 的状态与观测分工
EKF2 维护的核心状态包括：姿态四元数、速度、位置、陀螺零偏、加速度计零偏、磁力计偏置、风估计等。不同传感器并不都直接观测全部状态，而是各自约束其中一部分：
- IMU（陀螺+加速度计）：预测（prediction）的主力。陀螺积分姿态，加速度计在重力对齐后积分速度与位置。它是唯一持续高频推动状态演化的传感器。
- 磁力计：观测偏航（yaw）与磁偏置，修正陀螺积分带来的航向漂移。
- 气压计：观测高度（z 轴位置），抑制加速度计二次积分导致的高度发散。
- GPS：观测水平位置与速度，并提供全局参考；RTK 还可提供高精度位置。
- 视觉/光流/测距：在无 GPS 或室内环境下提供位置、速度或高度观测，属于外部视觉观测源。
关键认知：IMU 负责“推演”，其余传感器负责“纠偏”。EKF2 的鲁棒性来自多源观测的冗余与一致性检查，而不是单一传感器的精度。
二、数据流：从驱动到 EKF2 的完整链路
各传感器驱动通过 uORB 发布标准主题，EKF2 内部按固定顺序处理：先做 IMU 预测，再依次进行各观测更新。下图展示了这一数据流与各传感器在 EKF2 中的角色分工。
三、方法流程：EKF2 的更新顺序与参数控制
EKF2 在每个 IMU 周期执行预测，然后按优先级进行观测更新。理解顺序有助于判断“为什么某个传感器没起作用”。典型流程如下：
- IMU 预测：用陀螺和加速度计推进状态与协方差。
- 磁力计更新：若磁力计健康且融合使能，修正偏航。
- 气压计更新：修正高度与垂直速度。
- GPS 更新：修正水平位置、速度，并辅助航向。
- 视觉/光流更新：按配置修正位置或速度。
- 一致性检查：创新（innovation）超限时触发拒绝或传感器降级。
关键参数决定了各传感器是否参与融合：
EKF2_AID_MASK # 位掩码：GPS、光流、视觉、磁力计等使能
EKF2_HGT_MODE # 高度源：气压计 / 测距 / GPS / 视觉
EKF2_MAG_TYPE # 磁力计模式：自动 / 仅偏航 / 禁用
EKF2_GPS_CTRL # GPS 使用方式：位置、速度、航向
EKF2_OF_CTRL # 光流使能与高度源选择
EKF2_EV_CTRL # 视觉位姿使能
EKF2_BARO_CTRL # 气压计使能调试建议：先用
listener sensor_combined 确认 IMU 数据正常，再逐个检查 sensor_mag、sensor_baro、vehicle_gps_position 是否发布，最后核对 EKF2 参数是否使能对应观测。四、案例场景：室内光流悬停的高度漂移排查
某四旋翼在室内使用光流悬停，出现缓慢上升后突然掉高。排查步骤：
- 检查
EKF2_HGT_MODE：若设为气压计，室内气流扰动会导致高度漂移；应改为测距或光流高度。 - 检查
distance_sensor是否发布且量程覆盖飞行高度。 - 查看
ekf2_timestamps与创新日志，确认高度观测是否被拒绝。 - 确认光流质量与地面纹理，光流速度观测异常会间接影响高度估计。
最终将高度源切换为测距后，漂移消失。这说明传感器角色与参数配置必须匹配场景，而非默认值通吃。
五、常见误区与复盘
| 误区 | 现象 | 正确做法 |
|---|---|---|
| 认为 GPS 能修正高度 | GPS 高度噪声大，直接融合导致高度抖动 | 高度优先用气压计或测距，GPS 高度仅作参考 |
| 磁力计受干扰仍强制融合 | 偏航缓慢旋转或跳变 | 检查磁力计校准与 EKF2_MAG_TYPE，必要时禁用 |
| 忽略 IMU 零偏与振动 | 位置缓慢漂移、创新持续偏大 | 做加速度计/陀螺校准，检查减振与 IMU_GYRO_CUTOFF |
| 光流与测距不同步 | 室内悬停高度与速度估计矛盾 | 统一时间戳，核对 EKF2_OF_CTRL 与测距量程 |
EKF2 的观测拒绝机制会静默丢弃异常传感器数据。若发现某传感器“明明有数据却不生效”，优先查看创新门限与传感器健康状态，而不是反复重启飞控。
总结来看，IMU 是 EKF2 的推演引擎，磁力计、气压计、GPS、视觉与光流是不同维度的纠偏观测。只有理解每个传感器约束的状态分量、数据流路径与使能参数，才能在调试中快速定位问题根源，让状态估计在各类场景下保持稳定可靠。