3. ArduPilot导航架构：导航滤波器（EKF）、位置/速度/姿态估计、传感器融合
这一章，我们来聊聊ArduPilot的“大脑”——导航架构。
说白了，飞控要解决的核心问题就一个：我现在在哪？飞得怎么样？ 然后才能决定下一步怎么走。GPS能告诉你位置，但更新慢，还容易丢星；IMU（惯性测量单元）反应快，但时间长了会漂。怎么办？把两者结合起来，取长补短。这就是传感器融合，而EKF（扩展卡尔曼滤波器）就是干这个活的。
3.1 为什么需要EKF？
我刚开始接触飞控时，觉得直接用GPS数据不就完了？后来在项目里遇到一次强风，飞机被吹得偏航，GPS显示位置还在飘，但姿态已经乱套了。那次之后我彻底明白了：单一传感器靠不住。
EKF的作用，就是像一个“数据裁判”：
- 它相信IMU的短期变化（比如瞬间的角速度、加速度）
- 它相信GPS的长期位置（但会过滤掉噪声）
- 它还会用空速、地速、磁力计等数据互相校验
你想想看，如果只用GPS，飞机在翻滚时GPS根本来不及反应。EKF通过融合IMU，能实时估算出姿态，哪怕GPS短暂失效，也能维持几秒钟的稳定控制。
3.2 ArduPilot中的EKF实现
ArduPilot目前主推的是 EKF3（扩展卡尔曼滤波器第三代）。它比前代更稳定，对传感器故障的容错性更好。
EKF3内部维护了一个状态向量，包含：
- 位置（北、东、地）
- 速度（北、东、地）
- 姿态（四元数，表示机头朝向）
- 陀螺仪零偏（IMU的漂移误差）
- 加速度计零偏
- 地磁干扰（如果使用磁力计）
- 风速（没错，EKF还能估算风速！）
嗯，这里要注意：风速估算是固定翼飞行的关键。EKF通过对比空速（皮托管测得）和地速（GPS测得），反推出风速矢量。我在调参时发现，如果空速管堵了，EKF估算的风速会严重偏离，导致航向控制异常。
3.3 传感器融合流程
EKF的工作流程，可以简单理解为“预测-更新”循环：
- 预测步：根据IMU的角速度和加速度，推算下一时刻的位置、速度、姿态。这一步很快，频率高达400Hz。
- 更新步：当GPS、空速、磁力计等数据到来时（通常5-10Hz），用这些观测值修正预测结果。
我习惯把EKF比作一个“近视眼开车”：
- IMU是手感，你闭着眼也能感觉车在加速还是转弯（预测）
- GPS是偶尔看一眼路标，确认自己没跑偏（更新）
如果路标（GPS）突然没了，你还能靠手感开一段，但时间长了肯定偏。EKF也一样，GPS丢失后，它会逐渐切换到“纯惯性”模式，位置误差会随时间累积。
3.4 关键参数与调优
EKF虽然智能，但也不是万能的。有几个参数我建议你重点关注：
| 参数名 | 作用 | 我的建议 |
|---|---|---|
| EKF3_IMU_MASK | 选择使用哪个IMU（飞控通常有2-3个IMU） | 默认用主IMU，如果主IMU故障会自动切换 |
| EKF3_GPS_TYPE | GPS融合模式（0=关闭，1=使用，2=仅用于高度） | 固定翼建议设为1，除非你在室内飞行 |
| EKF3_MAG_CAL | 磁力计校准模式 | 我习惯用“飞行中校准”，但前提是飞机做过完整的磁力计旋转校准 |
| EKF3_GSF_RUN | 地面速度滤波器（用于无GPS时辅助） | 建议开启，尤其在城市峡谷等GPS易遮挡的环境 |
调参时，我建议先看日志里的 EKF3.velInnov 和 EKF3.posInnov 这两个字段。它们表示“新息”——也就是GPS观测值与EKF预测值的差异。如果这个值长期偏大（比如超过5米），说明EKF的预测模型或传感器数据有问题。
3.5 知识体系图
下面这张图，帮你理清EKF在导航架构中的位置：
从图中可以看到，EKF处于传感器和控制器之间。它不直接输出控制指令，而是输出干净、连续、可信的状态估计。后面的L1导航控制器和TECS（总能量控制系统）再根据这些估计去计算舵面指令。
3.6 实际项目中的经验
最后分享一个我踩过的坑。有一次在海上飞行，磁力计受到船体磁场干扰，EKF的姿态估计开始漂移。飞机在直线飞行时，机翼却慢慢倾斜，最后进入螺旋。事后分析日志发现，EKF的磁力计新息（magInnov）在几分钟内从0.1逐渐增大到0.8，但因为没有触发故障检测，EKF一直“相信”了错误的磁力计数据。
从那以后，我养成了一个习惯：在每次飞行前，检查EKF的故障标志位。在Mission Planner里，可以查看 EKF3.health 参数，如果小于1，说明EKF认为某些传感器不可靠。这时候别起飞，先排查问题。
EKF3_FAILURE_MODE=1，当EKF检测到严重故障时，强制切换到手动模式，让飞行员接管。
好了，这一章的内容就到这里。EKF是飞控的基石，理解它的工作原理，能帮你更好地诊断飞行中的异常。下一章我们会深入L1导航控制器，看看EKF输出的状态是如何被用来跟踪航线的。