PX4 EKF2 应用管理层 EKF2.cpp 详解

一、EKF2.cpp 的职责定位

EKF2.cpp 是 EKF 算法库与 PX4 飞控系统之间的粘合层（Glue Layer）。它不实现任何 EKF 数学算法，其职责是：

1. 通过 uORB 订阅传感器原始数据
2. 将 uORB 数据格式转换为 EKF 库接受的内部格式（imuSample, gnssSample 等）
3. 调用 EKF 算法库执行估计
4. 将 EKF 输出发布为 uORB 话题供飞控其他模块使用

EKF2 运行在 WorkQueue（wq:ekf2）上，由 sensor_combined 或 vehicle_imu 话题到达时触发 Run()。

相关基础：uORB 通信机制（上），uORB 通信机制（下）

---

二、Run() 主循环

void EKF2::Run() {
    // 1. 接收 IMU 数据（vehicle_imu 或 sensor_combined）
    if (_vehicle_imu_sub.update(&_vehicle_imu)) {
        imuSample imu_sample;
        // 转换 delta_velocity → imuSample.delta_vel
        // 转换 delta_angle  → imuSample.delta_ang
        _ekf.setImuData(imu_sample);  // 推入 IMU RingBuffer
    }

    // 2. 接收其他传感器（非每次 IMU 到达都有）
    if (_vehicle_gps_position_sub.updated()) {
        vehicle_gps_position_s gps;
        _vehicle_gps_position_sub.copy(&gps);
        gnssSample gnss;
        // 格式转换：经纬度、速度、精度...
        _ekf.setGpsData(gnss);
    }
    // 类似处理: setBaroData(), setMagData(), setRangeData() ...

    // 3. 调用 EKF 主循环（每次 IMU 到达时执行）
    bool updated = _ekf.update();

    if (updated) {
        // 4. 发布估计结果
        PublishAttitude();       // vehicle_attitude
        PublishLocalPosition();  // vehicle_local_position
        PublishGlobalPosition(); // vehicle_global_position
        PublishOdometry();       // vehicle_odometry
        PublishStatus();         // estimator_status
        PublishInnovations();    // estimator_innovations
    }
}

---

三、数据订阅与发布

3.1 订阅的 uORB 话题

话题	类型	说明
vehicle_imu	按实例订阅	每个 IMU 芯片对应一个实例（EKF2 有多实例）
vehicle_gps_position	可选	GNSS 位置/速度
vehicle_magnetometer	可选	磁力计数据
vehicle_air_data	可选	气压计气压和高度
distance_sensor	可选	测距仪
vehicle_optical_flow	可选	光流
vehicle_visual_odometry	可选	外部视觉/VIO/MoCap
landing_target_pose	可选	精确降落目标
airspeed_validated	可选	空速

3.2 发布的 uORB 话题

话题	下游使用者	关键字段
vehicle_attitude	mc_pos_control, Commander	姿态四元数 q
vehicle_local_position	mc_pos_control, Navigator	NED 位置/速度/加速度
vehicle_global_position	Navigator, GCS	经纬度、海拔
vehicle_odometry	外部系统	完整位姿
estimator_status	QGC 健康监控	新息、方差、错误标志
estimator_innovations	调试	各传感器新息和方差
wind	固定翼空速控制	风速估计
ekf_gps_drift	健康监控	GPS 漂移

---

四、多实例架构与 EKF2Selector

4.1 多实例设计

PX4 飞控板可能搭载多个 IMU（如 Pixhawk 6C 有 3 个 IMU）。EKF2 对每个 IMU 单独运行一个 EKF 实例，相互独立。每个实例都发布完整的估计结果，只是实例编号不同（vehicle_attitude:0、vehicle_attitude:1 等）。

4.2 EKF2Selector 选择策略

EKF2Selector 负责从多个 EKF 实例中选择一个最优输出，供控制器使用：

flowchart TD
    EKF0["EKF 实例 0<br/>(IMU 0)"] --> Selector
    EKF1["EKF 实例 1<br/>(IMU 1)"] --> Selector
    EKF2["EKF 实例 2<br/>(IMU 2)"] --> Selector
    Selector["EKF2Selector<br/>综合评分"] --> Primary["Primary 输出<br/>vehicle_attitude等"]
    Selector --> Fallback["Fallback 切换<br/>(主实例失效时)"]

评分准则（综合多个因素）：

1. 姿态估计健康（没有大的姿态翻转）
2. IMU 振动水平（滤除振动严重的 IMU）
3. 新息检验通过率（GPS/Baro/Mag 新息是否合理）
4. 传感器错误标志（卡尔曼滤波器未发散）

切换策略：Primary 实例评分显著降低时，无缝切换至 Fallback 实例。切换时对位置状态进行对齐，避免跳变。

---

五、参数更新机制

EKF2 通过 updateParams() 将 PX4 参数系统的值同步到 EKF 库内部的 parameters 结构：

void EKF2::updateParams() {
    ModuleParams::updateParams();  // 更新 @Parameter 声明的成员变量

    // 手动同步部分参数到 EKF 内部结构
    _ekf.get_mag_decl_deg()  = _param_ekf2_mag_decl.get();
    _ekf.get_gps_delay_ms()  = _param_ekf2_gps_delay.get();
    // ...
}

参数变更（通过 QGC 或 MAVLink 命令修改 EKF2_* 参数）触发 parameter_update 话题，EKF2 订阅后在下次 Run() 调用 updateParams()。

---

六、健康状态发布

estimator_status 是 EKF 健康状态的核心话题，关键字段：

struct estimator_status_s {
    float   vibe[3];              // 振动水平估计
    float   output_tracking_error[3]; // OutputPredictor 跟踪误差
    uint64_t time_slip;           // 积分时间滑移（实时性指标）

    bool    gps_check_fail_flags; // GPS 预检失败标志位
    uint16_t filter_fault_flags;  // 滤波器内部错误（bit flags）
    uint16_t innovation_check_flags; // 各传感器新息检验结果

    float   pos_horiz_accuracy;   // 水平位置精度（m, 95%）
    float   pos_vert_accuracy;    // 垂直位置精度（m, 95%）
    // ...
};

filter_fault_flags 的任意位置 1 都表示 EKF 状态异常（如协方差发散、四元数非法等），Commander 会据此触发估计器失效保护。

---

七、小结

EKF2.cpp 的核心价值在于：将 EKF 纯算法库与 PX4 模块化架构解耦。算法库（/EKF/）不依赖任何 PX4 特定的 uORB、参数系统，可以独立编译和单元测试；而 EKF2.cpp 负责所有 PX4 特定的 I/O 适配。

多实例 + Selector 的架构是 PX4 冗余估计的实现基础，在飞行安全上有实质意义。

上一篇：State.h 状态向量
下一篇：ekf.cpp 核心主循环
系列总目录：PX4 v1.16 源码解读总目录