4、姿态估计分析：EKF2状态估计器日志解读，判断姿态解算是否发散
姿态估计是飞控的「内功」。飞控能不能稳住，GPS 丢星后能不能撑住，全靠它。EKF2 状态估计器是 PX4 的默认主力，说白了，它就是个不断猜姿态、猜位置、猜速度的滤波器。猜得准，飞机稳如老狗；猜不准，那就是翻车现场。
我见过不少开发者，日志一打开就盯着姿态角看，觉得角度没飘就没事。其实不然。EKF2 发散往往不是突然发生的，它有个慢慢恶化的过程。今天我们就来聊聊，怎么从日志里提前嗅到发散的味道。
4.1 EKF2 日志的核心字段
先搞清楚我们要看什么。PX4 的 EKF2 日志主要分布在 ekf2_innovations、estimator_status、vehicle_attitude 这几个 topic 里。我个人习惯，拿到日志第一件事，先拉 estimator_status 里的几个关键 flag。
| 字段名 | 含义 | 正常范围 |
|---|---|---|
gps_check_fail_flags |
GPS 健康检查失败标志 | 0 |
control_mode_flags |
EKF 各通道启用状态 | 按需置位 |
filter_fault_flags |
滤波器故障标志 | 0 |
time_slip |
时间滑动量 | < 0.01s |
vel_innov / pos_innov |
速度/位置新息 | 接近 0 |
mag_innov |
磁力计新息 | ±0.5 以内 |
这里有个坑——很多人只看 filter_fault_flags 是不是 0。其实 gps_check_fail_flags 如果频繁跳变，说明 GPS 质量在边缘，EKF 会反复切换融合策略，姿态解算反而更容易出问题。
4.2 新息（Innovation）——发散的前兆
新息是什么？就是「测量值」和「预测值」的差。EKF 每来一个传感器数据，都会算一次新息。新息太大，说明滤波器已经跟不上真实状态了。
我一般盯着三个新息看：
- 速度新息（
vel_innov[0~2]）：正常在 ±0.5 m/s 以内。超过 1.0 就要警惕。 - 位置新息（
pos_innov[0~2]）：正常在 ±1.0 m 以内。超过 2.0 基本就是发散前兆。 - 磁力计新息（
mag_innov[0~2]）：这个最敏感。超过 ±0.5 且持续不回落，说明磁场干扰严重或航向估计在漂。
4.3 协方差矩阵——发散的本质
新息是表象，协方差才是本质。EKF 内部维护着一个协方差矩阵 P，它描述了滤波器对自己估计的「信心」。协方差越大，说明滤波器越不确定。
日志里能看到 covariances[0~23]，这是协方差矩阵的上三角元素。我重点关注：
covariances[0]（姿态角协方差）：正常 < 0.01 rad²。超过 0.1 说明姿态估计已经不可靠。covariances[4]（高度协方差）：正常 < 0.1 m²。超过 1.0 说明高度在飘。covariances[12]（陀螺零偏协方差）：这个值如果突然增大，说明滤波器在重新估计零偏，往往是 IMU 温度变化或振动导致的。
为什么会这样？我举个例子。有一次我在户外飞一架大四轴，起飞前一切正常，悬停 30 秒后开始缓慢低头。看日志发现 covariances[0] 从 0.005 一路涨到 0.2，同时磁力计新息从 0.1 涨到 0.8。后来排查发现，是电机线束离磁力计太近，大电流产生了磁场干扰。EKF 试图融合磁力计数据，但数据本身是错的，协方差就越来越大。
4.4 时间滑动（Time Slip）——被忽视的杀手
时间滑动 time_slip 是个冷门字段，但非常关键。它表示 EKF 内部的时间戳和实际时间之间的偏差。正常情况下 < 0.01s。如果超过 0.05s，说明 EKF 的计算跟不上传感器数据的到达速度。
我遇到过一台 Pixhawk 1，刷了最新的固件，跑 VIO + GPS 双融合。日志里 time_slip 经常跳到 0.1s，然后姿态角就开始剧烈震荡。后来发现是 SD 卡写入太慢，导致 EKF 的主循环被阻塞。换了一张高速卡，问题解决。
time_slip 的最大值。如果超过 0.02s，我会检查 CPU 负载和传感器频率。别等到炸机了再查。
4.5 判断发散的综合流程
光看一个指标不够。我总结了一套判断流程，你可以照着来：
- 看 flag：
filter_fault_flags是否为 0？不为 0 直接标红。 - 看新息：三个新息是否持续偏离零线？偏离超过 2 秒就要警惕。
- 看协方差：姿态协方差是否持续增长？增长趋势比绝对值更重要。
- 看时间滑动：
time_slip是否超过 0.02s？超过说明计算资源吃紧。 - 看姿态角：最后再确认姿态角是否出现高频抖动或缓慢漂移。
下面这张图是我自己整理的判断逻辑，你可以保存下来对照着看。
4.6 实战案例：一次典型的发散过程
我记得有一次帮朋友排查一架六轴。日志里 vel_innov[2]（Z 轴速度新息）从 -0.2 慢慢涨到 -1.5，持续了 5 秒。同时 covariances[0] 从 0.008 涨到 0.15。姿态角一开始没变化，但到第 6 秒，俯仰角突然开始 5° 的震荡。
这就是典型的「协方差先涨，新息后偏，姿态最后崩」的过程。如果你等到姿态角都抖了再切模式，往往已经来不及了。
关键结论： 协方差和新息是「前哨指标」，姿态角是「滞后指标」。想提前发现发散，盯紧前两个。
4.7 常见发散原因与对策
| 现象 | 可能原因 | 对策 |
|---|---|---|
| 磁力计新息持续偏大 | 磁场干扰 / 磁力计未校准 | 重新校准磁力计，检查电机线束布局 |
| 速度新息 Z 轴漂移 | 气压计受风影响 / 高度融合权重不对 | 检查气压计进风口，调整 EKF2_HGT_REF |
| 协方差整体增长 | 传感器数据率不足 / CPU 过载 | 降低传感器频率，检查 SD 卡写入速度 |
| time_slip 持续增大 | EKF 主循环被阻塞 | 检查 uORB 消息队列，减少不必要的订阅 |
嗯，这里要注意一点：有时候发散不是 EKF 本身的问题，而是传感器数据本身就有问题。比如 GPS 在楼宇间跳变，气压计被螺旋桨下洗气流干扰。EKF 只是「垃圾进，垃圾出」。所以排查发散问题时，先确认传感器数据质量，再怀疑滤波器参数。
我个人习惯，每次飞完都会把 estimator_status 里的关键字段拉出来画个趋势图。新息和协方差的趋势比绝对值更有价值。如果看到某个协方差在持续增长，哪怕现在还在正常范围，我也会标记为「需关注」。毕竟，等到它真的发散再处理，黄花菜都凉了。