卡尔曼滤波：从阿波罗登月到全球气象预报

卡尔曼滤波 (Kalman Filter)

为什么需要卡尔曼滤波

状态估计（State Estimation） 是现代机器人、自动驾驶和航空航天技术的基石。平衡多个信息来源，获得可靠的估计值对于各种工程实践非常重要，Kalman Filter 就是解决这个问题的经典算法，也是统计学上对不确定性的确定度量的思想体现。

我们希望获得精准的定位，假设有两个信息来源，但它们都有误差：

1. 你的车速表：显示 100 km/h，但实际可能在 90-110 km/h 之间。
2. 你的 GPS：显示位置，但信号漂移严重，可能在 50 米范围内乱晃。

卡尔曼滤波的核心哲学是：既然两者都有错，我就通过加权平均来找到真相。

如果传感器很贵很准（观测误差小），我就多信传感器一点。如果车身很稳（模型误差小），我就多信物理公式一点。这个信多少的比例系数，就是卡尔曼增益（Kalman Gain, $K$）。

卡尔曼滤波不仅提供最佳估计值，还提供不确定性的度量。

• 状态 $x$：想知道的值（如：位置、速度）。
• 协方差矩阵 $P$：不确定度（如：误差范围 $\pm 5$米）。$P$ 越小，说明越自信。

建立两个方程（世界的描述）

首先，我们必须用数学语言描述这个系统。假设系统是线性（这是卡尔曼滤波的局限性，我们后面就会开始去解决这个问题）的：

状态方程（物理模型）：

\[x_k = Fx_{k-1} + Bu_k + w_k\]

• $x_k$：当前状态。
• $F$：状态转移矩阵（比如：下一秒位置 = 上一秒位置 + 速度 $\times$ 时间）。
• $u_k$：控制量（比如踩了油门）。
• $w_k$：过程噪声（Process Noise），服从高斯分布 $N(0, Q)$。这里 $Q$ 代表物理模型的不靠谱程度。

观测方程（传感器模型）：

\[z_k = Hx_k + v_k\]

• $z_k$：传感器读数。
• $H$：观测矩阵（把状态映射到读数，比如状态是[位置, 速度]，传感器只测[位置]，$H$ 就是 $[1, 0]$）。
• $v_k$：测量噪声（Measurement Noise），服从高斯分布 $N(0, R)$。这里 $R$ 代表传感器的不靠谱程度。

核心算法

步骤 A：预测 (Time Update) —— “瞎猜阶段” 在看传感器之前，先算算大概在哪。

1. 推算状态：

   \[\hat{x}_k^- = F\hat{x}_{k-1} + Bu_k\]

2. 推算不确定性：

   \[P_k^- = FP_{k-1}F^T + Q\]

步骤 B：更新 (Measurement Update) —— “修正阶段” 现在看了一眼传感器 $z_k$，发现和预测的不一样，开始修正。

1. 计算卡尔曼增益 $K$：

   \[K_k = \frac{P_k^- H^T}{H P_k^- H^T + R}\]
   • 这是最关键的公式！
   • 直观理解：$K \approx \frac{\text{误差}}{\text{误差} + R}$。
   • 如果传感器噪声 $R \to 0$，则 $K \to 1$ （完全信传感器）。
   • 如果预测误差 $P \to 0$，则 $K \to 0$ （完全信预测）。

2. 修正状态（得到最终结果）：

   \[\hat{x}_k = \hat{x}_k^- + K_k(z_k - H\hat{x}_k^-)\]
   • 最终估计 = 预测值 + $K \times$ (测量值 - 预测的测量值)。
   • 括号里的部分叫残差 (Innovation)

3. 修正不确定性：

   \[P_k = (I - K_kH)P_k^-\]

滤波的效果

经过这一套“瞎猜-看传感器-修正”的循环，你会得到神奇的效果：

1. 更准（Optimal Estimation）： 你的最终估计值 $\hat{x}_k$ 会比任何单一来源（纯物理推算或纯传感器读数）都要准。因为它在统计学上融合了两者的信息，找到了误差最小的最佳平衡点。

2. 更稳（Smoothing）： 甚至如果你的 GPS 信号突然跳变（噪声），卡尔曼滤波会因为它认为 $R$（传感器噪声）很大或者 $P$（预测误差）很小，而选择不完全相信这次跳变，从而画出一条平滑的轨迹。

3. 不确定性收敛： 随着时间的推移，协方差矩阵 $P_k$ 会迅速收敛到一个稳定的最小值。这意味着系统对自己的状态越来越“自信”。

简单来说：它用物理规律约束了传感器的胡言乱语，又用传感器数据修正了物理模型的盲目自信。

工程实践

在代码里，矩阵 $F$ 和 $H$ 通常由物理定律决定，是固定的。但 $Q$ 和 $R$ 是你需要“调”的参数。

$R$ (测量噪声协方差):使用传感器相关参数或者反复测量计算方法。

$Q$ (过程噪声协方差):根据实际情况调整，如果模型比较准，就设小一点，如果模型误差比较大，就设大一点，需要根据情况来反复测试超参数。

$x_0$ (初始状态): 如果你不知道物体在哪，设为 0 或者第一次测量值即可。KF 会自我修正，很快收敛。

$P_0$ (初始协方差): 如果你对 $x_0$ 没把握，就把 $P_0$ 设得非常大（比如单位矩阵 $\times 1000$）。这意味着告诉滤波器：“我一开始完全是在瞎猜，请尽快依赖传感器数据修正我。”

总结

卡尔曼滤波以其优雅的递归形式和统计学上的最优性，成为了状态估计领域的黄金标准。虽然这里介绍的是最基础的线性卡尔曼滤波，但它不仅解决了“怎么算得准”的问题，更给了我们一种重要的思维方式：永远不要轻信单一信源，要用概率的眼光看世界。

然而，现实世界往往是非线性的（比如机器人不是走直线，而是转弯）。这时我们需要引入扩展卡尔曼滤波 (EKF)。

进阶：扩展卡尔曼滤波 (EKF)

EKF 是卡尔曼滤波家族中应用最广泛的算法。如果你拆开一台大疆无人机或者查看早期阿波罗登月的导航代码，你会发现里面全是 EKF。

为什么要 EKF？

标准 KF 有一个致命的假设：世界是线性的。但现实世界通常是非线性的（比如机器人运动涉及角度 $\sin/\cos$，雷达测距涉及平方根 $\sqrt{}$）。

如果不做处理直接用 KF 会发生什么？ 高斯分布穿过非线性函数后，形状会扭曲（变成像“香蕉”一样的奇怪形状），不再是高斯分布，导致标准 KF 的公式失效。

EKF 的核心思想： 既然非线性太难处理，那我们就在局部把它看作线性的。这就好比地球是圆的（非线性），但在此时此刻你脚下的这块地，我们可以把它当成平的（线性）来处理。

数学机理（线性化与雅可比矩阵）

在标准 KF 中，我们假设 $x_k = Fx_{k-1}$。但在 EKF 中，状态转移和观测变成了非线性函数：

\[\begin{aligned} x_k &= f(x_{k-1}, u_k) + w_k \\ z_k &= h(x_k) + v_k \end{aligned}\]

这里就有了一个矛盾：

• 算状态（均值）时：我们可以直接把上一步的估计值代入非线性函数 $f(\cdot)$，这没问题。
• 算不确定性（协方差 P）时：协方差矩阵不能直接“代入”非线性函数。我们不能简单的算 $P_k = f(P_{k-1})$。

EKF 的解决方案： 为了更新协方差 $P$，我们必须找到一个线性的矩阵来近似代表那个非线性函数在当前点的“变形程度”。这个矩阵就是雅可比矩阵（Jacobian Matrix）。

EKF 将标准 KF 中的固定矩阵 $F$ 和 $H$ 替换为随着状态变化而变化的雅可比矩阵：

• $F_k = \frac{\partial f}{\partial x} \mid_{\hat{x}_{k-1}}$ （状态转移函数的局部斜率）
• $H_k = \frac{\partial h}{\partial x} \mid_{\hat{x}_k^-}$ （观测函数的局部斜率）

EKF 的核心流程（改进版）：

1. 状态预测（用非线性函数，保准）： \(\hat{x}_k^- = f(\hat{x}_{k-1}, u_k)\)
2. 协方差预测（用雅可比矩阵，近似）： \(P_k^- = F_k P_{k-1} F_k^T + Q\)

这就像是：虽然路是弯的（非线性），但我每走一步都沿着切线方向（雅可比）去估算我的误差范围。

工程实践与挑战

什么时候不用 EKF？

• 非线性极强时： 线性化误差太大，必须用 UKF。
• 不想算雅可比矩阵时： 公式太难推，用 UKF 或 PF。
• 多峰分布时： 需要用粒子滤波（PF）

这也是为什么在非线性极强或不想推导公式时，我们会选择 无迹卡尔曼滤波 (UKF)，它避开了雅可比矩阵的计算，这大大节约了开发时间。

进阶二：无迹卡尔曼滤波 (UKF)

如果你觉得 EKF 里的雅可比矩阵（偏导数）让你头痛欲裂，或者你面对的系统根本没法求导（比如有很多 if-else 跳转），那么 UKF 就是你的救星。

直觉理解 (The Intuition)

还记得我们在 EKF 里遇到的问题吗？当一个高斯分布穿过一个非线性函数时，它出来的形状往往不再是标准的椭圆，而可能会变成弯曲的“香蕉”形状。

EKF 的做法： 强行把非线性函数变直（切线）。它只看均值点那里的斜率，不管周围的情况。

UKF 的做法： UKF 的发明者 Julier 和 Uhlmann 有一个著名的洞见：“近似一个概率分布（高斯），比近似一个任意的非线性函数要容易得多。”

核心思想： 我不去动那个复杂的非线性函数。相反，我从当前的状态分布里，挑选几个非常有代表性的点（Sigma 点）。把这几个点直接扔进那个非线性函数里跑一遍。看这几个点落地在哪，然后根据它们的新位置，重新计算出一个新的高斯分布。

数学机理：无迹变换 (Unscented Transform)

UKF 的核心魔法叫做 无迹变换 (UT)。既然非线性太难算，那我们就采样。

但不同于蒙特卡洛（Monte Carlo）的随机采样，UKF 采用的是一种确定性采样 (Deterministic Sampling)。

具体步骤如下：

1. 挑选 Sigma 点 (Sigma Points Selection)

假设你的状态向量 $x$ 有 $n$ 维。UKF 会在均值周围对称地选 $2n+1$ 个点。

• 中心点： $\mathcal{X}_0 = \mu$ (当前的均值)

• 周围点： $\mathcal{X}_i = \mu \pm (\sqrt{(n+\lambda)P})_i$

  • 这里 $\sqrt{P}$ 是协方差矩阵的平方根（通常通过 Cholesky 分解获得）。
  • $\lambda$ 是缩放参数，控制这些点离中心有多远。

2. 非线性传播 (Propagation)

这是最爽的一步。我们不需要推导复杂的雅可比矩阵，直接把这几个 Sigma 点 $\mathcal{X}_i$ 扔进你的非线性物理方程 $f(\cdot)$：

\[\mathcal{Y}_i = f(\mathcal{X}_i)\]

• 优势： 不需要任何线性化！哪怕你的函数里有断点、跳变、绝对值 (abs)，都没问题。

3. 重组分布 (Reconstruction)

现在我们得到了一组转换后的点 $\mathcal{Y}_i$。怎么变回高斯分布（均值和方差）呢？ 加权平均！

• 新均值： $\hat{y} = \sum_{i=0}^{2n} W_i^{(m)} \mathcal{Y}_i$
• 新协方差： $P_y = \sum_{i=0}^{2n} W_i^{(c)} (\mathcal{Y}_i - \hat{y})(\mathcal{Y}_i - \hat{y})^T$

这里 $W_i$ 是根据距离预先计算好的固定权重。

4. 完整的 UKF 闭环

有了预测的均值和协方差后，UKF 依然使用卡尔曼更新公式： \(K = P_{xy} P_{yy}^{-1}\) \(\hat{x} = \hat{x}^- + K(z - \hat{z})\)

关键在于，这里的互协方差 $P_{xy}$ 也是通过 Sigma 点加权计算出来的，完全避免了雅可比矩阵 $H$ 的显式计算。

总结

UKF 是 EKF 的完美升级版。 只要算力允许（现代嵌入式芯片通常都允许），在工程中通常首选 UKF。因为它不用算雅可比矩阵，代码更简洁，而且精度通常更高。

不过，这三种算法具备一个共同的缺陷，不能使用太大的状态维度，因为他的计算过程中需要不断对矩阵进行求逆运算，当状态维度过大时，计算量会急剧增加，且容易出现数值不稳定的情况。此时集合卡尔曼滤波（EnKF）应运而生。

进阶三：集合卡尔曼滤波 (EnKF)

EnKF 是为了解决一个特定的场景而生的：当状态维度 $n$ 极大（比如 $10^6$），且模型非线性时，怎么办？

标准 KF 需要存储和计算一个 $n \times n$ 的协方差矩阵 $P$。

• 如果 $n = 10^6$，矩阵 $P$ 就有 $10^{12}$ 个元素。
• 存这个矩阵需要 8TB 内存。
• 对它做一次求逆运算，哪怕用超级计算机也要算几百年。

EnKF 的核心： 既然存不下 $P$，那我们就不存它！我们用统计学的方法来模拟它。

EnKF 不再维护那个巨大的 $P$ 矩阵，而是维护一个 “集合” (Ensemble)。想象一下，本来你需要画出一个完美的椭圆（高斯分布），现在你只需要在纸上点 $N$ 个点（比如 $N=50$ 或 $100$）。只要这 $N$ 个点的分布形状和那个椭圆差不多，我们就能用这 $N$ 个点来代表那个高斯分布。

只有两步的循环

步骤 A：预测 (Forecast) —— 并行跑模型

这一步最简单，也是 EnKF 最强大的地方。不需要算雅可比矩阵，不需要线性化。你只要把这 $N$ 个样本，每一个都扔进你的非线性物理模型 $f(\cdot)$ 里跑一步。

• 关键点： 给每个样本都加上独立的随机过程噪声 $\mathcal{w}_i$。这是为了防止所有样本跑到最后变成同一个点（丧失多样性）。
• 这一步可以在 GPU 上并行，速度极快。

步骤 B：分析/更新 (Analysis) —— 统计学会魔法

这是 EnKF 的灵魂。我们拿到了 $N$ 个预测后的样本，现在有了观测值 $z$，怎么更新这 $N$ 个点？

EnKF 巧妙地利用样本统计量来代替那巨大的协方差矩阵 $P$。关键两步走：

1. 计算卡尔曼增益 $K$： 标准公式是：$K = P H^T (H P H^T + R)^{-1}$。 但在 EnKF 里，我们根本不存 $P$。我们直接用样本计算出 $PH^T$ 和 $HPH^T$ 这两项：
   • $PH^T \approx$ 状态集合与预测观测集合之间的协方差。
   • $HPH^T \approx$ 预测观测集合的自协方差。
   • 这样，我们就避开了 $10^6 \times 10^6$ 大矩阵的存储和计算，只涉及小矩阵求逆（通常观测数量远小于状态维度）。

2. 更新每一个样本（扰动观测）： 为了保持统计学上的正确性（方差一致性），我们不能只用同一个观测值 $z$ 去更新所有样本。我们需要生成 $N$ 个带噪声的观测值：

   \[z_i = z + v_i, \quad v_i \sim N(0, R)\]

   然后，对每一个样本 $x_i$ 单独做卡尔曼更新：

   \[x_i^{new} = x_i + K (z_i - H x_i)\]
3. 汇总结果 (Aggregation)： 更新了 $N$ 个不一样的 $x_i^{new}$，我到底信哪一个？ 答案是：全都不信，也全都信。 EnKF 的输出从来不是单个样本，而是这个更新后的集合。
   • 如果你需要一个具体的估计值，就计算集合的均值：$\hat{x} = \frac{1}{N} \sum x_i^{new}$。
   • 如果你需要知道不确定性，就计算集合的方差：$P = \text{Cov}(x^{new})$。
   • 这个新的集合，将直接作为下一时刻预测步骤的输入，周而复始。

工程上的问题

在实际工程（如 WRF 气象模式、海洋环流模式）中，直接跑 EnKF 通常会失败。因为它面临两个由样本量不足引起的问题：

伪相关 (Spurious Correlations) —— 远距离的蝴蝶效应

• 原因： EnKF 的核心假设是用 $N$ 个样本（比如 50 个）来统计估计 $10^6$ 维状态的协方差。统计学告诉我们，样本量太少时，计算出的相关系数会有巨大的采样误差。
• 表现： 如果你计算一下协方差矩阵，会发现一些荒谬的现象：巴西的温度竟然和德克萨斯的风速有 0.9 的相关性。这在物理上是不可能的，纯粹是数学上的巧合（噪声）。
• 后果： 当巴西的温度传感器传来数据，卡尔曼滤波器会根据这个错误的协方差，强行去修正德克萨斯的风速。全地球的数据乱成一锅粥。
• 解法：局域化 (Localization)
  • 原理： 强行引入物理常识——距离太远的状态之间没有相关性。
  • 操作： 在计算卡尔曼增益 $K$ 时，把计算出的协方差矩阵 $PH^T$ 点乘一个距离权重矩阵。距离越近权重越接近 1，距离超过一定半径（比如 500km）权重直接设为 0。这样就切断了那些远距离的虚假联系。

滤波发散 (Filter Divergence) —— 致命的过度自信

• 原因： 模型总是不完美的，而我们的 $N$ 个样本在迭代过程中，往往因为都受到了相同的观测信息牵引，导致它们长得越来越像（方差越来越小）。
• 表现：
  1. 集合的方差 $P$ 迅速趋近于 0。
  2. 根据公式 $K = PH^T(\dots)^{-1}$，卡尔曼增益 $K$ 也会趋近于 0。
  3. 结果： 滤波器认为自己已经完美掌握了真理（不确定性为 0），从而完全拒绝任何新的观测数据（$K=0$ 表示完全忽略观测）。即便真实状态已经跑偏了十万八千里，滤波器还在原地自嗨。
• 解法：协方差膨胀 (Covariance Inflation)
  • 原理： 既然你的方差总是估计得偏小，那我们就人为地把它增大一点，保持对新数据的敏感度。
  • 操作： 每次预测完，把所有样本 $x_i$ 偏离均值 $\bar{x}$ 的部分乘以一个系数 $\lambda > 1$（比如 1.01）： \(x_i^{new} = \bar{x} + \lambda (x_i - \bar{x})\)
  • 这就像给系统不断注入一点点不确定性，防止它过早地盲目自信。

总结

EnKF 为什么有效

1. 避开了雅可比矩阵： 不需要求导，直接扔进模型跑，非线性随便搞。
2. 避开了大矩阵存储： 用 50 个样本就能模拟 100 万维度的协方差。
3. 天然并行： 50 个样本可以扔给 50 个 CPU 核心同时跑。

前面介绍的方法都有一个共同点：假设单峰分布（不管怎么折腾，最后还是用均值和方差说话）。

如果分布不是单峰的，上述所有方法都会失效。这时候，就需要 粒子滤波 (Particle Filter)，一种基于Bayes滤波的改进方法。

附录：主流状态估计算法横向对比

方法	状态维度 (n)	分布假设	为什么这么做？	缺点
KF / EKF / UKF	维度 $n$ 决定了矩阵大小 $(n \times n)$	高斯 (Gaussian)	只要存均值和方差，计算快，不仅能迭代，还能求解析解。	处理不了“多峰”情况（如不知道自己在两个相似房间的哪一个）。
EnKF (集合卡尔曼)	维度 $n$ 可以很大，只存样本数 $N$	近似高斯	为了解决 $n$ 太大导致矩阵存不下的问题。	依然难以处理强非高斯分布。
粒子滤波 (PF)	维度 $n$ 不能太大	任意分布 (用粒子拟合)	为了解决“多峰”、“非高斯”等复杂情况。	计算量大，维度高了粒子不够用（粒子匮乏）。