4. EKF状态估计器调优：EKF3核心算法、协方差矩阵管理、GPS/IMU/磁力计融合策略、动态噪声调节、常见发散问题排查
EKF3，说白了就是ArduPilot的“大脑”。它把GPS、IMU、磁力计这些传感器的数据揉在一起，算出一个最靠谱的姿态和位置。我刚开始接触EKF3时，觉得它就是个黑盒子。后来调多了才发现，里面的门道其实挺多的。
这一章，咱们就把它拆开看看。我会把核心算法、协方差管理、传感器融合这些硬骨头，用我实际踩过的坑串起来讲。你跟着走一遍，以后遇到EKF发散就不会慌了。
核心观点：EKF3调优不是玄学。你理解了协方差矩阵怎么变、噪声参数怎么调，发散问题就能定位到具体环节。
4.1 EKF3核心算法：状态预测与观测更新
EKF3的本质，就是两步走：先预测，再修正。
- 预测步：用IMU的加速度和角速度，推算下一时刻的状态。这一步会引入不确定性，协方差矩阵会变大。
- 更新步：用GPS、磁力计、空速计等外部观测，修正预测结果。协方差矩阵会变小。
我见过不少开发者，一上来就调噪声参数。其实你首先得搞清楚，发散是发生在预测步还是更新步。怎么判断？看log里的EKF3.innov和EKF3.var字段。
我的习惯：调EKF3之前，先飞一组简单的悬停和直线航线。把log拉下来，看EKF3.innov的均值。如果某个轴的innovation长期偏离零，那对应的传感器大概率有问题。
4.2 协方差矩阵管理：EKF3的“信心指数”
协方差矩阵，你可以把它理解成EKF3对当前状态估计的“信心指数”。矩阵里的值越大，说明EKF3越不确定；值越小，说明它越自信。
EKF3里，协方差矩阵是动态调整的。它受两个因素影响：
- 过程噪声（Q矩阵）：IMU的噪声水平。你设的
EKF3_IMU_NOISE参数，直接影响Q矩阵。 - 观测噪声（R矩阵）：GPS、磁力计等传感器的噪声水平。比如
EKF3_GPS_NOISE。
我在项目里遇到过一个问题：飞机悬停时高度漂移。查log发现EKF3.var(3)（高度方差）一直在涨。后来发现是气压计噪声参数设得太小了，EKF3过于相信气压计，结果被风扰动带偏了。
避坑指南：协方差矩阵不是越小越好。你把它压得太小，EKF3就“听不进”新观测了。这叫滤波器“锁死”。我曾经在一条船上试过，GPS信号短暂丢失后，EKF3死活不重新收敛，就是因为协方差矩阵被压得太死。
4.3 GPS/IMU/磁力计融合策略
EKF3的融合策略，说白了就是“谁靠谱听谁的”。但不同传感器的靠谱程度，在不同场景下是变的。
| 传感器 | 强项 | 弱项 | EKF3融合策略 |
|---|---|---|---|
| IMU | 短时精度高，更新快 | 长期漂移 | 预测步主力，更新步辅助 |
| GPS | 绝对位置无漂移 | 更新慢，易受遮挡 | 水平位置主力，速度辅助 |
| 磁力计 | 提供绝对航向 | 易受干扰，需要校准 | 航向观测，融合时自动降权 |
EKF3里有个关键参数：EKF3_MAG_CAL。它控制磁力计融合的时机。我建议你设成2（自动学习），让EKF3自己判断磁力计是否可信。
为什么会这样？因为磁力计在电机大电流时，干扰特别大。你硬让它融合，航向反而会跳。EKF3的自动学习机制，会实时估计磁力计的噪声水平，干扰大时自动降低融合权重。
4.4 动态噪声调节：EKF3的“自适应”能力
EKF3有个很实用的功能：动态噪声调节。它可以根据当前飞行状态，自动调整过程噪声和观测噪声。
比如，飞机在急转弯时，IMU的加速度噪声会变大。EKF3检测到加速度计方差升高，会自动增大过程噪声，让滤波器更依赖GPS观测。反过来，GPS信号差时，它会增大GPS观测噪声，让滤波器更依赖IMU。
这个功能由EKF3_NOISE_MASK参数控制。我个人的建议是：
- 多旋翼：开启所有动态调节项（bitmask全1）。因为多旋翼机动性强，传感器噪声变化大。
- 固定翼：可以关闭加速度计动态调节（bitmask去掉第0位）。因为固定翼飞行相对平稳，加速度计噪声变化不大。
我的经验：动态噪声调节不是万能的。它只能应对传感器噪声的缓慢变化。如果GPS突然完全丢失，或者磁力计被强磁干扰，动态调节也救不了。这时候要靠EKF3的“健康监测”机制，自动切换到无GPS模式。
4.5 常见发散问题排查
EKF3发散，是飞控调试里最让人头疼的问题之一。我总结了一个排查流程，你按这个顺序走，基本能定位到根因。
- 检查传感器原始数据：先看IMU、GPS、磁力计的原始log。如果原始数据就有问题，EKF3发散是必然的。
- 看innovation序列：如果某个轴的innovation长期不归零，说明对应的观测模型有问题。比如GPS innovation大，可能是GPS天线安装位置不对，或者杆臂效应没补偿。
- 检查协方差矩阵：如果某个状态的方差持续增大，说明EKF3对这个状态越来越没信心。常见原因是过程噪声设得太小，或者观测更新频率太低。
- 检查时间同步：EKF3要求所有传感器的时间戳对齐。如果GPS和IMU的时间戳偏差超过10ms，发散是迟早的事。
我曾经遇到一个案例：飞机在转弯时EKF3发散。查了三天log，最后发现是GPS的更新频率设成了5Hz，而IMU是200Hz。EKF3在两次GPS更新之间，全靠IMU积分，协方差矩阵涨得太快，一转弯就发散。把GPS更新频率提到10Hz后，问题解决。
注意：EKF3发散时，不要盲目调参数。先看log，定位到具体环节。调参数只是最后一步。
4.6 EKF3调优参数速查表
下面这张表，是我平时调EKF3最常用的参数。你把它存下来，调试时对照着看。
| 参数名 | 默认值 | 推荐范围 | 说明 |
|---|---|---|---|
EKF3_IMU_NOISE |
0.01 | 0.005 - 0.02 | IMU过程噪声。越大，EKF3越依赖外部观测 |
EKF3_GPS_NOISE |
0.5 | 0.3 - 1.0 | GPS位置观测噪声。越大，EKF3越不信任GPS |
EKF3_MAG_NOISE |
0.05 | 0.02 - 0.1 | 磁力计观测噪声。干扰大时适当调大 |
EKF3_NOISE_MASK |
0 | 0 - 31 | 动态噪声调节掩码。多旋翼建议全开 |
EKF3_MAG_CAL |
2 | 0 - 3 | 磁力计融合模式。建议设成2（自动学习） |
4.7 EKF3核心流程框架图
下面这张图，把EKF3的预测-更新循环、协方差管理、传感器融合关系画清楚了。你对照着看，能更直观地理解整个流程。
这张图把EKF3的核心循环画得很清楚。你注意看协方差矩阵P的位置——它连接了预测步和观测更新步，是整个滤波器的“中枢神经”。
4.8 小结
EKF3调优，说白了就是管理好“信心”和“观测”之间的平衡。协方差矩阵是信心的量化，动态噪声调节是信心的自适应，传感器融合是信心的分配。你把这三点吃透了，发散问题就能迎刃而解。