PX4 EKF2 协方差矩阵预测 covariance.cpp

发表于 更新于 分类于 ， ， 阅读次数： 本文字数： 843 阅读时长 ≈ 3 分钟
解析 PX4 EKF2 covariance.cpp 的核心职责：协方差矩阵初始化、IMU 驱动的预测传播 P=FPF'+Q、数值稳定性约束，以及各状态量的初始不确定度配置。

一、协方差矩阵的作用

协方差矩阵 P 是 EKF 的核心数据结构之一，其物理意义是状态估计误差的统计描述：

  • P 的对角元 P[i][i] = 状态分量 i 的方差，即估计误差的平方期望
  • P 的非对角元 P[i][j] = 状态 i 与 j 的协方差，描述误差的相关性

P 越大，表示 EKF 对该状态量的估计越不确信；卡尔曼增益 K 正比于 P，P 大时 EKF 更信任传感器测量，给予更大的修正力度。

EKF2 的 P 矩阵是 24×24（含地形则 25×25），covariance.cpp 负责其全生命周期管理。

二、协方差初始化 initialiseCovariance()

初始化在 initialiseFilter() 中调用，将 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
31
void Ekf::initialiseCovariance() {
    P.setZero();

    // 姿态不确定度（rad²）
    // roll/pitch 由加速度计估计，精度约 0.1 rad
    // yaw 由磁力计/GPS估计，精度约 0.3 rad；无磁力计时设大一些
    P(0,0) = P(1,1) = sq(0.1f);
    P(2,2) = sq((_params.mag_fusion_type == MagFuseType::NONE) ? 1.0f : 0.3f);

    // 速度不确定度（m/s）²：初始静止约 0.1 m/s
    P(3,3) = P(4,4) = P(5,5) = sq(0.1f);

    // 位置不确定度（m）²：初始 GPS 精度约 5 m
    P(6,6) = P(7,7) = sq(5.0f);
    P(8,8) = sq(1.0f);  // 高度精度较好

    // 陀螺仪零偏（rad/s）²
    P(9,9) = P(10,10) = P(11,11) = sq(rad_per_sec_to_dpss(_params.switch_on_gyro_bias));

    // 加速度计零偏（m/s²）²
    P(12,12) = P(13,13) = P(14,14) = sq(_params.switch_on_accel_bias);

    // 地球磁场（Gauss）²：由磁力计精度决定
    P(15,15) = P(16,16) = P(17,17) = sq(_params.mag_noise);

    // 机体磁偏差（Gauss）²
    P(18,18) = P(19,19) = P(20,20) = sq(_params.mag_bias_faded);

    // 风速（m/s）²：初始风速未知，设较大值
    P(21,21) = P(22,22) = sq(_params.initial_wind_uncertainty);
}

三、协方差预测 predictCovariance()

predictCovariance() 实现了离散时间 EKF 的协方差传播：

其中：

  • 是状态转移矩阵（系统雅可比），由 IMU 数据计算
  • 是过程噪声协方差矩阵

3.1 F 矩阵的稀疏性利用

完整的 F 矩阵是 24×24，但大部分元素为零（稀疏矩阵）。EKF2 使用 Symforce 自动生成的符号化展开代码，只计算非零元素，避免全矩阵乘法（24³ = 13824 次浮点运算 → 优化后仅需数百次）。

关键的 F 矩阵耦合项（非零非对角块）：

行（状态） 列（状态） 物理含义
速度 姿态误差 速度预测依赖机体→导航系旋转，姿态误差影响速度预测误差
速度 加速度计零偏 加速度计零偏直接影响速度积分
位置 速度 位置是速度的积分
姿态 陀螺仪零偏 陀螺仪零偏直接影响姿态积分

3.2 过程噪声 Q

Q 矩阵描述系统动力学模型的不确定性（随机游走噪声）。EKF2 的 Q 是对角矩阵：

1
2
3
4
5
6
7
8
9
// 陀螺仪过程噪声（持续随机游走）
const float gyro_noise_sq = sq(_params.gyro_noise) * dt;
// 加速度计过程噪声
const float accel_noise_sq = sq(_params.accel_noise) * dt;

// Q 贡献到协方差预测（简化表达）：
// 姿态方向：受陀螺仪噪声驱动
// 速度方向：受加速度计噪声驱动
// 零偏：受零偏随机游走噪声驱动（通常很小，确保零偏估计缓慢漂移）

四、数值稳定性约束 constrainCovariance()

实际代码中协方差矩阵会因数值误差导致对称性破坏或对角元变负，需要定期约束：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
void Ekf::constrainCovariance() {
    // 1. 强制对称：P = (P + P') / 2
    for (int i = 0; i < _k_num_states; i++) {
        for (int j = i+1; j < _k_num_states; j++) {
            P(i,j) = P(j,i) = 0.5f * (P(i,j) + P(j,i));
        }
    }

    // 2. 对角元下限（防止除零）
    for (int i = 0; i < _k_num_states; i++) {
        P(i,i) = math::max(P(i,i), _min_cov_diag[i]);
    }

    // 3. 对角元上限（防止发散）
    // 姿态方差上限 (1 rad)²
    P(0,0) = math::min(P(0,0), 1.0f);
    // 速度方差上限 (1e6 m/s)²
    P(3,3) = math::min(P(3,3), 1e6f);
    // ...
}

五、协方差重置场景

以下情况会触发局部或全局协方差重置：

场景 动作
initialiseFilter() 调用 全局重置为初始对角矩阵
磁力计融合崩溃（fuseYaw 病态） 调用 initialiseCovariance()（全局重置）
速度/位置状态重置 对应分块清零（消除与其他状态的相关性）
传感器切换（如 GPS→EV） 对对应状态协方差膨胀（增大不确定度）
长时间 GPS 丢失 位置方差随时间增大（过程噪声累积）

局部重置示例（GPS 速度重置时）：

1
2
3
4
5
6
7
8
9
// 速度重置时，清除速度与其他状态的协方差（相关性不再有效）
for (int i = 0; i < _k_num_states; i++) {
    P(3,i) = P(i,3) = 0.0f;
    P(4,i) = P(i,4) = 0.0f;
    P(5,i) = P(i,5) = 0.0f;
}
// 重置速度方差为 GPS 速度精度的平方
P(3,3) = P(4,4) = sq(gps_sample.sacc);
P(5,5) = sq(gps_sample.vacc);

六、小结

covariance.cpp 实现了 EKF 的不确定度管理：

  1. 初始化——设定合理先验，反映各传感器精度
  2. 预测传播——Symforce 自动代码高效计算 F·P·F’+Q
  3. 数值稳定——对称强制 + 上下限约束，防止计算病态
  4. 局部重置——传感器切换时精确清理相关性，避免错误传播

P 矩阵的健康状态直接决定卡尔曼增益的合理性，是整个 EKF 系统可靠工作的基础。

上一篇：ekf.cpp 核心主循环
下一篇：融合调度 control.cpp
系列总目录：PX4 v1.16 源码解读总目录