第三章 PX4姿态估计（EKF2）：传感器融合的核心秘密
说实话，很多飞手调了半年参数，都不知道PX4真正在背后干了什么。你调的是PID，但姿态估计不准，PID调得再好也是白搭。这一章，我们就来扒开EKF2的底裤。
EKF2，全称Extended Kalman Filter 2，是PX4里负责姿态估计的核心模块。它把IMU、GPS、视觉、磁力计这些传感器的数据揉在一起，输出一个最靠谱的姿态和位置估计。我当年第一次接触它的时候，觉得这东西就是个黑盒子。后来踩了不少坑，才慢慢摸清它的脾气。
核心观点：EKF2不是万能的，但它不干活，你的飞机就是瞎子。
3.1 EKF2传感器融合原理
说白了，EKF2干的事就两件：预测和更新。
- 预测：根据IMU的加速度和角速度，推算下一时刻的状态。这一步很快，但会累积误差。
- 更新：用GPS、视觉、磁力计等外部传感器的观测值，修正预测的误差。这一步慢，但准。
你想想看，IMU每秒跑几百次，GPS每秒才来10次。EKF2就是在高频预测和低频修正之间找平衡。我见过有人把IMU频率设得极高，结果预测误差还没等GPS来修正，飞机已经飘了。
3.2 IMU数据处理
IMU是EKF2的“心脏”。没有它，其他传感器都是摆设。但IMU数据天生有噪声和偏差。
我个人习惯，拿到一个新飞控，第一件事就是看IMU的原始数据。你可以在QGroundControl的Mavlink Inspector里看IMU_RAW消息。如果加速度计在静止时有超过0.1m/s²的波动，或者陀螺仪有超过0.1rad/s的漂移，那就要小心了。
小技巧：EKF2内部会对IMU做圆锥补偿和划桨补偿。这两个算法专门处理旋转和振动带来的误差。如果你发现悬停时飞机慢慢往一个方向漂，多半是IMU的偏差没校准好，而不是PID的问题。
IMU数据进入EKF2后，会经过三个步骤：
- 预积分：把高频的IMU数据压缩成相对运动增量。
- 偏差估计：实时估算陀螺仪和加速度计的零偏。
- 时间对齐：把IMU的时间戳和GPS/视觉的时间戳对齐。
我曾经遇到过一个问题：飞机在快速转弯时，姿态估计突然跳变。查了半天，发现是IMU的采样时间戳和GPS的时间戳差了20毫秒。EKF2里有个参数EKF2_IMU_POS_X,Y,Z，就是用来补偿IMU和GPS之间的物理安装位置差异的。很多人忽略了这个，结果融合出来的位置总是滞后。
3.3 视觉/GPS融合
GPS在室外好用，但一到室内或者城市峡谷，信号就完蛋。这时候视觉就顶上来了。
EKF2支持两种视觉融合方式：
- 视觉里程计（VIO）：只提供相对运动，不提供绝对位置。适合室内。
- 视觉定位（Visual SLAM）：提供绝对位置和姿态。适合有地图的场景。
我个人的经验是，在室外用GPS+视觉融合，效果比单独用GPS好得多。尤其是在有风的情况下，GPS会飘，但视觉能稳住。但要注意，视觉的延迟比GPS大。EKF2里有个EKF2_EV_DELAY参数，就是用来补偿视觉延迟的。我一般设成0.1秒，然后根据实际效果微调。
| 传感器 | 优点 | 缺点 | EKF2关键参数 |
|---|---|---|---|
| GPS | 绝对位置，无漂移 | 室内无效，更新率低 | EKF2_GPS_CHECK |
| 视觉（VIO） | 室内可用，更新率高 | 有累积漂移 | EKF2_EV_DELAY |
| 磁力计 | 提供航向参考 | 易受干扰 | EKF2_MAG_TYPE |
注意：视觉和GPS的坐标系必须对齐。EKF2默认使用ENU（东-北-天）坐标系。如果你的视觉输出是NED（北-东-地），记得在参数里设置EKF2_EV_AID为1，并检查EKF2_EV_GPS_YAW_OFF。
3.4 EKF2参数调优（SYS_MC_EST_GROUP）
终于到了调参环节。很多人一上来就改EKF2_*参数，结果越调越乱。我建议你先搞清楚一个东西：SYS_MC_EST_GROUP。
这个参数决定了多旋翼用哪个估计器。默认是1，也就是EKF2。如果你改成2，那就是用LPE（Local Position Estimator）。但LPE已经快被淘汰了，我建议你老老实实用EKF2。
调EKF2，核心是调噪声协方差矩阵。说白了，就是告诉EKF2：你有多相信IMU？你有多相信GPS？
EKF2_GPS_V_NOISE：GPS速度噪声。默认0.5 m/s。如果你在开阔地带，可以降到0.3。如果在城市峡谷，建议升到1.0。EKF2_GPS_P_NOISE：GPS位置噪声。默认1.0 m。这个值越大，EKF2越不相信GPS，越依赖IMU。EKF2_IMU_P_NOISE：IMU位置噪声。默认0.01 m/s²。这个值越小，IMU的权重越大。
我遇到过最典型的问题：飞机在悬停时上下晃动。检查发现，EKF2_GPS_P_NOISE设得太小，导致EKF2过于相信GPS的高度，而GPS的高度本身就有1-2米的跳动。把噪声值调到2.0后，问题立刻解决。
调参口诀：GPS噪声大，飞机稳但反应慢；GPS噪声小，飞机灵但容易抖。找到平衡点，才是高手。
还有一个容易被忽略的参数：EKF2_MAG_TYPE。这个参数控制磁力计的使用方式。默认是0（自动），但如果你在室内或者有强磁场干扰的地方，我建议改成2（仅用于航向初始化），或者干脆改成3（禁用）。
嗯，这里要注意：磁力计在EKF2里主要是用来修正航向的。但如果你有视觉或者双天线GPS，航向可以从那里来，磁力计反而成了干扰源。我有个项目，飞机在高压线附近飞，磁力计被干扰得一塌糊涂。关了磁力计后，航向反而更稳了。
最后，调完参数一定要看EKF2_INNOV这个日志。它告诉你EKF2的“创新”有多大——也就是预测值和观测值之间的差异。如果某个传感器的创新一直很大，说明你给它的噪声参数设错了，或者那个传感器本身有问题。
避坑指南：我曾经在调参时发现GPS的创新一直很大，换了几个GPS模块都没用。最后发现是飞控的GPS接口虚焊了。所以，调参之前，先确认硬件没问题。