PX4 EKF2 ekf.cpp 核心主循环详解

一、update() 主循环结构

ekf.cpp 的 update() 函数是 EKF 算法的最高级入口，每次 IMU 降采样完成后由 EKF2.cpp 调用一次（约 250 Hz）：

bool Ekf::update() {
    // 1. 尝试从 IMU RingBuffer 取最旧数据
    if (!_imu_buffer.pop_first_older_than(_time_delayed_us,
                                           &_imu_sample_delayed)) {
        return false;  // 暂无延迟时刻的 IMU 数据，跳过
    }

    // 2. 首次运行时初始化滤波器
    if (!_filter_initialised) {
        _filter_initialised = initialiseFilter();
        if (!_filter_initialised) return false;
    }

    // 3. 计算本步 dt（使用 IMU 样本内置时间）
    _dt_ekf_avg = 0.998f * _dt_ekf_avg + 0.002f * _imu_sample_delayed.delta_ang_dt;

    // 4. 状态预测（积分 IMU 增量到状态向量）
    predictState(_imu_sample_delayed);

    // 5. 协方差预测（P = F*P*F' + Q）
    predictCovariance(_imu_sample_delayed);

    // 6. 融合调度（调用各传感器融合函数）
    controlFusionModes(_imu_sample_delayed);

    // 7. 更新 OutputPredictor（延迟修正传播到实时状态）
    _output_predictor.correctOutputStates(_imu_sample_delayed.time_us,
                                           getQuaternion(), getVelocity(),
                                           getPosition(), getGyroBias());

    return true;
}

---

二、初始化条件 initialiseFilter()

EKF 初始化是一个积累静止数据的过程，需满足多个条件：

flowchart TD
    Start("[首次调用]") --> CheckIMU{"IMU 数据<br/>是否充足?"}
    CheckIMU -->|否| WaitIMU["等待积累足够 IMU 样本"]
    CheckIMU -->|是| CalcTilt["用加速度计计算 roll / pitch"]
    CalcTilt --> CheckYaw{"有可靠航向源?"}
    CheckYaw -->|磁力计有效| MagYaw["用磁力计初始化 yaw"]
    CheckYaw -->|GPS 双天线| GpsYaw["用 GPS 航向初始化 yaw"]
    CheckYaw -->|无磁无GPS| ZeroYaw["yaw = 0, 等待校正"]
    MagYaw --> InitCov["初始化协方差 P"]
    GpsYaw --> InitCov
    ZeroYaw --> InitCov
    InitCov --> CheckGPS{"GPS 可用且<br/>预检通过?"}
    CheckGPS -->|是| SetOrigin["设置本地坐标原点"]
    CheckGPS -->|否| BaroOrigin["用气压计高度设定 Z 轴原点"]
    SetOrigin --> Done("[初始化完成]")
    BaroOrigin --> Done

初始化失败的主要原因：

• IMU 在运动（加速度计不稳定，无法估计重力方向）
• 磁力计数据异常（软铁干扰大）
• GPS 预检失败（精度差、卫星数不足）

---

三、predictState() 状态预测数学

状态预测基于 IMU 的角增量 和速度增量 （加速度计积分 - 已去除估计重力）：

3.1 姿态预测

姿态用四元数表示，角增量转换为旋转四元数后与名义四元数相乘：

代码实现：

const Quatf dq(AxisAnglef{imu.delta_ang - _state.gyro_bias * imu.delta_ang_dt});
_state.quat_nominal = (_state.quat_nominal * dq).normalized();

注意：角增量在应用前减去了陀螺仪零偏估计 _state.gyro_bias * dt，这是 EKF 实时补偿传感器偏差的体现。

3.2 速度预测

速度在导航（NED）坐标系下积分，加速度增量需从机体系转到导航系：

其中 是机体到导航系的旋转矩阵（由 _state.quat_nominal 提取），（NED 坐标系重力朝 D 轴正方向）。

const Vector3f delta_vel_nav =
    _R_to_earth * (imu.delta_vel - _state.accel_bias * imu.delta_vel_dt)
    + _earth_rate_NED % imu.delta_vel * imu.delta_vel_dt;  // Coriolis 修正

_state.vel += delta_vel_nav;
_state.vel(2) += CONSTANTS_ONE_G * imu.delta_vel_dt;  // 重力

3.3 位置预测

位置用梯形积分（避免纯后向欧拉的滞后）：

_state.pos += (_state.vel + 0.5f * delta_vel_nav) * imu.delta_vel_dt;

3.4 零偏/磁场/风速预测

这些缓慢变化的量使用随机游走模型（不预测，保持不变，仅在协方差矩阵中增加过程噪声）：

// gyro_bias, accel_bias, mag_I, mag_B, wind_vel 不改变名义值
// 过程噪声在 predictCovariance() 中添加到对应对角元

---

四、水平加速度健康监测

预测中会实时检测加速度计异常：

// 检查削波
const bool delta_vel_clipped =
    imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2];

if (delta_vel_clipped) {
    // 抑制被削波轴的卡尔曼增益，避免错误修正
    clearInhibitedStateKalmanGains();
}

---

五、controlFusionModes() 调用时机

在每步 IMU 预测后，controlFusionModes() 检查各传感器 RingBuffer 是否有延迟时刻的新数据：

flowchart LR
    predict["predictState +<br/>predictCovariance"] --> ctrl["controlFusionModes"]
    ctrl --> gps["controlGpsFusion<br/>位置/速度"]
    ctrl --> mag["controlMagFusion<br/>航向/磁场"]
    ctrl --> baro["controlBaroFusion<br/>高度"]
    ctrl --> rng["controlRangeFinderFusion<br/>高度"]
    ctrl --> flow["controlOpticalFlowFusion<br/>光流速度"]
    ctrl --> ev["controlEvFusion<br/>外部视觉"]
    ctrl --> height["controlHeightFusion<br/>多源高度仲裁"]
    ctrl --> yaw["controlGpsYawFusion<br/>GPS航向"]

每个 control*Fusion() 函数决定：

1. 是否有新数据需要融合
2. 是否通过新息检验（未通过则拒绝）
3. 是否调用 measurementUpdate() 执行卡尔曼更新

---

六、小结

ekf.cpp 的 update() 实现了预测 → 融合的标准 EKF 节拍：

• 预测阶段：IMU 积分驱动，每步固定执行，高频（250 Hz）
• 更新阶段：由传感器数据到达时机决定，各传感器独立异步触发
• 零偏估计：通过协方差传播和卡尔曼更新逐步收敛，不在预测阶段显式积分

理解 update() 的执行流程是读懂后续 covariance.cpp 和 control.cpp 的前提。

上一篇：EKF2 应用管理层
下一篇：协方差矩阵预测 covariance.cpp
系列总目录：PX4 v1.16 源码解读总目录