3. 卡尔曼滤波入门
在 ArduPilot 的 EKF3 中，卡尔曼滤波不是一段孤立的数学公式，而是把 IMU、气压计、磁罗盘、GPS 等传感器数据融合成“位置、速度、姿态、零偏”等状态估计的工程框架。理解它，关键要抓住四个词：状态空间模型、预测、更新、卡尔曼增益。本节用直观方式把这四件事串起来，为后续 EKF3 的 24 维状态、协方差矩阵和传感器融合打基础。
一、状态空间模型：把“系统”写成向量与矩阵
卡尔曼滤波的第一步，是把被估计对象写成状态向量 x。在简化的一维位置估计中，状态可以写成：
x = [位置 p, 速度 v]^T系统如何随时间变化，由状态转移矩阵 F 描述。若采样周期为 dt，则：
p(k+1) = p(k) + v(k) * dt
v(k+1) = v(k)
F = [1 dt
0 1]同时，传感器不会直接告诉我们“状态”，而是给出与状态相关的观测。例如 GPS 测位置，观测矩阵 H = [1 0]；气压计测高度，也类似。状态空间模型就是：
x(k+1) = F * x(k) + B * u(k) + w(k)
z(k) = H * x(k) + v(k)其中 w 是过程噪声，v 是观测噪声。EKF3 中状态维度更高，但结构完全一致：F 描述惯性递推，H 描述“哪个传感器看哪个状态”。
二、预测与更新：卡尔曼滤波的两步循环
卡尔曼滤波每个周期只做两件事：预测和更新。预测利用上一时刻状态和运动模型，推算当前时刻状态；更新利用当前传感器观测，对预测结果进行修正。
- 预测：先验状态
x⁻ = F x，先验协方差P⁻ = F P Fᵀ + Q。 - 更新：计算卡尔曼增益
K = P⁻ Hᵀ (H P⁻ Hᵀ + R)⁻¹，再修正状态x = x⁻ + K (z − H x⁻)，最后更新协方差P = (I − K H) P⁻。
其中 Q 是过程噪声协方差，R 是观测噪声协方差。预测让状态“往前走”，更新让状态“被传感器拉回来”。EKF3 中，预测由 IMU 高频驱动，更新由 GPS、气压计、磁罗盘等低频观测触发。
三、协方差传播：不确定性如何“长大”和“缩小”
协方差矩阵 P 是卡尔曼滤波的灵魂。它不描述状态本身，而描述“我们对状态有多不确定”。预测阶段，P⁻ = F P Fᵀ + Q，不确定性会因运动模型和过程噪声而增大；更新阶段，P = (I − K H) P⁻，观测到来后不确定性被压缩。
在 EKF3 中，P 是一个 24×24 的矩阵，对角线元素对应每个状态量的方差。例如位置方差变大，说明位置估计变“虚”；GPS 更新后位置方差下降，说明估计更可信。工程上调 Q 和 R，本质上就是在调“更信模型”还是“更信传感器”。
四、卡尔曼增益：信任的“分配器”
卡尔曼增益 K 决定更新时“听预测多少、听观测多少”。当观测噪声 R 很大时，K 变小，滤波器更相信预测；当预测协方差 P⁻ 很大时，K 变大，滤波器更相信观测。
| 条件 | 卡尔曼增益 K | 滤波器行为 | EKF3 场景 |
|---|---|---|---|
| 观测噪声 R 很大 | K 变小 | 更信任预测 | GPS 信号差、多路径严重 |
| 预测协方差 P⁻ 很大 | K 变大 | 更信任观测 | 长时间无 GPS，位置不确定度大 |
| R 小且 P⁻ 大 | K 接近 1 | 几乎直接采用观测 | 高精度 RTK 首次定位 |
| R 大且 P⁻ 小 | K 接近 0 | 几乎忽略观测 | 磁罗盘受干扰、气压计突变 |
五、案例与误区：从一维到 EKF3
假设无人机悬停，IMU 预测位置缓慢漂移，GPS 每秒给一次位置。预测阶段位置方差从 0.5 涨到 0.8；GPS 更新时 R = 0.3，计算得 K ≈ 0.73，于是新位置 = 预测位置 + 0.73 ×（GPS 位置 − 预测位置）。若 GPS 突然变差，R 增大，K 自动减小，滤波器不会跟着跳变。
K 是随协方差动态变化的，不是固定系数。另一个误区是只调 Q 不调 R，导致滤波器要么过度信任 IMU 而漂移，要么过度信任 GPS 而抖动。
在 ArduPilot 中，EKF3 的参数如 EK3_ACC_P_NSE、EK3_GBIAS_P_NSE、EK3_MAG_P_NSE 等，正是用来设定过程噪声；而各传感器的噪声则由驱动层和 EK3_*_NOISE 类参数描述。理解预测、更新、协方差传播和卡尔曼增益，就能看懂 EKF3 日志中 NKF1、NKF2、NKF3 里状态和协方差的变化，也能在排障时判断“是模型错了，还是传感器错了”。
一句话总结：状态空间模型定义“估什么”，预测和更新定义“怎么估”，协方差传播定义“有多确定”，卡尔曼增益定义“信谁更多”。这四者构成卡尔曼滤波的完整闭环，也是 EKF3 组合导航的数学骨架。