The Kalman Filter Family: KF, EKF, UKF, and EnKF

卡尔曼滤波 (Kalman Filter)

为什么需要卡尔曼滤波

状态估计（State Estimation）是机器人、自动驾驶和航空航天中的基础问题。工程系统需要把物理模型和带噪声的传感器读数结合起来，估计无法直接准确观测的状态。Kalman Filter 为线性状态空间模型提供了一套递归计算方法。

假设我们要估计车辆的位置和速度，可以使用两个信息来源，但它们都有误差：

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

车速表和 GPS 测量的量并不完全相同。卡尔曼滤波先用运动模型把上一时刻的位置和速度推到当前时刻，再用 GPS 等观测修正预测。这个过程可以理解为按照不确定性分配权重，但不只是对两个读数做简单平均。

如果传感器噪声较小，更新时会更多参考观测；如果模型预测的不确定性较小，更新幅度就会减小。两者之间的权重由**卡尔曼增益（Kalman Gain, $K$ ）**决定。

在线性、高斯噪声等假设成立时，卡尔曼滤波可以得到最小均方误差意义下的估计，并同时维护估计的不确定性。

状态 $x$ ：想知道的值（如：位置、速度）。
协方差矩阵 $P$ ：描述状态估计误差的方差和变量之间的协方差。 $P$ 越小，表示模型认为当前估计越集中，但它不是直接等同于“误差范围 $\pm 5$ 米”。

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

首先用数学语言描述系统。标准卡尔曼滤波假设状态转移和观测关系是线性的，并为过程噪声与测量噪声指定协方差：

状态方程（物理模型）：

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) —— 在看传感器之前，先估计大概在哪。

推算状态：
$\hat{x}_k^- = F\hat{x}_{k-1} + Bu_k$
推算不确定性：
$P_k^- = FP_{k-1}F^T + Q$

步骤 B：更新 (Measurement Update) —— 看到传感器 $z_k$ 后，用观测修正预测。

计算卡尔曼增益 $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$ （完全信预测）。
修正状态（得到最终结果）：
$\hat{x}_k = \hat{x}_k^- + K_k(z_k - H\hat{x}_k^-)$
- 最终估计 = 预测值 + $K \times$ (测量值 - 预测的测量值)。
- 括号里的部分叫残差 (Innovation)
修正不确定性：
$P_k = (I - K_kH)P_k^-$

在假设成立时能得到什么

经过“预测—观测—修正”的循环，滤波器会持续更新状态和不确定性：

融合模型与观测： 当模型、噪声假设和参数设置合理时，最终估计 $\hat{x}_k$ 通常比只使用物理预测或单次传感器读数更稳定。
抑制部分测量噪声： 如果 GPS 信号突然跳变（噪声），卡尔曼滤波会因为 $R$ （传感器噪声）较大或者 $P$ （预测误差）较小，而选择不完全相信这次跳变，从而画出一条平滑的轨迹。
跟踪不确定性： 在系统可观测、参数稳定等条件下，协方差矩阵 $P_k$ 可能逐渐趋于稳定。模型失配或噪声设置不当时，协方差也可能失真或发散。

直观地说，物理模型提供连续预测，传感器负责纠正预测偏差，两者的相对权重随不确定性变化。

工程实践

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

R

（**测量噪声协方差**）：可以根据传感器规格、静态测量或重复实验估计。

Q

（**过程噪声协方差**）：描述状态模型没有覆盖的扰动。模型遗漏的运动或外部干扰越大，通常需要设置得越大，并通过残差和实际轨迹反复检查。

x_0

（**初始状态**）：可以使用第一次测量值或已有先验。初始化偏差能否被修正，取决于系统的可观测性和后续观测质量。

P_0

（**初始协方差**）：如果对

x_0

缺少把握，可以设置较大的初始协方差，使滤波器在早期更新中更多参考观测。数值仍需与状态量纲和实际误差范围匹配。

总结

卡尔曼滤波的递归形式适合实时状态估计，但标准版本依赖线性模型和噪声假设。现实系统存在明显非线性时，需要使用扩展卡尔曼滤波等变体。

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

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

EKF 是卡尔曼滤波家族中常用的非线性状态估计方法，广泛用于机器人、导航和传感器融合。

为什么要 EKF？

标准 KF 要求状态转移和观测模型是线性的。实际系统经常包含非线性关系，例如机器人运动中的角度 $\sin/\cos$ ，以及雷达测距中的平方根 $\sqrt{}$ 。

高斯分布经过非线性函数变换后，通常不再保持严格的高斯形状。直接套用标准 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})$ 。

为了更新协方差 $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 的预测过程可以写成：

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

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

工程实践与挑战

什么时候不用 EKF？

非线性较强、局部线性化误差明显时：可以考虑 UKF 或粒子滤波（PF）。
雅可比矩阵难以推导或维护时：可以考虑 UKF、自动微分或数值方法。
状态分布明显多峰时：粒子滤波通常更合适。

UKF 避开了显式雅可比矩阵，在部分非线性系统中能减少推导和维护成本，但计算量、参数设置和数值稳定性仍需单独评估。

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

当雅可比矩阵难以推导，或局部线性化精度不足时，可以考虑无迹卡尔曼滤波（UKF）。

直觉理解 (The Intuition)

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

EKF 的做法： 在均值附近使用一阶导数近似非线性函数，因此精度依赖当前点附近的线性化效果。

UKF 的做法： Julier 和 Uhlmann 提出的思路是，不直接线性化非线性函数，而是选择一组确定性的采样点来近似状态分布经过函数后的变化。

UKF 从当前状态分布中选择一组 Sigma 点，让每个点经过原始非线性函数，再根据变换后的点重新估计均值和协方差。

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

这个过程称为 无迹变换（Unscented Transform, 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)

优势： 不需要显式的一阶线性化。不过，函数存在不连续、强跳变或数值不稳定时，UKF 的近似仍可能失效，需要通过实验检查。

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 更容易实现，也可能得到更好的近似。但它不是 EKF 的通用替代：状态维度、计算预算、模型结构和已有实现都会影响选择。

KF、EKF 和 UKF 都需要维护协方差并进行矩阵运算。状态维度很大时，存储和计算成本会迅速增加，也更容易遇到数值稳定性问题。集合卡尔曼滤波（EnKF）用样本集合近似协方差，适合处理某些高维系统。

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

EnKF 主要面向状态维度很大、直接维护完整协方差矩阵成本过高的场景，例如维度达到 $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) —— 用样本统计量更新集合

预测得到 $N$ 个样本后，下一步是用观测值 $z$ 更新整个集合。

EnKF 利用样本统计量代替完整协方差矩阵 $P$ 。更新过程包括以下三步：

计算卡尔曼增益 $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$ 大矩阵的存储和计算，只涉及小矩阵求逆（通常观测数量远小于状态维度）。
更新每一个样本（扰动观测）： 为了保持统计学上的正确性（方差一致性），我们不能只用同一个观测值 $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)$
汇总结果 (Aggregation)： 更新后得到 $N$ 个不同的 $x_i^{new}$ 。EnKF 保留整个集合，而不是从中选择某一个样本作为输出。
- 如果你需要一个具体的估计值，就计算集合的均值： $\hat{x} = \frac{1}{N} \sum x_i^{new}$ 。
- 如果你需要知道不确定性，就计算集合的方差： $P = \text{Cov}(x^{new})$ 。
- 这个新的集合，将直接作为下一时刻预测步骤的输入，周而复始。

工程上的问题

在 WRF 气象模式、海洋环流模式等高维应用中，有限集合会带来明显的采样误差。实际系统通常还需要处理以下两个问题：

伪相关 (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，新的观测难以修正已经偏离的状态估计。
解法：协方差膨胀 (Covariance Inflation)
- 原理： 既然方差总是估计得偏小，就人为地把它增大一点，保持对新数据的敏感度。
- 操作： 每次预测完，把所有样本 $x_i$ 偏离均值 $\bar{x}$ 的部分乘以一个系数 $\lambda > 1$ （比如 1.01）： $x_i^{new} = \bar{x} + \lambda (x_i - \bar{x})$
- 这就像给系统不断注入一点点不确定性，防止它过早地盲目自信。

总结

EnKF 的主要优势是直接运行非线性模型，不需要显式维护完整协方差矩阵，而且集合成员可以并行计算。少量样本能够让百万维状态估计变得可计算，但近似质量依赖集合规模、局域化、协方差膨胀和具体物理模型。

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

如果状态分布明显多峰，依赖单峰高斯近似的方法可能无法保留不同模式。这时可以考虑 粒子滤波（Particle Filter），用带权粒子近似一般形式的贝叶斯滤波分布。

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

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