PX4 EKF2 辅助函数与卡尔曼更新 ekf_helper.cpp

发表于 更新于 分类于 ， ， 阅读次数： 本文字数： 592 阅读时长 ≈ 2 分钟
解析 PX4 EKF2 ekf_helper.cpp 中的通用辅助函数：measurementUpdate() 卡尔曼更新核心、精度报告、新息测试比率、零偏管理以及航迹推算状态更新。

一、measurementUpdate() — 卡尔曼更新核心

measurementUpdate() 是所有传感器融合的最终执行函数，实现标准 EKF 的测量更新步骤：

标准卡尔曼更新公式

给定观测矩阵 H（1×24）、新息 y（标量）、新息方差 S = HPH’ + R：

EKF2 使用 Joseph 稳定形式（而非简化的 P = (I-KH)P），保证数值正定性。

代码实现（简化）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
void Ekf::measurementUpdate(
    VectorState &H,          // 观测矩阵行向量 H (1×24)
    const float innovation,  // 新息 y = z - Hx
    const float innov_var)   // 新息方差 S
{
    // 1. 计算卡尔曼增益 K = P*H' / S
    VectorState K;
    for (int i = 0; i < _k_num_states; i++) {
        K(i) = 0.0f;
        for (int j = 0; j < _k_num_states; j++) {
            K(i) += P(i,j) * H(j);
        }
        K(i) /= innov_var;
    }

    // 2. 更新状态向量 x = x + K * y
    // 注意：姿态更新通过旋转向量误差注入，不是直接修改
    fuse(K, innovation);

    // 3. 更新协方差 P（Joseph 形式）
    // P = (I - K*H) * P * (I - K*H)' + K * R * K'
    SquareMatrix24f KH;  // K*H 矩阵
    for (int i = 0; i < _k_num_states; i++) {
        for (int j = 0; j < _k_num_states; j++) {
            KH(i,j) = K(i) * H(j);
        }
    }
    SquareMatrix24f ImKH = SquareMatrix24f::identity() - KH;
    P = ImKH * P * ImKH.T() + K.outerProduct(K) * (innov_var - P_contribution);
}

fuse() 函数 — 状态修正

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
void Ekf::fuse(const VectorState &K, const float innovation) {
    // 各状态分量直接加法更新
    _state.vel += K.slice<3,1>(3,0) * innovation;
    _state.pos += K.slice<3,1>(6,0) * innovation;
    _state.gyro_bias  += K.slice<3,1>(9,0)  * innovation;
    _state.accel_bias += K.slice<3,1>(12,0) * innovation;
    _state.mag_I      += K.slice<3,1>(15,0) * innovation;
    _state.mag_B      += K.slice<3,1>(18,0) * innovation;
    _state.wind_vel   += K.slice<2,1>(21,0) * innovation;

    // 姿态：旋转向量误差注入四元数
    const Vector3f dq = K.slice<3,1>(0,0) * innovation;
    _state.quat_nominal = Quatf(_state.quat_nominal * AxisAnglef(dq)).normalized();

    // 零偏限幅（防止估计值超出物理合理范围）
    _state.gyro_bias  = matrix::constrain(_state.gyro_bias,  -_params.gyro_bias_lim, _params.gyro_bias_lim);
    _state.accel_bias = matrix::constrain(_state.accel_bias, -_params.accel_bias_lim, _params.accel_bias_lim);
}

二、fuseDirectStateMeasurement() — 直接状态测量

对于直接测量某个状态量的传感器（如直接测量速度的 GPS），提供简化接口：

1
2
3
4
5
6
7
8
9
10
11
// 单维状态直接测量（避免构造完整 H 矩阵）
void Ekf::fuseDirectStateMeasurement(
    const float innov,
    const float innov_var,
    const int state_index)  // 状态分量索引（如速度 = 3,4,5）
{
    // H 矩阵是单位向量（仅 state_index 处为 1，其余为 0）
    VectorState H{};
    H(state_index) = 1.0f;
    measurementUpdate(H, innov, innov_var);
}

三、精度报告函数

3.1 位置精度 get_ekf_lpos_accuracy()

1
2
3
4
5
6
7
8
void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const {
    // 水平位置精度（1σ，m）
    // = sqrt(P_NN + P_EE)，即位置协方差的水平分量
    *ekf_eph = sqrtf(P(6,6) + P(7,7));

    // 垂直位置精度
    *ekf_epv = sqrtf(P(8,8));
}

这些值通过 estimator_status 话题发布，供 vehicle_local_position.eph/epv 使用，Navigator 和安全模块据此判断定位是否可信。

3.2 新息测试比率

1
2
3
4
5
6
7
8
9
10
float Ekf::getHeadingInnovationTestRatio() const {
    // 返回最近一次航向新息的 NIS 比率
    // > 1.0 表示新息超过 1σ，> 门限² 表示被拒绝
    return _aid_src_yaw.test_ratio;
}

float Ekf::getHorizontalVelocityInnovationTestRatio() const {
    return math::max(_aid_src_gnss_vel.test_ratio[0],
                     _aid_src_gnss_vel.test_ratio[1]);
}

这些比率通过 estimator_innovations 话题发布，是调试传感器融合质量的关键指标。

四、零偏管理

4.1 resetGyroBias() / resetAccelBias()

1
2
3
4
5
6
7
8
9
10
11
void Ekf::resetGyroBias() {
    _state.gyro_bias.setZero();  // 清零零偏估计（保守重置，重新收敛）
    // 重置零偏协方差到初始值（增大不确定度，加快重新估计）
    for (int i = 9; i < 12; i++) {
        P(i,i) = sq(_params.switch_on_gyro_bias);
        // 清除与其他状态的相关性
        for (int j = 0; j < _k_num_states; j++) {
            if (j != i) P(i,j) = P(j,i) = 0.0f;
        }
    }
}

4.2 updateIMUBiasInhibit() — 零偏估计抑制

当飞机处于某些特殊状态时，应停止更新零偏估计（避免动态机动被错误解释为零偏）：

1
2
3
4
5
6
7
8
9
10
void Ekf::updateIMUBiasInhibit() {
    // 条件：飞机正在高动态机动 OR 在地面静止（此时零偏可能被重力成分影响）
    const bool inhibit = _control_status.flags.vehicle_at_rest
                      || _fault_status.flags.bad_acc_vertical;

    if (inhibit) {
        // 清零陀螺/加速度计的卡尔曼增益（P 矩阵对应列不参与更新）
        // 通过 clearInhibitedStateKalmanGains() 实现
    }
}

五、航迹推算（Dead Reckoning）状态

当所有外部定位传感器（GPS/EV/光流）失效后，EKF 进入航迹推算模式，仅靠 IMU 积分维持位置估计：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
void Ekf::updateDeadReckoningStatus() {
    // 判断水平定位辅助源是否活跃
    const bool pos_aiding = _control_status.flags.gps
                         || _control_status.flags.ev_pos
                         || _control_status.flags.opt_flow;

    const bool vel_aiding = _control_status.flags.gps
                         || _control_status.flags.ev_vel;

    if (!pos_aiding && !vel_aiding) {
        // 进入纯航迹推算
        _time_last_hor_pos_aiding = 0;

        // 计算水平位置漂移（仅靠 IMU 积分，位置方差随时间增长）
        // P(6,6) 和 P(7,7) 受过程噪声驱动持续增大
        _control_status.flags.dead_reckoning = true;
    }
}

航迹推算期间，vehicle_local_position.dead_reckoning = true，高层模块（如 mc_pos_control）应降低对位置精度的要求，可能触发悬停或降落。

六、辅助源状态管理 updateAidSourceStatus()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
template<int N>
void Ekf::updateAidSourceStatus(
    AidSourceStatus<N>& status,
    const matrix::Vector<float, N>& observation,
    const matrix::Vector<float, N>& observation_variance,
    const matrix::Vector<float, N>& innovation,
    const matrix::Vector<float, N>& innovation_variance,
    const float gate_size)
{
    status.observation           = observation;
    status.observation_variance  = observation_variance;
    status.innovation            = innovation;
    status.innovation_variance   = innovation_variance;

    // 计算归一化新息平方
    float test_ratio_sum = 0.0f;
    for (int i = 0; i < N; i++) {
        test_ratio_sum += sq(innovation[i]) / innovation_variance[i];
    }
    status.test_ratio = test_ratio_sum / N;
    status.innovation_rejected = (status.test_ratio > sq(gate_size));
}

这一函数统一了所有传感器在融合前的状态更新，是连接 control.cpp 调度逻辑和具体融合实现的桥梁。

七、小结

ekf_helper.cpp 提供了 EKF2 的底层通用操作：

  • measurementUpdate() 是卡尔曼更新的唯一实现点，所有融合最终汇聚于此
  • fuse() 负责将数学修正量正确注入各类状态（特别是姿态的四元数旋转处理）
  • 精度报告函数将协方差矩阵转换为可供高层模块使用的置信度指标
  • 零偏管理和航迹推算状态更新保证了 EKF 在复杂飞行场景中的鲁棒性

上一篇：高度融合与多源仲裁
系列总目录：PX4 v1.16 源码解读总目录
下一章：Navigator 导航模块