PX4 EKF2 State.h 状态向量详解

发表于 更新于 分类于 ， ， 阅读次数： 本文字数： 682 阅读时长 ≈ 2 分钟
解析 PX4 EKF2 状态向量的完整定义，包括 24 维 StateSample 的内存布局、各分量物理意义、索引映射以及姿态用旋转向量误差代替四元数的数学原因。

一、状态向量概述

EKF 的状态向量 是估计器运行的核心，对应着 EKF 在某一时刻对飞机真实状态的最优估计。EKF2 中状态向量的维数为 24（若含地形估计则为 25）：

分量组 符号 维数 物理意义
姿态旋转向量误差 3 当前姿态与参考四元数之间的小角误差（rad）
NED 速度 3 北东地坐标系速度（m/s）
NED 位置 3 北东地坐标系位置（m）
陀螺仪零偏 3 角速度偏差估计（rad/s）
加速度计零偏 3 加速度偏差估计（m/s²）
地球磁场（NED） 3 导航坐标系地球磁场分量（Gauss）
机体磁偏差 3 机体固连磁场偏差（Gauss）
风速（NE） 2 北向/东向风速（m/s）
合计 — 24 —

二、StateSample 结构体（state.h）

PX4 EKF2 通过 Symforce 自动生成状态结构体，物理布局为：

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
struct StateSample {
    // [0:2]   姿态旋转向量误差（δθ）
    Vector3f    quat_nominal{};  // 实际类型：Quatf，但索引操作用 rotation_vector
    // 注意：内部运算使用四元数 _state.quat_nominal（Quatf）
    // 协方差矩阵 P 对应的误差状态是旋转向量 δθ（3维），而非四元数（4维）

    // [3:5]   NED速度（m/s）
    Vector3f    vel{};

    // [6:8]   NED位置（m）
    Vector3f    pos{};

    // [9:11]  陀螺仪零偏（rad/s）
    Vector3f    gyro_bias{};

    // [12:14] 加速度计零偏（m/s²）
    Vector3f    accel_bias{};

    // [15:17] 地球磁场NED分量（Gauss）
    Vector3f    mag_I{};

    // [18:20] 机体磁偏差（Gauss）
    Vector3f    mag_B{};

    // [21:22] 风速 North/East（m/s）
    Vector2f    wind_vel{};

    // [23]    地形高度（m，若启用 HAGL 估计）
    // float   terrain{};  // 在扩展状态中
};

三、为何用旋转向量误差而非四元数

问题：四元数是 4 维，但只有 3 个自由度（受 约束），直接用于 EKF 协方差矩阵会造成奇异。

解决方案：EKF2 采用误差状态卡尔曼滤波（ESKF）：

  • 名义状态（nominal state）：_state.quat_nominal — 存储完整四元数，用于预测
  • 误差状态（error state）：旋转向量 — 用于卡尔曼更新（协方差矩阵 P 是 24×24）

融合时，卡尔曼增益计算出误差修正量 ，再通过：

将误差注入名义四元数，最后对四元数归一化。

四、协方差矩阵 P 的维数与索引

协方差矩阵 P 是 24×24（或 25×25）浮点数矩阵，其索引与状态分量的对应：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
// 状态索引枚举（P矩阵行/列索引）
enum StateIndex {
    // 旋转向量误差
    quat_error_0 = 0,
    quat_error_1 = 1,
    quat_error_2 = 2,

    vel_N = 3, vel_E = 4, vel_D = 5,
    pos_N = 6, pos_E = 7, pos_D = 8,

    gyro_bias_0 = 9,  gyro_bias_1 = 10, gyro_bias_2 = 11,
    accel_bias_0 = 12, accel_bias_1 = 13, accel_bias_2 = 14,

    mag_I_0 = 15, mag_I_1 = 16, mag_I_2 = 17,
    mag_B_0 = 18, mag_B_1 = 19, mag_B_2 = 20,

    wind_N = 21, wind_E = 22,

    // 地形（扩展状态）
    terrain = 23,
};

五、OutputPredictor 的输出状态

OutputPredictor 维护独立的实时积分状态（无延迟），供飞控直接使用：

1
2
3
4
5
6
7
8
9
10
11
12
13
struct outputSample {
    uint64_t    time_us{};
    Quatf       quat_nominal{};   // 实时姿态四元数
    Vector3f    vel{};            // NED速度
    Vector3f    pos{};            // NED位置
};

struct outputVert {
    float   vert_vel{};           // 垂直速度（D方向）
    float   vert_vel_integ{};     // 垂直速度积分（用于高度修正）
    float   dt{};
    uint64_t time_us{};
};

OutputPredictor 以 IMU 原始频率（1 kHz）运行，每收到延迟 EKF 的修正量后，通过互补滤波对实时积分状态进行校正，保证输出的低延迟与无漂移。

六、状态初始化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
void Ekf::initialiseFilter() {
    // 姿态：由加速度计计算初始 roll/pitch，yaw=0 或磁力计/GPS航向
    _state.quat_nominal = Quaternion::from_euler(...);

    // 速度：全部归零
    _state.vel.setZero();

    // 位置：归零（本地原点 = 起飞点）
    _state.pos.setZero();

    // 零偏：归零（通过融合逐步估计）
    _state.gyro_bias.setZero();
    _state.accel_bias.setZero();

    // 磁场：由磁力计测量设定初始值
    _state.mag_I = _mag_sample_delayed.mag; // 近似值，后续精化

    // 协方差 P：初始化为对角矩阵，各项为初始不确定度的平方
    initialiseCovariance();
}

七、小结

  • EKF2 使用 24 维（含地形 25 维）误差状态向量，协方差矩阵 P 也是对应维数。
  • 姿态用旋转向量误差 + 名义四元数的 ESKF 形式，避免约束奇异性。
  • 状态索引与 P 矩阵的行/列一一对应，是阅读协方差预测（covariance.cpp）和融合代码（measurementUpdate()）的关键。

上一篇：RingBuffer 环形缓冲区
下一篇：EKF2 应用管理结构 EKF2.cpp
系列总目录：PX4 v1.16 源码解读总目录