3、状态估计核心：卡尔曼滤波原理（预测-更新模型）、扩展卡尔曼滤波（EKF）在飞控中的应用、传感器融合策略
各位同学，欢迎来到状态估计这一章。
说实话，飞控系统里最让我着迷的部分，就是状态估计。你想想看，我们手头只有一堆 noisy 的传感器数据——加速度计乱跳、陀螺仪飘移、GPS 偶尔抽风——但我们却要从中榨出飞行器最真实的姿态、位置和速度。这活儿，说白了就是“从垃圾堆里淘金”。
而卡尔曼滤波，就是我们手里最趁手的淘金铲。
3.1 卡尔曼滤波：预测-更新模型
卡尔曼滤波的核心思想，我可以用一句话概括：用模型预测未来，用测量修正预测。
它不是一个黑魔法，而是一个递归的、最优的估计器。为什么说“最优”？因为它同时考虑了模型的不确定性和测量的不确定性，然后给出一个方差最小的估计值。
核心五公式（离散卡尔曼滤波）：
预测阶段：
x̂_k|k-1 = A·x̂_k-1|k-1 + B·u_k
P_k|k-1 = A·P_k-1|k-1·Aᵀ + Q更新阶段：
K_k = P_k|k-1·Hᵀ·(H·P_k|k-1·Hᵀ + R)⁻¹
x̂_k|k = x̂_k|k-1 + K_k·(z_k - H·x̂_k|k-1)
P_k|k = (I - K_k·H)·P_k|k-1我来拆解一下这五个公式到底在干什么。
3.1.1 预测阶段：先猜一个
第一个公式 x̂_k|k-1 = A·x̂_k-1|k-1 + B·u_k，意思就是：根据上一时刻的状态，结合我们已知的控制输入（比如油门量），用运动模型推算出当前时刻的状态应该是什么。
第二个公式 P_k|k-1 = A·P_k-1|k-1·Aᵀ + Q，是在算这个“猜测”的置信度。Q 是过程噪声协方差矩阵，它代表了我们对模型的信任程度。Q 设得越大，说明我们越不相信模型，后续就更依赖测量值。
个人经验： 我在调参时，Q 矩阵经常被忽略。很多人只盯着 R（测量噪声）调，结果发现滤波发散。记住，Q 和 R 是一对冤家，要一起调。我曾经在一个四旋翼项目里，因为 Q 设得太小，导致滤波器对模型过度自信，姿态估计滞后了 200ms，差点炸机。
3.1.2 更新阶段：用测量来纠偏
第三个公式 K_k = P_k|k-1·Hᵀ·(H·P_k|k-1·Hᵀ + R)⁻¹，计算的是卡尔曼增益 K。这个 K 值决定了我们到底该信模型多一点，还是信传感器多一点。
- 如果测量噪声 R 很小（传感器很准），K 会变大，我们更相信测量。
- 如果模型预测的协方差 P 很小（模型很准），K 会变小，我们更相信模型。
第四个公式 x̂_k|k = x̂_k|k-1 + K_k·(z_k - H·x̂_k|k-1)，就是最终的修正。括号里的 z_k - H·x̂_k|k-1 叫“新息”（innovation），说白了就是“测量值减去预测值”的残差。然后用 K 去加权这个残差，修正预测值。
第五个公式更新协方差，为下一轮迭代做准备。
避坑指南： 我曾经遇到过一个情况，滤波器跑着跑着就发散了。查了半天，发现是 P 矩阵失去了正定性。原因是数值精度问题，导致 P 矩阵在迭代中变成了非对称矩阵。解决方案很简单：每次更新后强制对称化：P = (P + Pᵀ) / 2。这个小技巧救了我一命。
3.2 扩展卡尔曼滤波（EKF）在飞控中的应用
标准的卡尔曼滤波假设系统是线性的。但现实世界哪有那么美好？飞行器的运动模型是非线性的，传感器模型也是非线性的。怎么办？
EKF 的思路很粗暴：在估计点附近做一阶泰勒展开，把非线性系统线性化。
3.2.1 EKF 的核心改动
相比标准 KF，EKF 只改了两个地方：
- 状态预测： 不再用矩阵乘法，而是直接用非线性函数 f 计算：
x̂_k|k-1 = f(x̂_k-1|k-1, u_k) - 协方差传播： 用雅可比矩阵 F 代替原来的 A 矩阵：
P_k|k-1 = F·P_k-1|k-1·Fᵀ + Q，其中 F = ∂f/∂x
在飞控里，最常见的 EKF 应用就是姿态估计。我们通常用四元数来表示姿态，因为四元数没有万向锁问题。
飞控中 EKF 的典型状态向量：
x = [q0, q1, q2, q3, bg_x, bg_y, bg_z, ba_x, ba_y, ba_z]ᵀ其中 q 是四元数，bg 是陀螺仪零偏，ba 是加速度计零偏。
测量更新通常来自加速度计（提供重力方向参考）和磁力计（提供航向参考）。
我建议： 在实现 EKF 时，一定要小心雅可比矩阵的计算。手动求导容易出错，我个人的习惯是用 MATLAB 的 Symbolic Toolbox 或者 Python 的 SymPy 先推导一遍，再手写 C 代码。另外，注意四元数的归一化——每次更新后都要把四元数归一化，否则姿态会慢慢漂移。
3.3 传感器融合策略
有了卡尔曼滤波这个工具，我们怎么把不同的传感器捏合到一起？
飞控里常见的传感器融合策略，我归纳为三种模式：
| 融合策略 | 典型应用 | 优点 | 缺点 |
|---|---|---|---|
| 松耦合 | GPS + IMU | 实现简单，计算量小 | 无法利用传感器间的约束，精度有限 |
| 紧耦合 | GPS + IMU（原始观测量） | 精度高，抗干扰能力强 | 计算量大，实现复杂 |
| 深耦合 | GNSS 基带 + IMU | 极端环境下仍能工作 | 需要修改硬件，通用性差 |
在消费级飞控里，用得最多的是松耦合和紧耦合。我个人更倾向于紧耦合，虽然代码量翻倍，但效果确实好。
3.3.1 实际融合架构
以我参与过的一个开源飞控项目为例，传感器融合的流程是这样的：
嗯，这张图里有个关键点：IMU 的数据（陀螺仪和加速度计）是高频的，通常 1kHz 左右，用来做 EKF 的预测步。而 GPS 和磁力计是低频的，10Hz 左右，用来做更新步。这种“高频预测 + 低频修正”的模式，是飞控状态估计的经典打法。
3.3.2 时间戳对齐——一个容易被忽视的坑
我在项目中遇到过最头疼的问题，就是传感器时间戳没对齐。IMU 的时间戳是本地时钟，GPS 的时间戳是 UTC 时间，两者之间有个固定的延迟。如果不做补偿，EKF 的更新步就会用错时刻的测量值，导致估计结果出现周期性抖动。
避坑指南： 我曾经在一个项目中，GPS 数据到了之后直接丢进 EKF 更新，结果发现位置估计有 0.5 米的周期性误差。查了三天，最后发现是 GPS 数据从接收到被软件处理，中间有 20ms 的延迟。解决方案：在 GPS 数据包里带上硬件时间戳，然后在 EKF 里做时间补偿。说白了，就是要把所有传感器数据都对齐到同一个时间基准上。
3.4 小结
卡尔曼滤波不是万能的，但没有卡尔曼滤波的飞控是万万不能的。从标准 KF 到 EKF，核心思想没变：用模型预测，用测量修正。传感器融合策略的选择，取决于你的算力、精度要求和开发周期。
我个人建议初学者先从松耦合的 KF 开始，跑通了再上紧耦合的 EKF。别一上来就搞深耦合，那玩意儿调试起来能让你怀疑人生。
最后分享一个调参口诀：
- Q 大 R 小 → 滤波器响应快，但噪声大
- Q 小 R 大 → 滤波器平滑，但滞后严重
- 先调 Q 让预测跟上动态，再调 R 让测量抑制噪声
好了，状态估计的核心就讲到这里。下一节我们会深入飞控的另一个核心模块——控制律设计。嗯，到时候再聊。