第四章 扩展卡尔曼滤波：EKF数学基础、状态预测与更新、协方差矩阵调参、EKF在eVTOL中的应用
各位同学，欢迎来到第四章。
说实话，卡尔曼滤波这个名字，搞飞控的没人不知道。但真正把它用明白，尤其是在eVTOL这种强非线性系统里用好，那又是另一回事了。我记得我刚入行那会儿，照着教科书抄了一个标准KF，结果一跑仿真，直接发散。后来才明白——线性假设在旋翼机这种大机动场景下，根本撑不住。
所以这一章，咱们就死磕扩展卡尔曼滤波，也就是EKF。我会把数学基础、预测更新流程、协方差调参这些硬骨头，掰开了揉碎了讲。最后再结合eVTOL的实际场景，看看这东西到底怎么落地。
4.1 为什么标准卡尔曼滤波不够用？
标准KF的核心假设是：系统是线性的，噪声是高斯白噪声。但eVTOL的动力学模型，说白了就是一堆非线性微分方程。你想想看，空气动力学、电机响应、机体姿态变化，哪个是线性的？
我曾在一次试飞中遇到过这样的情况：用标准KF做姿态估计，悬停时精度还行，一旦进入前飞加速模式，估计值就开始剧烈抖动。后来排查发现，是状态转移矩阵的线性近似在高速工况下完全失效了。
EKF的思路其实很朴素：既然系统是非线性的，那就在每个时间步，对非线性函数做一阶泰勒展开，用雅可比矩阵来近似线性化。说白了，就是用切线去拟合曲线。
核心要点：EKF不是对非线性系统的精确解，而是在每个时刻用线性化近似去逼近真实状态。这种近似在弱非线性场景下效果很好，但强非线性时要注意。
4.2 EKF的数学基础
咱们先看两个核心方程。一个是状态方程，一个是观测方程。
状态方程：x_k = f(x_{k-1}, u_k) + w_k
观测方程：z_k = h(x_k) + v_k其中f和h都是非线性函数。w_k是过程噪声，v_k是观测噪声。两者都假设为零均值高斯分布。
EKF的核心步骤分两步：预测和更新。
4.2.1 预测步骤
预测就是根据上一时刻的状态，推算当前时刻的先验估计。公式如下：
x_pred = f(x_est, u)
P_pred = F * P_est * F^T + Q这里的F就是f对状态x的雅可比矩阵。Q是过程噪声协方差矩阵。
嗯，这里要注意：雅可比矩阵的计算一定要准确。我见过不少新手直接用手算导数，结果符号搞反了，导致滤波器发散。我个人的习惯是用符号计算工具（比如SymPy）先推导一遍，再手动验证。
4.2.2 更新步骤
更新步骤就是用观测值去修正预测值。公式如下：
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_est = x_pred + K * (z - h(x_pred))
P_est = (I - K * H) * P_predH是观测函数h对状态x的雅可比矩阵。R是观测噪声协方差矩阵。K就是卡尔曼增益，它决定了我们更相信预测还是更相信观测。
个人经验：卡尔曼增益K的物理意义很直观。如果观测噪声很大（R大），K就小，滤波器更依赖预测。如果过程噪声很大（Q大），K就大，滤波器更依赖观测。调参的本质就是在Q和R之间找平衡。
4.3 协方差矩阵调参：避坑指南
协方差矩阵调参，是EKF落地中最让人头疼的部分。没有之一。
我曾经在一个项目中，花了两周时间调Q和R矩阵。一开始滤波器总是发散，后来发现是Q矩阵设置得太小，导致滤波器对模型过于自信，观测更新跟不上实际变化。
调参的几个基本原则，我总结如下：
| 参数 | 物理含义 | 调大影响 | 调小影响 |
|---|---|---|---|
| Q（过程噪声） | 模型不确定性 | 滤波器响应快，但易受噪声干扰 | 滤波器平滑，但可能跟不上真实变化 |
| R（观测噪声） | 传感器不确定性 | 滤波器更依赖预测，响应变慢 | 滤波器更依赖观测，易受传感器噪声影响 |
| P（估计误差） | 当前状态的不确定性 | 初始收敛快，但可能震荡 | 初始收敛慢，但稳态精度好 |
避坑指南：我曾经犯过一个低级错误——把Q和R设成对角矩阵后就不管了。实际上，如果状态变量之间存在耦合（比如位置和速度），非对角元素也需要考虑。否则滤波器会丢失相关性信息，导致估计精度下降。
调参的实用技巧：
- 先调R。用传感器静态数据估算噪声方差，这个相对容易。
- 再调Q。从较小的值开始，逐步增大，观察滤波器的响应速度。
- 最后微调P的初始值。一般设为单位矩阵乘以一个较大的数，保证初始收敛速度。
4.4 EKF在eVTOL中的应用
好了，理论讲完了，咱们看看EKF在eVTOL上到底怎么用。
eVTOL的飞控系统，通常需要融合多种传感器：IMU、GPS、气压计、磁力计，甚至视觉或激光雷达。EKF的作用就是把所有这些信息融合起来，得到一致的状态估计。
4.4.1 典型的状态向量
对于eVTOL，我常用的状态向量包含：
x = [位置(3), 速度(3), 姿态四元数(4), 陀螺零偏(3), 加速度计零偏(3)]一共16维。为什么用四元数？因为欧拉角在俯仰±90度时会有奇异性，而eVTOL的机动范围很大，四元数更安全。
4.4.2 预测模型
预测模型就是eVTOL的动力学方程。简化版如下：
位置更新：p_k = p_{k-1} + v_{k-1} * dt
速度更新：v_k = v_{k-1} + (R(q) * a_meas - g) * dt
姿态更新：q_k = q_{k-1} * exp(0.5 * omega_meas * dt)其中R(q)是旋转矩阵，a_meas是加速度计测量值，omega_meas是陀螺仪测量值。这里的关键是姿态更新用了四元数指数映射，避免了欧拉角的奇异性。
4.4.3 观测模型
观测模型取决于你用了哪些传感器。以GPS+气压计为例：
GPS位置观测：z_gps = p + v_gps
气压计高度观测：z_baro = p_z + v_baro这里的v_gps和v_baro就是观测噪声，对应R矩阵中的元素。
实际项目中的教训：有一次我在eVTOL上测试EKF，发现高度估计在低空时抖动很厉害。排查后发现是气压计受到旋翼下洗气流的影响，噪声特性发生了变化。解决方案是在R矩阵中根据飞行状态动态调整气压计的噪声方差——悬停时增大R，前飞时减小R。
4.5 本章小结
EKF是eVTOL飞控中状态估计的基石。它的核心思想是用雅可比矩阵做线性化近似，然后套用标准KF的框架。调参是难点，但也是体现工程师经验的地方。
我个人建议，初学者先从简单的2维或3维状态开始，把EKF的流程跑通，再逐步扩展到完整的16维状态。不要一上来就搞复杂模型，否则出了问题根本不知道是哪里错了。
这张图展示了EKF在eVTOL中的完整流程。从预测到更新，再到输出状态估计，每一步都离不开雅可比矩阵和协方差矩阵的配合。调参就是在这张图上找到Q和R的最佳平衡点。
最后说一句：EKF不是万能的。如果系统非线性太强（比如大角度机动），一阶线性近似可能不够。这时候可以考虑UKF（无迹卡尔曼滤波）或者粒子滤波。但作为入门，EKF是必须啃下来的硬骨头。
公众号:蓝海资料掘金营，微信deep3321