PX4 飞控源码剖析：姿态解算与 EKF 滤波器调优

引言

PX4 Autopilot 是目前最流行的开源飞控系统之一，广泛应用于多旋翼、固定翼、VTOL 等多种机型。其 EKF2（扩展卡尔曼滤波器）模块是整个状态估计的核心，负责融合 IMU、GPS、气压计、磁力计等传感器数据。

姿态解算基础

PX4 中姿态表示采用四元数（Quaternion），相比欧拉角避免了万向节锁死。源码中核心实现在 src/modules/ekf2/EKF/ 目录下，关键结构：

struct StateSample {
    Quatf  quat_nominal;   // 名义四元数（主体）
    Vector3f vel;           // 速度 NED 坐标系
    Vector3f pos;           // 位置
    Vector3f delta_ang_bias; // 陀螺仪偏置
    Vector3f delta_vel_bias; // 加速度计偏置
};

EKF 预测与更新

EKF 的核心是两个步骤：

1. Predict（预测）：利用 IMU 陀螺仪和加速度计数据推算状态
2. Update（更新）：利用 GPS、气压计等外部传感器修正预测值

在 PX4 中，IMU 数据以 250Hz 驱动预测，GPS 以 5Hz 驱动更新。源码中 Ekf::predictState() 和 Ekf::controlFusionModes() 是关键入口。

实际调优经验

• EKF2_IMU_NOISE：IMU 噪声协方差，该值太小会导致滤波器过度信任 IMU、忽略 GPS 修正；太大则姿态响应迟钝。建议从 0.01 开始微调。
• EKF2_GPS_P_NOISE：GPS 位置噪声，空旷场地设置 0.5，有遮挡环境放宽至 2.0。
• EKF2_MAG_NOISE：磁力计噪声，注意区分机架磁场干扰，必要时启用磁力计校准。

在实际试飞中，通过 ekf2_replay 工具回放飞行日志离线调参，可将定位误差从 1.5m 降低至 0.3m，效果显著。