PX4 EKF2 common.h 公共数据结构详解

一、传感器样本结构体

common.h 是 EKF 库的公共头文件，定义了所有模块共享的数据结构和枚举类型。这些结构体被 RingBuffer<T> 模板类直接实例化使用。

1.1 IMU 样本 imuSample

struct imuSample {
    uint64_t    time_us{};           // 采样时间戳（微秒）
    Vector3f    delta_ang{};         // 角增量（rad），= ω * dt（陀螺仪积分）
    Vector3f    delta_vel{};         // 速度增量（m/s），= a * dt（加速度计积分）
    float       delta_ang_dt{};      // 角增量对应的时间间隔
    float       delta_vel_dt{};      // 速度增量对应的时间间隔
    bool        delta_vel_clipping[3]{};  // 各轴加速度削波标志
};

关键设计说明：

• 存储的是增量（delta），而非原始角速度/加速度。增量在降采样过程中累积，避免混叠。
• delta_vel_clipping 用于检测加速度计饱和，饱和时抑制对应轴的卡尔曼增益（clearInhibitedStateKalmanGains）。

1.2 GNSS 样本 gnssSample

struct gnssSample {
    uint64_t time_us{};             // 时间戳
    double   lat{};                 // 纬度（度）
    double   lon{};                 // 经度（度）
    float    alt{};                 // WGS84 高度（米）
    float    sacc{};                // 速度精度（m/s，1σ）
    float    hacc{};                // 水平位置精度（m，1σ）
    float    vacc{};                // 垂直位置精度（m，1σ）
    Vector3f vel{};                 // NED 速度（m/s）
    float    pdop{};                // 位置精度因子
    uint8_t  nsats{};               // 可见卫星数
    bool     fix_type{};            // 定位类型（3D FIX）
    float    yaw{};                 // GPS 航向（需双天线）
    float    yaw_acc{};             // 航向精度
    float    yaw_offset{};          // 安装偏置
};

1.3 磁力计样本 magSample

struct magSample {
    uint64_t time_us{};
    Vector3f mag{};    // 机体坐标系磁场矢量（Gauss）
    bool     reset{};  // 传感器重置标志
};

磁力计在 EKF 中负责航向约束（yaw fusion）以及地球磁场矢量估计（3 维）+ 机体磁偏差估计（3 维），共 6 个状态分量。

1.4 其他样本结构

结构体	主要字段	用途
baroSample	time_us, hgt(m)	气压计高度融合
gpsSample（旧名）	→ gnssSample	—
rangeSample	time_us, rng(m), quality	测距仪高度融合
airspeedSample	time_us, true_airspeed, eas2tas	空速融合
flowSample	time_us, flow_xy_rad, gyro_xyz_rad, quality	光流融合
extVisionSample	time_us, pos, vel, quat, pos_var, vel_var, angVar, reset_counter	外部视觉融合
dragSample	time_us, accelXY(机体坐标系)	阻力模型辅助风速估计

---

二、辅助源状态结构 AidSourceStatus

AidSourceStatus<N> 是一个模板结构，N 为观测维数（1、2 或 3 维）：

template<int N>
struct AidSourceStatus {
    // 融合决策标志
    bool    fusion_enabled   {false};  // 该辅助源是否使能
    bool    data_available   {false};  // 当前是否有有效数据
    bool    innovation_rejected {false}; // 新息检验是否拒绝
    bool    fused            {false};  // 本周期是否完成融合

    // 新息相关
    matrix::Vector<float, N> innovation{};           // 新息 y = z - Hx
    matrix::Vector<float, N> innovation_variance{};  // 新息方差 S = HPH'+R
    matrix::Vector<float, N> observation{};          // 观测值 z
    matrix::Vector<float, N> observation_variance{}; // 观测噪声方差 R

    float    test_ratio{};         // 归一化新息平方 χ² 检验比率
    uint64_t time_last_fuse_us{};  // 上次成功融合的时间戳
};

设计要点：

• 新息拒绝机制（innovation_rejected）基于归一化新息平方（NIS）检验：test_ratio = innov² / S。若超过阈值（通常 EKF2_*_GATE 参数控制），则本周期拒绝该测量。
• time_last_fuse_us 用于检测传感器超时，决定是否降级（fallback）。

---

三、EKF 参数结构 parameters

parameters 结构体集中存放所有 EKF 可调参数。大部分与 EKF2_* 类 PX4 参数对应，通过 EKF2.cpp 的 updateParams() 写入。

关键参数分类

struct parameters {
    // ---- 传感器噪声 ----
    float gyro_noise        {1.5e-2f};  // 陀螺仪过程噪声（rad/s）
    float accel_noise       {3.5e-1f};  // 加速度计过程噪声（m/s²）
    float baro_noise        {2.0f};     // 气压计高度噪声（m，1σ）
    float gps_vel_noise     {0.5f};     // GPS速度噪声（m/s）
    float gps_pos_noise     {0.5f};     // GPS位置噪声（m）
    float mag_noise         {5.0e-2f};  // 磁力计噪声（Gauss）

    // ---- 新息拒绝阈值（"gate" = 标准差倍数的平方）----
    float heading_innov_gate   {2.6f};  // 航向新息门 (σ)
    float gps_vel_innov_gate   {5.0f};
    float gps_pos_innov_gate   {5.0f};
    float baro_innov_gate      {5.0f};

    // ---- 时间延迟（ms）----
    int32_t gps_delay_ms    {110};
    int32_t baro_delay_ms   {0};
    int32_t mag_delay_ms    {0};

    // ---- 传感器使能标志 ----
    int32_t mag_fusion_type  {0};  // 0=自动, 1=磁力计(3D), 4=无磁力计
    bool    gnss_vel_use     {true};
    bool    gnss_pos_use     {true};
    bool    gnss_hgt_use     {false};

    // ---- 零偏估计限幅 ----
    float gyro_bias_lim      {0.349f};  // ≈20°/s，陀螺仪零偏上限（rad/s）
    float accel_bias_lim     {0.4f};    // 加速度计零偏上限（m/s²）
    // ...
};

设计原则：EKF 的参数调试核心在于过程噪声 vs 观测噪声的权衡：

• 过程噪声大 → EKF 对模型置信度低 → 更信任传感器 → 响应快但更嘈杂
• 观测噪声大 → EKF 对传感器置信度低 → 更信任模型预测 → 平滑但滞后

---

四、枚举类型

4.1 高度传感器类型

enum class HeightSensor {
    BARO  = 0,
    GNSS  = 1,
    RANGE = 2,
    EV    = 3,
    NONE  = 4,
};

4.2 偏航估计模式

enum class MagFuseType {
    AUTO   = 0,   // 自动（室外用磁力计，室内用陀螺积分）
    HEADING = 1,  // 仅航向融合
    MAG_3D  = 2,  // 完整3D磁场融合
    NONE    = 5,  // 无磁力计（依赖双天线GPS或EV）
    // ...
};

---

五、小结

common.h 是 EKF 内部所有模块的数据契约：

1. 样本结构体 → 规定了 RingBuffer 和 estimator_interface 如何存储传感器数据
2. AidSourceStatus → 统一了各传感器融合的状态报告接口
3. parameters → 集中管理调参入口，与 PX4 参数系统对接

理解这些数据结构是阅读后续融合代码（control.cpp、covariance.cpp 等）的前提。

相关文章：RingBuffer 环形缓冲区
上一篇：EKF2 目录与层级架构
系列总目录：PX4 v1.16 源码解读总目录