1. EKF初探：什么是EKF？为什么ArduPilot需要它？EKF在飞控中的角色定位
各位同学好，我是你们这门课的老朋友。今天咱们聊聊EKF——扩展卡尔曼滤波器。说实话，我第一次接触这玩意儿的时候，也是一头雾水。什么预测、更新、协方差矩阵……听着就头大。但后来在项目里摔了几次机，才真正明白这东西有多重要。
EKF，说白了，就是一个数学工具。它的任务只有一个：在充满噪声的世界里，帮你猜出最接近真实的状态。你想想看，飞控上的GPS、IMU、磁力计、气压计，哪个传感器没噪声？GPS飘个几米是常事，IMU零偏慢慢跑，磁力计受个干扰就乱跳。如果没有EKF，这些数据你根本没法直接用。
核心一句话：EKF不是传感器，它是传感器的“大脑”——把一堆不靠谱的数据，融合成一个靠谱的估计。
1.1 为什么ArduPilot离不开EKF？
ArduPilot早期用的是DCM（方向余弦矩阵）算法。DCM简单、轻量，但有个致命问题：它只能处理姿态，没法融合GPS和气压计来做位置估计。你想让无人机定点悬停、自主返航？DCM做不到。
我记得2015年左右，ArduPilot团队决定全面转向EKF。当时社区里吵得很凶，有人说EKF太复杂、计算量太大。但后来大家发现，没有EKF，多传感器融合根本玩不转。说白了，EKF就是飞控的“中枢神经”——它把姿态、位置、速度、偏置全部揉在一起，给出一个统一的估计。
具体来说，ArduPilot里的EKF负责以下工作：
- 姿态估计：融合陀螺仪、加速度计、磁力计，输出roll/pitch/yaw
- 位置/速度估计：融合GPS、气压计、光流、视觉里程计
- 传感器偏置估计：实时校准陀螺零偏、加速度计偏置
- 故障检测：当某个传感器数据异常时，自动降低其权重
一个小经验：我刚开始调EKF参数时，总想着把所有传感器都开满。结果发现，传感器越多，EKF越容易“打架”。后来我学会了一个原则——能用少的，就别用多的。比如室内飞行，GPS信号差，干脆把GPS权重调低，靠光流和气压计。
1.2 EKF在飞控中的角色定位
咱们用一张图来理解EKF在整个飞控系统里的位置。这张图我画了很多遍，每次给新同事讲都拿它出来。
从这张图你能看到，EKF处于传感器和控制器之间。它不直接控制电机，但它提供的状态估计，是所有控制决策的基础。没有EKF，控制器就像瞎子摸象——只知道传感器读数，不知道真实状态。
1.3 EKF到底在算什么？
咱们不搞复杂的数学推导，我直接说EKF在ArduPilot里具体算哪些东西。打开ArduPilot的EKF源码（在libraries/AP_NavEKF/目录下），你会发现它维护了一个状态向量，包含以下24个状态量：
| 状态分组 | 具体状态量 | 数量 |
|---|---|---|
| 姿态四元数 | q0, q1, q2, q3 | 4 |
| 速度（NED坐标系） | Vn, Ve, Vd | 3 |
| 位置（NED坐标系） | Pn, Pe, Pd | 3 |
| 陀螺零偏 | gbx, gby, gbz | 3 |
| 加速度计偏置 | abx, aby, abz | 3 |
| 地磁矢量 | mx, my, mz | 3 |
| 磁偏角 | decl | 1 |
| 风速 | Vwx, Vwy | 2 |
| 气压计高度偏置 | baro_hgt_bias | 1 |
| 光流/视觉偏置 | flow_bias_x, flow_bias_y | 2 |
嗯，看到这个表你可能有点晕。别急，咱们不需要记住每个状态量。你只需要知道：EKF把所有可能影响飞行的因素都建模进去了。陀螺会漂移？它帮你估计漂移量。磁力计受干扰？它帮你估计地磁方向。甚至风速它都帮你算——这对固定翼特别有用。
注意：EKF的状态量越多，计算量越大，调参也越复杂。ArduPilot默认开启了所有24个状态，但你可以通过参数关闭某些功能。比如你只用室内飞行，没有GPS，就可以把风速估计关掉，减少计算负担。
1.4 EKF的两步走：预测 & 更新
EKF的核心逻辑其实就两步：预测和更新。我用自己的话给你解释一下。
预测步：EKF根据上一时刻的状态，结合IMU的角速度和加速度，猜出当前时刻的状态。这个过程也叫“时间更新”。说白了，就是“我猜你现在应该在哪”。
更新步：EKF拿到GPS、气压计、磁力计等传感器的实际测量值，跟预测值对比，算出误差，然后用这个误差修正状态。这个过程也叫“测量更新”。说白了，就是“我看看实际数据，修正一下刚才的猜测”。
这两步循环往复，每来一次IMU数据就预测一次，每来一次GPS数据就更新一次。频率上，IMU通常是200-400Hz，GPS只有5-10Hz。所以EKF大部分时间在做预测，偶尔才做更新。
避坑指南：我曾经遇到一个案例，GPS更新频率设置得太低（1Hz），导致EKF长时间只靠IMU预测。结果无人机在风大的时候位置漂移越来越严重，最后直接飞偏了。后来我把GPS更新频率调到5Hz，问题就解决了。所以，传感器更新频率和EKF的预测-更新节奏一定要匹配。
1.5 为什么不用更简单的滤波器？
有人可能会问：既然EKF这么复杂，为什么不用互补滤波或者简单的低通滤波？
互补滤波确实简单，但它只能融合两个传感器，而且没法处理非线性关系。比如你要把GPS的位置（米）和IMU的加速度（米/秒²）融合，这俩单位都不一样，互补滤波搞不定。
低通滤波就更不用说了，它只能平滑单个传感器的噪声，没法做多传感器融合。
EKF的优势在于：
- 多传感器融合：可以同时融合GPS、IMU、磁力计、气压计、光流、视觉等
- 状态估计：不仅能估计姿态位置，还能估计传感器偏置、风速等隐变量
- 不确定性量化：EKF会输出每个状态的协方差，告诉你“这个估计有多可信”
- 故障检测：通过协方差和残差，可以判断传感器是否异常
当然，EKF也有缺点。它假设系统是高斯分布的，而且对非线性系统的近似（泰勒展开）有时会失效。但总的来说，在飞控领域，EKF是目前最成熟、最可靠的选择。
1.6 本章小结
咱们这一章把EKF的定位讲清楚了。总结一下：
- EKF是飞控的“大脑”，负责把多个传感器的数据融合成统一的状态估计
- ArduPilot的EKF维护24个状态量，涵盖姿态、位置、速度、偏置等
- EKF的核心是“预测-更新”两步循环，IMU驱动预测，其他传感器驱动更新
- 相比互补滤波和低通滤波，EKF能处理多传感器融合和非线性问题