PX4源码深读(二):EKF2的24维状态和它吞掉的那些传感器

上一篇我画了从传感器到电机的完整数据流。数据流过了一遍，模块位置都清楚了，但有一个模块我只是点了下名——EKF2。

这篇专门拆它。

不是因为它最复杂（虽然确实最复杂），而是因为PX4里所有"智能"都集中在这一个模块。GPS定位靠它，气压计定高靠它，视觉定位靠它，地磁定向靠它。它错了，后面全错。而它要同时吃六个以上不同频率、不同精度的传感器，在毫秒级的时间里给出一个自洽的24维状态估计。

读懂EKF2，不是为了调参数，是为了知道它什么时候在骗你。

EKF2不是卡尔曼滤波

先纠正一个常见误解：EKF2不是"一个扩展卡尔曼滤波器"。

它是一个多观测源的扩展卡尔曼滤波框架。区别在于：经典EKF有一个预测步、一个更新步，测量矩阵H是固定的。EKF2有24个状态变量，但观测模型有七套——GPS一套、气压计一套、地磁计一套、光流一套、视觉定位一套、测距仪一套、空速一套。每套有自己的H矩阵和噪声模型，独立触发更新。

这意味着EKF2的每个主循环可能是这样的：

预测（IMU驱动，250Hz固定）
  → GPS更新（5-10Hz，有新数据时触发）
  → 气压计更新（50Hz，有新数据时触发）
  → 地磁计更新（100Hz，有新数据时触发）
  → 光流更新（可能有，可能没有）
  → 视觉定位更新（可能有，可能没有）

预测步永远跑，更新步按传感器到达频率各自独立触发。这就是为什么EKF2能同时处理频率差两个数量级的传感器——它不需要等所有传感器到齐才计算，每个传感器到了就更新一次。

这个设计的代价是：更新顺序影响结果。GPS先到和气压计先到，最终状态会差一点点。差异在噪声量级内，不会发散，但严格说结果不唯一。这是所有多观测源EKF的通病，不是PX4的bug。

24维状态向量

EKF2的状态向量是24维。不看代码的人以为是4维（四元数），实际上远不止：

状态索引    变量                  维度    来源
0-3        四元数姿态 q          4      IMU预测
4-6        速度 vn,ve,vd         3      IMU预测 + GPS/光流/视觉更新
7-9        位置 pn,pe,pd         3      IMU预测 + GPS/视觉更新
10-12      陀螺仪偏差 δgx,δgy,δgz  3    在线估计
13-15      加速度计偏差 δax,δay,δaz  3   在线估计
16-18      地磁场 NED分量        3      地磁计更新
19-21      地磁场 body分量       3      地磁计更新
22         风速 north            1      GPS更新（多旋翼/固定翼）
23         风速 east             1      GPS更新

四元数4维 + 速度3维 + 位置3维 = 10维是核心运动状态。剩下14维全是偏差和外部量估计。

为什么要估计传感器偏差？因为MEMS IMU的零偏会随温度漂移。上电时陀螺仪偏差可能是0.01 rad/s，飞十分钟后变成0.03 rad/s。如果不在线估计偏差，累积误差会让姿态在几分钟内发散。

加速度计偏差更棘手——它和重力方向耦合。悬停时加速度计测量的是重力反方向（9.8 m/s²向上），偏差0.1 m/s²看起来很小，但被积分两次后就是位置漂移：0.5 × 0.1 × t²，10秒偏差5米。这就是为什么多旋翼纯惯导悬停漂移严重，不是算法差，是加速度计偏差在自由落体。

地磁场8维看起来浪费——为什么NED和body各3维，总共6维？因为EKF2同时估计地磁场在NED坐标的参考值和body坐标的偏差。前者是地磁场本身（缓变），后者是机载磁干扰（随机体动作变化）。分开估计才能在有磁干扰时还能用磁力计做航向修正——它知道哪些是地磁、哪些是干扰。

风速2维只在固定翼和多旋翼返航时有用。多旋翼的风速估计本质是"GPS速度减去期望悬停速度"，精度不高但够用。

预测步：IMU驱动的24维传播

预测步是EKF2的心跳，250Hz，雷打不动。

// src/modules/ekf2/EKF/ekf.cpp
voidEkf::predictIMU(float dt)
{
// 1. 用陀螺仪积分姿态
const Vector3f delta_angle(gyro - _state.gyro_bias);
const Quaternionf dq = _state.quat * Quaternionf(delta_angle);
    _state.quat = dq;
    _state.quat.normalize();

// 2. 用加速度计积分速度和位置
const Vector3f delta_vel(acc - _state.accel_bias);
const Vector3f delta_vel_NED = _state.quat.rotate(delta_vel);
    _state.vel += delta_vel_NED * dt + Vector3f(0, 0, GRAVITY) * dt;
    _state.pos += _state.vel * dt;

// 3. 协方差传播
// F是24×24的状态转移雅可比矩阵
// P = F * P * F^T + Q
    _P = _F * _P * _F.T() + _Q;
}

三件事：状态传播、误差传播、协方差传播。

状态传播是直觉能理解的——陀螺积分姿态，加速度积分速度再积分位置。但注意那个_state.quat * Quaternionf(delta_angle)——四元数乘法不是向量加法，delta_angle要先转成增量四元数再和当前姿态相乘。这一步的线性化误差就是EKF中"扩展"的代价。

协方差传播是整个EKF最昂贵的计算。P是24×24的对称矩阵，576个元素。一次传播需要两次24×24矩阵乘法和一次加法，在STM32H7上大约耗时200μs。这也是为什么EKF2的CPU占用经常排在第一——不是状态传播慢，是协方差传播慢。

Q矩阵（过程噪声）是调参的核心。陀螺仪的过程噪声设大了，EKF会更相信地磁计和GPS；设小了，会更相信IMU。PX4默认值是基于典型MEMS IMU标定的，换了一个更好的IMU（比如战术级），不改Q就浪费了精度；换了一个更差的，不改Q就会发散。

更新步：六套观测模型，六个入口

每个传感器有自己的更新函数。以GPS为例：

voidEkf::updateGps()
{
// 1. 计算创新（innovation）：测量值减预测值
    Vector3f innov_pos;
    innov_pos(0) = _gps.pos - _state.pos(0);
    innov_pos(1) = _gps.pos - _state.pos(1);
    innov_pos(2) = _gps.pos - _state.pos(2);

// 2. 计算创新协方差
// S = H * P * H^T + R
// H是观测矩阵，R是测量噪声
    Matrix3f S = _H_pos * _P * _H_pos.T() + _R_gps;

// 3. 创新一致性检验（卡方检验）
// 如果创新太大，说明预测和测量矛盾，拒绝本次更新
float innov_gate = 5.0f; // 5σ门限
if (innov_pos.norm() > innov_gate * sqrtf(S.trace())) {
return; // 拒绝
    }

// 4. 计算卡尔曼增益
// K = P * H^T * S^{-1}
    Matrix24x3f K = _P * _H_pos.T() * S.inverse();

// 5. 状态更新
// x = x + K * innovation
    _state.vec += K * innov_pos;

// 6. 协方差更新
// P = (I - K * H) * P
    _P = (_I - K * _H_pos) * _P;
}

六个步骤，每套观测模型都走同样的流程，只有H矩阵和R矩阵不同。

创新一致性检验是EKF2鲁棒性的关键。正常情况下，创新（测量减预测）应该服从零均值高斯分布，方差由S决定。如果创新远超预期（超过5σ），说明要么测量坏了，要么模型错了——不管哪种，都应该拒绝这次更新而不是强行融合。这就是为什么GPS被遮挡时PX4不会立刻飞走——大的创新被拒绝了，EKF退回到纯惯导预测。

但这个机制有盲区：如果偏差缓慢积累，创新不会突然变大，而是持续偏移。卡方检验抓不住这种"温水煮青蛙"式的故障。这也是为什么长航时飞行在GPS信号边缘时容易出现静默漂移——不是EKF坏了，是它以为自己还好。

地磁计更新：最难搞的那个传感器

地磁计在EKF2里的处理比其他传感器复杂得多，因为它有三个独有的问题：

1. 磁干扰无处不在

电机电流、电调、电池线、碳纤维螺丝——飞机上一切带电流或铁磁性的东西都会干扰磁力计。干扰随油门变化，随姿态变化，基本不可能标定掉。

2. 地磁场倾角

在杭州（北纬30°），地磁场和水平面夹角约45°。这意味着水平分量只有总强度的70%。如果你只想用磁力计做航向（yaw），只有水平分量有用，剩下30%是噪声放大器。

3. 双参考系估计

这就是那8维地磁状态的来源。EKF2同时估计NED参考地磁和body偏差，用两者之差来分离真正的地磁场和机上干扰：

// 磁力计预测模型
// h_pred = R_to_body * mag_NED + mag_bias_body
Vector3f Ekf::predictMag()
{
const Vector3f mag_pred = _state.quat.rotate_inverse(_state.mag_NED)
                            + _state.mag_bias;
return mag_pred;
}

_state.mag_NED是地磁场在地球坐标的向量（应该缓变），_state.mag_bias是机载干扰（随姿态变化）。通过持续观测差值，EKF2能在线分离两者。

但在强干扰环境下（比如靠近高压线），这个分离会崩溃——干扰太大，模型假设不成立。PX4的处理方式是：如果地磁创新连续被拒绝，自动降低地磁权重，最终完全放弃地磁修正，退回GPS航向。这就是为什么有些飞机在城区飞着飞着yaw突然跳了一下——EKF2刚完成了从磁力计航向到GPS航向的切换。

多IMU融合：sensor模块的投票制

EKF2不直接订阅原始IMU数据。中间隔了一个sensor模块，负责多IMU投票和优先级选择。

// src/modules/sensors/sensors.cpp
voidSensors::Run()
{
// 如果有多个陀螺仪，取优先级最高的
// 如果最高优先级数据超时，降级到下一个
int best_gyro = selectBestGyro();

// 融合策略：不是加权平均，是选优
    _combined.gyro_rad[0] = _gyro[best_gyro].x;
    _combined.gyro_rad[1] = _gyro[best_gyro].y;
    _combined.gyro_rad[2] = _gyro[best_gyro].z;
}

选优而不是加权平均，这个设计是有意为之。两个IMU如果精度差一个量级（比如一个内部陀螺、一个外部MARG），加权平均反而会被差的拖累。选优保证了EKF2吃到的始终是最好的数据，代价是切换时有一个跳变。

这个跳变在EKF2里没有专门处理。如果你在飞的过程中一个IMU挂了，sensor模块切到备用IMU，EKF2会看到一个时间不连续的角速度跳变。短时间表现为姿态估计抖一下，长时间如果备用IMU偏差不同，陀螺仪偏差估计需要重新收敛。

这也是我之前做时间同步方案时关注的问题：多IMU系统里，时间戳对齐比数据融合更重要。两个800Hz的IMU，如果采样时刻差1ms，融合出来的角速度就有1ms的时序错位，积分后就是姿态误差。

协方差矩阵P的秘密

P矩阵是EKF2最占内存也最耗算力的数据结构。24×24 = 576个float，2.25KB。在STM32F4上这不是小数目——RAM总共才192KB，P矩阵吃掉1%。

但P矩阵也是EKF2最被低估的工具。很多人只关注状态向量，不看P的对角线。实际上P的对角线就是每个状态的不确定度。

• P(7,7) = 北向位置的不确定度（方差）。如果GPS丢了5秒，这个值会指数增长。当它超过阈值时，Commander会触发位置失效保护。
• P(10,10) = 陀螺仪X轴偏差的不确定度。刚上电时很大（偏差未知），随着观测累积逐渐减小。如果飞机一直在地面不动，偏差不确定度收敛到最小——这就是为什么PX4建议上电后静置几秒。
• P(0,0) = 姿态四元数w分量的不确定度。如果地磁和GPS航向都不可用，这个值会持续增长，直到触发yaw失效保护。

读懂P的对角线，就知道EKF2"有多自信"。一个不自信的EKF不一定错，但一个过于自信的EKF一定危险。

PX4有个参数EKF2_POS_GATE控制位置创新门限（默认5σ），另一个EKF2_VEL_GATE控制速度创新门限。门限越大，EKF2越"宽容"，越不容易拒绝测量；门限越小，越"多疑"，越容易退回惯导。调这两个参数本质上在调EKF2的性格——信任传感器还是信任自己。

EKF2的运行时序：不是你想的那样

很多人以为EKF2是"先预测、再更新、然后输出"的线性流程。实际上EKF2的主循环是这样的：

voidEkf::update()
{
// 1. IMU预测（每次都执行）
    predictIMU(_dt_imu);

// 2. 按优先级尝试各传感器更新
// 每个更新函数内部检查是否有新数据
if (_gps_data_ready)    updateGps();
if (_baro_data_ready)   updateBaro();
if (_mag_data_ready)    updateMag();
if (_flow_data_ready)   updateFlow();
if (_vision_data_ready) updateVision();
if (_range_data_ready)  updateRange();

// 3. 更新各种衍生状态
// 这些不是EKF状态，是从EKF状态计算的
    computeAirspeed();
    computeWind();
    computeOffset();

// 4. 发布结果
    publishAttitude();
    publishLocalPosition();
    publishGlobalPosition();
}

关键点：更新顺序是固定的（GPS → 气压 → 磁力计 → 光流 → 视觉 → 测距），不是按数据到达时间排序的。这意味着即使磁力计数据比GPS先到，也会等GPS更新完再处理磁力计。

这个固定顺序是有意设计——GPS的位置更新会影响速度估计，而速度估计影响后续所有更新的创新计算。如果先更新磁力计再更新GPS，磁力计用的速度预测是旧的，创新偏大，可能被误拒绝。所以GPS优先级最高，确保速度估计最新。

但这也意味着低优先级传感器会被"拖慢"。如果你的飞机只有光流定位（没有GPS），光流的更新频率会受限于气压计和磁力计的处理时间。实际上这个影响很小（微秒级），但在高负载情况下可能体现出来。

一个真实的调试场景

我之前遇到过一个案例：多旋翼在室内悬停，EKF2频繁报"yaw fusion timeout"告警，然后自动切换到GPS航向——但室内没有GPS，于是yaw直接飘。

查代码后发现原因链：

1. 室内磁干扰大 → 磁力计创新频繁超门限 → 磁力计更新被拒绝
2. 连续N次拒绝后，EKF2判定磁力计不可靠 → 降低磁力计权重
3. 权重降为零后，yaw没有观测源 → 不确定度P(3,3)持续增长
4. P(3,3)超过阈值 → 触发"yaw fusion timeout"告警
5. EKF2尝试切换到GPS航向 → 室内无GPS → 切换失败
6. yaw纯惯导漂移 → 飞机原地打转

解决方案不是调参数（门限调大了磁干扰会更严重），是在室内用外部视觉定位（MOCAP/VIO）提供yaw观测。EKF2本身已经支持——updateVision()里同时处理位置和姿态观测，只要视觉系统提供yaw，地磁计就可以完全关掉。

这个案例说明：EKF2的告警不是问题本身，是症状。 "yaw fusion timeout"不是EKF2坏了，是它在告诉你：我没有可用的yaw观测源了。读懂这个信号，才知道该从外部补什么。

读EKF2代码的建议

如果真要啃EKF2源码，我的建议是：

不要从ekf.cpp开始读。 那个文件5000多行，入口在中间，逻辑跳来跳去，第一次读一定迷路。

先读这三个文件：

1. ekf_derive.cpp — 所有预测步的雅可比矩阵推导。读懂这个文件，就知道F矩阵每个元素是怎么来的。EKF2的F矩阵不是数值差分算的，是手推的解析雅可比——精度高，但代码可读性差。
2. mag_control.cpp — 地磁计融合的完整逻辑。这个文件展示了EKF2处理复杂传感器的方法：多假设、在线标定、失效检测。读懂这个，其他传感器的更新函数都是简化版。
3. geo.cpp — 坐标转换工具集。EKF2内部同时使用NED、body、FRD三种坐标系，这个文件是翻译字典。

读懂这三个文件后，再回头读ekf.cpp，每个函数的上下文就清楚了。

小结

EKF2不是一个滤波器，是一个多观测源的融合框架。它的核心设计决策是：

• 预测步固定频率、更新步按需触发——解决多速率传感器融合
• 创新一致性检验——自动拒绝坏数据，但也抓不住慢漂移
• 双参考系地磁估计——在干扰中分离地磁和干扰，但强干扰下会崩溃
• 选优而非加权——多IMU选最好的用，切换时有跳变
• P矩阵对角线是不确定度的实时光度——读懂它就知道EKF2的自知之明

最后一句话：EKF2的代码不漂亮，变量名很长，注释很少，到处是魔数。但它运行在几十万架飞机上，每天处理几十亿个传感器采样。可靠比漂亮重要。

代码基于PX4 v1.14，EKF2模块路径：src/modules/ekf2/EKF/