一、EKF2概述与核心原理
各位同学好，我是你们的飞控算法讲师。今天咱们来聊聊PX4里最核心的传感器融合模块——EKF2。说实话，我接触飞控这么多年，EKF2是我见过最“聪明”也最“倔强”的算法。聪明在于它能从一堆噪声数据里提取出精准的姿态和位置，倔强在于参数没调好时，它能把飞机带沟里去。
嗯，咱们先搞清楚EKF2在PX4里到底是个什么地位。
1.1 EKF2在PX4中的地位
说白了，EKF2就是PX4飞控的“大脑中枢”。所有传感器数据——IMU、GPS、磁力计、气压计、光流、视觉里程计——最终都要汇聚到EKF2这里，由它统一处理，输出我们需要的姿态、位置、速度信息。
我个人习惯把飞控系统比作一个团队：
- 传感器是“情报员”，各自汇报信息但都有噪声
- EKF2是“指挥官”，综合各路情报做出最优判断
- 控制器是“执行者”，根据指挥官的命令去干活
你看，指挥官要是判断错了，整个团队就乱套了。我在项目中遇到过好几次，飞机起飞后莫名其妙地往一边偏，排查到最后发现是EKF2的磁力计融合参数没调好。所以，理解EKF2的工作原理，是每个飞控工程师的必修课。
1.2 EKF2与LPE/EKF1的区别
很多新手会问：为什么PX4要搞这么多估计器？其实这是历史遗留问题。我刚开始做PX4开发时，用的还是EKF1和LPE。后来EKF2出来，基本就把它们淘汰了。
咱们用表格对比一下：
| 特性 | EKF2 | LPE | EKF1 |
|---|---|---|---|
| 传感器支持 | IMU、GPS、磁力计、气压计、光流、视觉里程计、测距仪 | IMU、GPS、磁力计、气压计、测距仪 | IMU、GPS、磁力计、气压计 |
| 多传感器融合 | 支持任意组合，动态切换 | 固定组合 | 固定组合 |
| 状态向量维度 | 24维（可扩展） | 15维 | 15维 |
| 协方差管理 | 自适应协方差调整 | 固定协方差 | 固定协方差 |
| 故障检测 | 内置传感器故障检测与隔离 | 无 | 有限 |
| 当前状态 | 默认推荐，持续维护 | 已废弃 | 已废弃 |
你看，EKF2在传感器支持、灵活性、故障处理上都强太多了。我记得有一次在室内用光流定位，LPE死活融合不好，换成EKF2后问题迎刃而解。所以现在PX4官方也推荐大家直接用EKF2，别在LPE上浪费时间了。
1.3 状态向量与协方差矩阵
好，咱们进入正题。EKF2到底在估计什么？说白了，它维护了一个“状态向量”，里面包含了所有我们关心的物理量。
EKF2的24维状态向量长这样：
状态向量 x = [位置(3), 速度(3), 姿态四元数(4), 陀螺零偏(3), 加速度计零偏(3),
地磁(3), 风速(3), 气压计偏移(1), 测距仪偏移(1)]嗯，这里要注意，姿态是用四元数表示的，不是欧拉角。为什么？因为四元数没有万向锁问题，而且计算更稳定。我在项目中吃过万向锁的亏，飞机俯仰到90度时，航向突然就乱跳了。从那以后，我对四元数就特别有好感。
除了状态向量，EKF2还维护了一个协方差矩阵P。这个矩阵描述了我们对每个状态估计的不确定度。你想想看：
- 如果GPS信号好，位置协方差就小
- 如果GPS丢了，位置协方差就慢慢变大
- 如果IMU有噪声，速度协方差就会增大
协方差矩阵是EKF2的“自信指数”。它告诉算法：哪些信息可靠，哪些不可靠。算法会根据协方差的大小，自动调整对不同传感器的信任程度。
1.4 预测与更新两步走
EKF2的核心逻辑就两个步骤：预测和更新。说白了就是“先猜后改”。
咱们用一张流程图来展示这个过程：
你看这个流程，其实很直观：
- 预测步骤：用IMU数据（加速度计和陀螺仪）通过运动模型，推算下一时刻的状态。这一步频率很高，通常400-1000Hz。说白了就是“根据上一秒的状态，猜下一秒会变成什么样”。
- 更新步骤：当GPS、磁力计、气压计等传感器有数据时，用这些观测值来修正预测结果。这一步频率较低，比如GPS只有5-20Hz。说白了就是“用实际测量值来纠正猜测的偏差”。
这里有个关键点：预测步骤依赖IMU，更新步骤依赖其他传感器。如果IMU质量差，预测就会漂移；如果其他传感器噪声大，更新就会引入误差。我在项目中调试时，经常需要平衡这两者的权重。
嗯，到这里，EKF2的核心原理就讲完了。总结一下：
- EKF2是PX4的“大脑中枢”，负责融合所有传感器
- 它比LPE和EKF1更强大、更灵活
- 状态向量包含24个变量，协方差矩阵描述不确定度
- 预测用IMU，更新用其他传感器，两步循环往复
下一章咱们会深入讲解EKF2的参数配置，包括怎么调协方差、怎么设置传感器权重。到时候我会分享一些实际项目中的调参案例，保证让你少走弯路。