卡尔曼滤波算法详细理解

1. 问题的定义

状态是 x，控制输入 u

线性形式：

$$x_k = F_k x_{k-1} + B_k u_k + w_k$$

• $F$：状态转移
• $B u$：控制
• $w$：过程噪声

EKF 则是非线性版本：

$$x_k = f(x_{k-1}, u_k) + w_k$$

有观测 z，可能是非线性的

线性 KF：

$$z_k = H x_k + v_k$$

EKF：

$$z_k = h(x_k) + v_k$$

卡尔曼滤波的目标是根据“预测状态 + 观测残差”来估计最优的 x

卡尔曼滤波就是求最优线性无偏估计器（BLUE）。

融合两个信息：

• 预测： 来自系统物理模型
• 观测： 来自传感器测量

融合时的权重 = 协方差的逆（信息矩阵）

这是 KF 的本质：

• 协方差大 → 不确定度大 → 权重小
• 协方差小 → 不确定度小 → 权重大

可以认为 KF 的过程就是对预测和观测的加权融合（信息矩阵为权）

科尔曼滤波主要包括几个个步骤：

• 1） 预测阶段 = 根据控制传播状态 x 和协方差矩阵 P

x 为状态，也就是需要估计未知数。 P 为对应的协方差矩阵。

$$x_{k|k-1} = f(x_{k-1|k-1})$$ $$P_{k|k-1} = F P_{k-1|k-1} F^\top + Q$$

• 状态走一步
• 协方差随状态传播
• 加上过程噪声

协方差矩阵的更新叫 不确定性的前向传播。

• 2）修正阶段或者叫更新阶段 = 根据观测残差来最小化误差

这里是卡尔曼滤波算法的核心。
本质是：

找一个 x 的更新方式，使得估计误差的协方差最小。

我们假设更新结构是：

$$x_{k|k} = x_{k|k-1} + K (z - h(x_{k|k-1}))$$

其中观测残差是：

$$y_k = z_k - h(\hat{x}_{k|k-1})$$

其中 h 函数是观测函数，可能是非线性的。

这里 K 就是卡尔曼增益， 第一项是预测的更新，第二项是观测残差。上面公式通俗理解就是有了预测的状态，有来观测到的残差，然后用卡尔曼增益根据观测到的残差来修正预测的状态。

• 目标
  有了更新的结构， 下面需要估计上面方程最终状态的协方差矩阵 P。最后，我们的目标是最小化协方差矩阵。上面公式观测是已知的， 预测也是已经计算好的， 唯一的变量是 K。所以我们推导出协方差矩阵 P 以后，求使得 P 最小化的 K，那么就可以得到最终的状态。

比较幸运的是，协方差矩阵是一个二次型，所以只要求导数，然后令导数等于 0，就可以得到 K。我们暂时先忽略二次型的推导，先整体看一下卡尔曼滤波的过程。

• 3. 计算卡尔曼增益 K

当前，KF 唯一要做的，就是：

对协方差 P 对 K 求导 = 0

结果得到：

$$K = P H^\top (H P H^\top + R)^{-1}$$

就是卡尔曼增益。

这个增益刚好体现：

• $P$：预测的不确定性
• $R$：测量噪声
• $H$：测量模型

1. 观测残差（Innovation）：

• 记忆：“观测减预测”

2. 线性化观测：

$$\mathbf{H}_k = \frac{\partial h}{\partial \mathbf{x}}\Big|_{\hat{\mathbf{x}}_{k|k-1}}$$

• EKF 核心：把非线性观测拉直线 ，如果 KF 那么不必求导。

3. 卡尔曼增益：

$$\mathbf{K}_k = \mathbf{P}_{k|k-1}\mathbf{H}_k^\top (\mathbf{H}_k\mathbf{P}_{k|k-1}\mathbf{H}_k^\top + \mathbf{R}_k)^{-1}$$

• 4） 更新：

状态更新：

$$\hat{\mathbf{x}}_{k+1|k+1} = \hat{\mathbf{x}}_{k+1|k} + \mathbf{K}(\mathbf{z} - h(\hat{\mathbf{x}}_{k+1|k}))$$

协方差更新：

$$\mathbf{P}_{k+1|k+1} = (\mathbf{I} - \mathbf{K} \mathbf{H}) \mathbf{P}_{k+1|k}$$

总结

一共就三个步骤：

• 1 进行状态预测和协方差预测。
• 2 计算卡尔曼增益
• 3 进行状态更新和协方差更新。

那么其中最为难的部分是卡尔曼增益求解的推导，下面详细介绍。

卡尔曼增益的求解

🟦 1. 基本公式

我们假设最优更新形式是：

$$x^+ = x^- + K (z - H x^-)$$

这里 H 是观测残差对状态变量的雅克比矩阵。K 是卡尔曼增益。

真实值：$x$
预测值：$x^-$
更新值：$x^+$
我们使用$x^-$ 和$x^+$更方便进行公式的推导。 $^+$表示更新状态，$^-$表示预测状态。

上面公式是 卡尔曼滤波的核心结构假设。

下一步就是定义“误差”。

🟦 2. 定义误差（KF 推导的灵魂）

$$e^- = x - x^-$$ $$e^+ = x - x^+$$

其中$e^-$是预测误差，$e^+$是更新误差。
记住：误差 = 真值 - 估计值

🟦 3. 写出更新后的误差（关键一步！）

我们用状态更新公式：

$$x^+ = x^- + K (z - H x^-)$$

代入误差定义：$$e^+ = x - x^+$$

$$e^+ = x - \left[x^- + K(z - Hx^-)\right]$$

展开：

$$e^+ = (x - x^-) - K(z - Hx^-)$$

替换观测模型：

$$z = Hx + v$$

代入：

$$e^+ = e^- - K(Hx + v - Hx^-)$$

合并：

$$e^+ = e^- - K(H(x - x^-) + v)$$ $$e^+ = (I - KH)e^- - Kv$$

🟦 4. 现在从误差推协方差

协方差定义：

$$P^+ = E[e^+ (e^+)^T]$$
这里同样使用$^+$表示更新状态， $^-$表示预测状态。

把误差方程代入：

$$e^+ = (I - KH)e^- - Kv$$

$$P^+ = E\left[( (I-KH)e^- + Kv ) ( (I-KH)e^- + Kv )^T \right]$$

期望里面是 $(a+b)(a+b)^T$ 型：

$$(a+b)(a+b)^T = aa^T + ab^T + ba^T + bb^T$$

带回我们的 a 和 b：

• $$a = (I-KH)e^-$$
• $$b = Kv$$

展开得：

$$P^+ = E\left[ (I-KH)e^- e^{-T}(I-KH)^T +(I-KH)e^-v^TK^T +K v e^{-T}(I-KH)^T +K v v^T K^T \right]$$

根据$$E[A+B] = E[A] + E[B]$$

所以：

$$P^+ = E[(I-KH)e^- e^{-T}(I-KH)^T] + E[(I-KH)e^- v^T K^T] + E[K v e^{-T}(I-KH)^T] + E[K v v^T K^T]$$

因为期望只对随机变量起作用：

$$E[ A X B ] = A E[X] B$$

所以我们把 K、H、I 都提到外面：

$$P^+ = (I-KH)\, E[e^- e^{-T}] \,(I-KH)^T + (I-KH)\, E[e^- v^T] \,K^T + K\,E[v e^{-T}]\, (I-KH)^T + K\,E[v v^T]\,K^T$$

假设：

$$E[e^- v^T] = 0,\quad E[v e^{-T}] = 0$$

这是卡尔曼滤波的经典假设：

• 预测误差 e⁻ 来自系统模型噪声 w
• 观测噪声 v 来自传感器
• 两者 统计独立

所以：

$$(I-KH)\,E[e^- v^T] K^T = 0$$ $$K\,E[v e^{-T}] (I-KH)^T = 0$$

把 E[e⁻e⁻ᵀ] 换成 P⁻，E[vvᵀ] 换成 R

预测误差的协方差：

$$E[e^- e^{-T}] = P^-$$

观测噪声的协方差：

$$E[v v^T] = R$$

带入之后最终：

$$P^+ = (I-KH)P^-(I-KH)^T + K R K^T$$

现在我们已经推导出更新状态对应的协方差矩阵的表达是，那么剩下的问题就是求协防差矩阵最小化的问题，未知量是 K，也就是卡尔曼滤波增益。 我们希望求得 K 使得最小化协方差矩阵。 最后带入原卡尔曼滤波方程，就会得到最终的状态方程。 这里最小化协方差矩阵的理由其实是因为假设问题是高斯的，这是一个广义最小二乘问题。

如何求 K？

我们可以看到，本质上 $P^+$ 是一个二次型（quadratic form）关于 K 的函数， 而二次函数的最小值就是对导数求 0 的解。所以我们只需要对 $P^+$ 求导，并令导数为 0，就可以求出 K 的最小值。

为什么 KF 中的协方差关于 K 是一个“凸二次函数”？

把公式写清晰一点：

$$P^+ = (I-KH)P^-(I-KH)^T + K R K^T$$

这里把它视作 关于 K 的二次型：

• $K R K^T$ 是 “正定矩阵 $R$” 乘以 K 的二次项
• $(I-KH)P^-(I-KH)^T$ 展开之后，也包含一次项和二次项
• 所以整个表达式是一个纯二次函数

二次函数最小值总是出现在导数为 0 的地方。所以直接求导，令倒数等于 0 即可。

求导推导

我们从矩阵展开开始，看清 K 的项是怎样出现的。

先把：

$$P^{+}=(I-KH)P^-(I-KH)^T$$

展开：
$$P^{+} = P^{-} - K H P^{-} - P^{-} H^{T} K^{T} + K H P^{-} H^{T} K^{T}$$

然后加上观测噪声项：

$$+ K R K^T$$

$$P^{+}=P^{-} - K H P^{-} - P^{-} H^{T} K^{T} + K H P^{-} H^{T} K^{T} + K R K^{T}.$$

目标是选择 $K$ 使得 $P^{+}$ “尽可能小”。为把矩阵目标变成标量方便求导，我们最常用的做法是最小化 $J(K)=\operatorname{tr}(P^{+})$（trace 保持矩阵半正定性信息，也是标准做法）。

1) 用 trace 把问题标量化

取迹并利用 trace 的线性与循环性质：

$$\begin{aligned} J(K)&=\operatorname{tr}(P^{+}) \\ &=\operatorname{tr}(P^{-}) - \operatorname{tr}(K H P^{-}) - \operatorname{tr}(P^{-} H^{T} K^{T}) \\ &\qquad + \operatorname{tr}(K H P^{-} H^{T} K^{T}) + \operatorname{tr}(K R K^{T}). \end{aligned}$$

利用 $\operatorname{tr}(A)=\operatorname{tr}(A^T)$ 和循环性质 $\operatorname{tr}(ABC)=\operatorname{tr}(BCA)=\operatorname{tr}(CAB)$，可以把第二项与第三项合并为一项的两倍（因为它们互为转置）：

$$\operatorname{tr}(K H P^{-}) \quad\text{与}\quad \operatorname{tr}(P^{-} H^{T} K^{T}) = \operatorname{tr}\big((K H P^{-})^{T}\big)$$

所以

$$J(K)=\operatorname{tr}(P^{-}) - 2\,\operatorname{tr}(K H P^{-}) + \operatorname{tr}\big(K (H P^{-} H^{T}) K^{T}\big) + \operatorname{tr}\big(K R K^{T}\big).$$

把包含 $K K^{T}$ 的项合并，定义

$$S \triangleq H P^{-} H^{T} + R,$$

（注意 $S$ 是对称的，且在可观测/有噪声假设下可逆），于是有：

$$J(K)=\operatorname{const} - 2\,\operatorname{tr}(K H P^{-}) + \operatorname{tr}\big(K S K^{T}\big),$$

其中常数项 $\operatorname{tr}(P^{-})$ 与 $K$ 无关，可忽略。

2) 求导规则速记（矩阵微分/导数常用规则）

在下面求导时我们用到常见结果（可当作记忆规则）：

• $\displaystyle \frac{\partial}{\partial K}\operatorname{tr}(K A) = A^{T}$.
  （A 不含 K 时成立）
• $\displaystyle \frac{\partial}{\partial K}\operatorname{tr}(K S K^{T}) = 2 K S$ ，当 $S$ 对称时可以写成 $2KS$（更严格地，导数是 $K(S+S^{T})$，若 $S=S^{T}$ 则为 $2KS$）。

这些都可以用矩阵微分的定义或将微分写成迹的形式验算，不过我们在此直接使用结果以保持清晰。

3) 对 $K$ 求导并令导数为 0

对 $J(K)$ 关于 $K$ 求导：

$$\frac{\partial J}{\partial K} = -2\,(H P^{-})^{T} + 2 K S.$$

解释每项来源：

• $-2\,\operatorname{tr}(K H P^{-})$ 对 $K$ 的导数给出 $-2 (H P^{-})^{T} = -2 P^{-}H^{T}$。等价写法常见为 $-2 P^{-} H^{T}$ 或 $-2 (HP^{-})^{T}$。
• $\operatorname{tr}(K S K^{T})$ 对 $K$ 的导数给出 $2 K S$（因为 $S$ 对称）。

设导数为零求极值：

$$-2 P^{-} H^{T} + 2 K S = 0.$$

两边除以 2 并整理：

$$K S = P^{-} H^{T}.$$

由于 $S = H P^{-} H^{T} + R$（通常是可逆的），得到：

$$\boxed{\,K = P^{-} H^{T} \big(H P^{-} H^{T} + R\big)^{-1}\,}$$

这就是标准的卡尔曼增益公式。