从贝叶斯滤波到卡尔曼滤波：五方程推导与工程实践

1. 从贝叶斯视角重新认识卡尔曼滤波

很多人第一次接触卡尔曼滤波，是在控制理论或者信号处理的教材里，看到一堆矩阵公式和递推方程，推导过程能看懂，但合上书就想不起来为什么要这么算。我自己当年也是这样，直到后来把卡尔曼滤波放回贝叶斯滤波的框架里重新理解了一遍，才发现它其实是一个非常优雅的工程近似——用高斯分布的性质，把贝叶斯滤波里那个根本算不出来的积分，变成了几个矩阵乘法和加法。

这个系列的前四篇一直在铺贝叶斯滤波的基础：从最一般的贝叶斯公式出发，到递推贝叶斯估计，再到重要性采样和粒子滤波。到了卡尔曼滤波这里，核心问题变成了：当系统是线性的、噪声是高斯的，贝叶斯滤波能不能有一个闭式解？答案是能，而且这个闭式解就是卡尔曼滤波。

所以这篇文章不是单纯地推公式，而是想回答三个问题：第一，卡尔曼滤波到底在贝叶斯框架里做了什么近似；第二，那五个经典方程是怎么一步步从贝叶斯递推里"长"出来的；第三，推导过程中哪些地方容易想当然，实际写代码时又该注意什么。

适合的读者是已经了解贝叶斯滤波基本思想、但被卡尔曼滤波的矩阵推导卡住的人，或者已经会用filterpy、pykalman这类库、但想搞清楚内部到底在算什么的人。如果你完全没接触过贝叶斯滤波，建议先看本系列前面的内容，否则这里的很多符号会让你觉得突兀。

2. 卡尔曼滤波成立的两个硬前提

2.1 线性高斯系统到底意味着什么

卡尔曼滤波不是万能的，它的所有推导都建立在两个假设之上：状态转移和观测模型都是线性的，且过程噪声和观测噪声都服从高斯分布。这两条缺一不可，而且它们不是"锦上添花"的假设，而是推导能够闭合的根本原因。

线性意味着状态转移可以写成矩阵形式：

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

观测方程同样线性：

$$ z_k = H_k x_k + v_k $$

其中 $w_{k-1} \sim \mathcal{N}(0, Q)$，$v_k \sim \mathcal{N}(0, R)$。这里 $F_k$ 是状态转移矩阵，$H_k$ 是观测矩阵，$Q$ 和 $R$ 分别是过程噪声和观测噪声的协方差矩阵。

为什么线性这么关键？因为高斯分布经过线性变换之后仍然是高斯分布。如果 $x$ 服从高斯分布，那么 $Fx$ 也服从高斯分布，只是均值和协方差变了。这个性质叫做高斯分布的线性不变性，它是卡尔曼滤波能够用均值和协方差两个量就完整描述整个后验分布的根本原因。

2.2 高斯假设带来的"参数化红利"

如果状态分布不是高斯，那么即使系统线性，后验分布也可能变得非常复杂，可能需要用无数个参数才能描述。但高斯分布只需要两个参数：均值 $\mu$ 和协方差 $\Sigma$。这意味着贝叶斯递推里的"分布"这个无限维的对象，被压缩成了有限维的矩阵运算。

我经常用一个类比来解释这件事：贝叶斯滤波的一般形式就像是要你描述一个任意形状的物体，你得用点云、网格、或者一堆参数；而卡尔曼滤波相当于假设这个物体是个椭球，你只需要记录它的中心和三个轴的长度就够了。椭球假设当然不是对所有物体都成立，但对于很多实际系统来说，它已经足够好了。

注意：高斯假设和线性假设是绑在一起的。如果系统非线性但噪声高斯，可以用扩展卡尔曼滤波（EKF）做局部线性化；如果系统线性但噪声非高斯，卡尔曼滤波给出的只是最优线性估计，不是真正的最优估计。这两点在实际选型时经常被混淆。

3. 贝叶斯递推到卡尔曼五方程的完整链路

3.1 预测步：从Chapman-Kolmogorov到先验高斯

贝叶斯滤波的预测步本质上是计算先验分布：

$$ p(x_k | z_{1:k-1}) = \int p(x_k | x_{k-1}) p(x_{k-1} | z_{1:k-1}) dx_{k-1} $$

这个积分在一般情况下是算不出来的，但在线性高斯假设下，$p(x_{k-1} | z_{1:k-1})$ 是高斯分布，$p(x_k | x_{k-1})$ 也是高斯分布，两个高斯分布做卷积，结果仍然是高斯分布。

具体推导一下。假设上一时刻的后验是：

$$ p(x_{k-1} | z_{1:k-1}) = \mathcal{N}(\hat{x}{k-1|k-1}, P{k-1|k-1}) $$

状态转移是 $x_k = F_k x_{k-1} + B_k u_k + w_{k-1}$，其中 $w_{k-1} \sim \mathcal{N}(0, Q)$。那么先验分布的均值就是：

$$ \hat{x}{k|k-1} = F_k \hat{x}{k-1|k-1} + B_k u_k $$

协方差是：

$$ P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q $$

这两个式子就是卡尔曼滤波的预测方程。很多人背下来就完事了，但我想强调一下协方差那个式子里的 $F P F^T$ 是怎么来的。如果 $x$ 的协方差是 $P$，那么 $Fx$ 的协方差是 $F P F^T$，这是线性变换对协方差的影响。再加上过程噪声 $Q$，就得到了先验协方差。这里 $Q$ 是直接加在协方差上的，因为过程噪声和状态是独立的。

3.2 更新步：高斯乘积公式的妙用

更新步是卡尔曼滤波最精彩的部分。贝叶斯公式告诉我们：

$$ p(x_k | z_{1:k}) = \frac{p(z_k | x_k) p(x_k | z_{1:k-1})}{p(z_k | z_{1:k-1})} $$

分母是一个归一化常数，不依赖于 $x_k$，所以后验正比于似然乘以先验：

$$ p(x_k | z_{1:k}) \propto p(z_k | x_k) p(x_k | z_{1:k-1}) $$

在线性高斯假设下，先验是 $\mathcal{N}(\hat{x}{k|k-1}, P{k|k-1})$，似然是 $\mathcal{N}(H_k x_k, R)$。两个高斯分布相乘，结果仍然是高斯分布。这里用到一个重要公式：两个高斯分布的乘积（归一化后）仍然是高斯分布，其均值和协方差有闭式表达。

具体来说，如果：

$$ p_1(x) = \mathcal{N}(x; \mu_1, \Sigma_1), \quad p_2(x) = \mathcal{N}(x; \mu_2, \Sigma_2) $$

那么乘积正比于：

$$ \mathcal{N}(x; \mu, \Sigma) $$

其中：

$$ \Sigma = (\Sigma_1^{-1} + \Sigma_2^{-1})^{-1}, \quad \mu = \Sigma (\Sigma_1^{-1} \mu_1 + \Sigma_2^{-1} \mu_2) $$

把这个公式套到我们的问题上，先验对应 $\mu_1 = \hat{x}{k|k-1}$，$\Sigma_1 = P{k|k-1}$；似然对应 $\mu_2$ 需要稍微处理一下，因为似然是 $z_k = H_k x_k + v_k$，所以 $x_k$ 的"等效观测"是 $H_k^{-1} z_k$（如果 $H_k$ 可逆），协方差是 $H_k^{-1} R H_k^{-T}$。但更常见的做法是直接用矩阵求逆引理来化简。

3.3 卡尔曼增益的来历与直观解释

经过一番矩阵运算（这里省略中间步骤，重点看结果），后验均值和协方差可以写成：

$$ \hat{x}{k|k} = \hat{x}{k|k-1} + K_k (z_k - H_k \hat{x}_{k|k-1}) $$

$$ P_{k|k} = (I - K_k H_k) P_{k|k-1} $$

其中卡尔曼增益：

$$ K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R)^{-1} $$

这个增益矩阵的直观含义非常值得说清楚。$H_k P_{k|k-1} H_k^T + R$ 是观测预测的协方差，也就是"我们预期观测值会有多大不确定性"。$P_{k|k-1} H_k^T$ 是状态和观测之间的互协方差。卡尔曼增益本质上是在做一件事：根据先验不确定性和观测不确定性的相对大小，决定更相信预测还是更相信观测。

如果观测噪声 $R$ 很小，那么 $K_k$ 会接近 $H_k^{-1}$，后验几乎完全由观测决定；如果先验协方差 $P_{k|k-1}$ 很小，那么 $K_k$ 会接近零，后验几乎完全由预测决定。这个权衡是卡尔曼滤波最核心的直觉，也是调参时最应该关注的地方。

3.4 五个方程的汇总与符号约定

把上面的推导整理一下，卡尔曼滤波的五个经典方程是：

符号约定：$\hat{x}{k|k-1}$ 表示在 $k$ 时刻、利用 $k-1$ 时刻及之前的观测得到的估计，也叫先验估计；$\hat{x}{k|k}$ 表示利用 $k$ 时刻观测更新后的估计，也叫后验估计。这个双下标记号在推导时非常重要，写代码时如果搞混了，结果会完全错误。

4. 推导中最容易想当然的几个地方

4.1 协方差更新为什么不用 $(I-KH)P(I-KH)^T + KRK^T$

如果你去看一些教材，会发现协方差更新有两个版本。一个是上面写的 $P_{k|k} = (I - K_k H_k) P_{k|k-1}$，另一个是所谓的Joseph形式：

$$ P_{k|k} = (I - K_k H_k) P_{k|k-1} (I - K_k H_k)^T + K_k R K_k^T $$

这两个在数学上是等价的（在最优增益下），但数值性质完全不同。第一个式子计算量小，但在 $K_k$ 接近最优值时，$(I - K_k H_k)$ 可能接近奇异，导致协方差矩阵失去正定性，长期递推后可能出现负特征值，滤波器直接发散。

Joseph形式虽然计算量大一点，但它对任意 $K_k$ 都保持正定性，数值稳定性好得多。我在实际项目里，只要不是嵌入式那种极度受限的场景，都会优先用Joseph形式。这个坑我在一个姿态估计项目里踩过，跑了几个小时之后协方差矩阵出现负对角元，滤波器输出开始乱跳，换成Joseph形式之后问题消失。

4.2 增益公式里的矩阵求逆什么时候会出问题

$K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R)^{-1}$ 里有一个矩阵求逆，维度是观测维度。如果观测维度很高，或者 $H_k P_{k|k-1} H_k^T + R$ 接近奇异，求逆会不稳定。

常见的情况是观测维度大于状态维度，或者多个观测通道高度相关。这时候可以考虑用伪逆，或者改用信息滤波形式（信息矩阵的逆），后者在某些场景下数值性质更好。另一个实用技巧是给 $R$ 加一个很小的对角项，相当于人为增加观测噪声，牺牲一点最优性换取数值稳定。

提示：如果 $R$ 是奇异矩阵，说明某些观测方向没有噪声，这在物理上通常意味着观测有冗余或者模型有问题。实际中很少遇到严格奇异的 $R$，但接近奇异的情况不少，需要留意条件数。

4.3 初始协方差 $P_0$ 设大了会怎样

$P_0$ 的设定经常被忽视。如果 $P_0$ 设得过大，初始阶段卡尔曼增益会接近 $H^{-1}$，滤波器几乎完全信任观测，收敛很快但可能对异常观测很敏感。如果 $P_0$ 设得过小，滤波器会过度信任初始估计，收敛慢，甚至可能收敛到错误的值。

我的经验是，$P_0$ 应该反映你对初始状态的真实不确定度。如果你完全不知道初始状态，可以设一个较大的对角矩阵，但不要大到离谱（比如 $10^6$ 这种），否则数值上容易出问题。一个实用的做法是先用前几个观测做一次最小二乘估计，用那个估计的协方差作为 $P_0$。

5. 从推导到代码：一个可运行的实现框架

5.1 用Python实现一维卡尔曼滤波

理论说再多，不如跑一遍代码。下面是一个一维匀速运动模型的卡尔曼滤波实现，状态是位置和速度，观测是位置。这个例子虽然简单，但包含了卡尔曼滤波的所有核心步骤。

import numpy as np class KalmanFilter1D: def __init__(self, dt, process_noise, obs_noise, init_state, init_cov): # 状态转移矩阵：位置 += 速度 * dt self.F = np.array([[1, dt], [0, 1]]) # 观测矩阵：只观测位置 self.H = np.array([[1, 0]]) # 过程噪声协方差 self.Q = process_noise * np.array([[dt**3/3, dt**2/2], [dt**2/2, dt]]) # 观测噪声协方差 self.R = np.array([[obs_noise]]) # 初始状态和协方差 self.x = init_state self.P = init_cov def predict(self): self.x = self.F @ self.x self.P = self.F @ self.P @ self.F.T + self.Q def update(self, z): # 观测预测 z_pred = self.H @ self.x # 新息 y = z - z_pred # 新息协方差 S = self.H @ self.P @ self.H.T + self.R # 卡尔曼增益 K = self.P @ self.H.T @ np.linalg.inv(S) # 状态更新 self.x = self.x + K @ y # 协方差更新（Joseph形式） I = np.eye(self.P.shape[0]) self.P = (I - K @ self.H) @ self.P @ (I - K @ self.H).T + K @ self.R @ K.T return self.x, self.P

这段代码里有两个细节值得说。第一，过程噪声协方差 $Q$ 的构造用了连续时间白噪声模型的离散化结果，$dt^3/3$ 和 $dt^2/2$ 这些系数不是随便写的，它们来自对连续时间过程噪声的积分。第二，协方差更新用了Joseph形式，虽然多几次矩阵乘法，但数值上更稳。

5.2 调参时真正该动的是哪几个量

很多人调卡尔曼滤波的时候，喜欢同时动 $Q$、$R$、$P_0$，结果越调越乱。我的建议是分清楚这三个量的角色：

• $R$ 反映的是传感器噪声，这个通常可以从数据手册或者静态测试中估计出来，不应该随便调。
• $Q$ 反映的是模型不准确的程度，这个是最应该调的。如果模型很准，$Q$ 可以设小；如果模型有未建模的动态，$Q$ 要设大。
• $P_0$ 只影响初始收敛阶段，稳态之后它的影响会被遗忘。

一个实用的调参流程是：先固定 $R$ 为传感器噪声的估计值，然后调 $Q$ 直到滤波结果既不过于平滑（滞后）也不过于抖动（噪声大）。如果观测有异常值，可以考虑用鲁棒卡尔曼滤波或者对新息做卡方检验。

5.3 新息序列：判断滤波器是否健康的窗口

新息 $y_k = z_k - H_k \hat{x}{k|k-1}$ 是卡尔曼滤波里一个非常有用的诊断量。如果滤波器工作正常，新息应该是一个零均值的白噪声序列，其协方差是 $S_k = H_k P{k|k-1} H_k^T + R$。

如果新息出现以下情况，说明滤波器有问题：

我习惯在调试阶段把新息和 $3\sigma$ 边界画出来，如果新息频繁超出边界，说明滤波器对自己的不确定性估计过于乐观，需要增大 $Q$ 或 $R$。

6. 卡尔曼滤波的边界与常见误用

6.1 非线性系统直接套卡尔曼滤波的后果

卡尔曼滤波的推导严格依赖于线性假设。如果系统是非线性的，比如状态转移里有三角函数、有平方项，直接套卡尔曼滤波相当于用线性模型去近似非线性系统，误差会随着时间累积。

常见的做法是扩展卡尔曼滤波（EKF），在当前估计点对非线性函数做一阶泰勒展开，得到局部线性模型。但EKF也有自己的问题：线性化误差、雅可比矩阵计算复杂、在强非线性场景下可能发散。无迹卡尔曼滤波（UKF）用sigma点做确定性采样，避免了雅可比矩阵，精度通常比EKF好，但计算量更大。

选择哪种滤波，取决于非线性程度和计算资源。如果非线性不强，EKF够用；如果非线性强且计算资源充足，UKF更稳；如果状态维度很高，可能需要考虑粒子滤波或者信息滤波。

6.2 观测维度低于状态维度时的可观测性问题

卡尔曼滤波能工作，前提是系统可观测。如果某些状态从来没有被观测到，或者观测组合无法唯一确定状态，那么协方差矩阵在某些方向上不会收敛，滤波器可能给出看似合理但实际错误的结果。

一个典型的例子是用只有位置观测的传感器去估计位置、速度、加速度。如果加速度模型是随机的，那么加速度的可观测性依赖于位置观测的二阶差分，噪声会被放大。这时候需要仔细设计 $Q$，或者增加观测维度。

判断可观测性的一个实用方法是看 $P_{k|k}$ 的特征值。如果某些特征值长期不下降，说明对应的状态方向不可观测或者观测信息很弱。

6.3 什么时候该放弃卡尔曼滤波

卡尔曼滤波不是万能的。如果系统强非线性、噪声非高斯、状态维度极高、或者模型完全未知，卡尔曼滤波及其变体可能都不是好选择。这时候可以考虑粒子滤波、无迹滤波、或者基于学习的方法。

但反过来说，很多实际系统在局部范围内近似线性高斯，卡尔曼滤波的效果已经足够好，而且计算效率高、可解释性强。我在做工程选型时，通常先试卡尔曼滤波，如果效果不达标再考虑更复杂的方法，而不是一上来就用最复杂的。

7. 推导之外：几个值得记住的工程直觉

推导完了，代码也跑了，最后分享几个我在实际项目里总结的直觉，这些在教材里通常不会写，但用起来很管用。

第一，卡尔曼滤波的本质是加权平均。预测和观测各有一个估计，卡尔曼增益决定了两者的权重。权重由各自的不确定性决定，不确定性小的权重大。这个直觉在调参时非常有用：如果你觉得滤波器太信任观测，就增大 $R$；如果太信任预测，就增大 $Q$。

第二，协方差矩阵的对角元是各状态的不确定性，非对角元是状态之间的相关性。如果两个状态高度相关，观测其中一个会同时降低另一个的不确定性。这个性质在多传感器融合里非常有用，也是卡尔曼滤波比单独滤波再平均更好的原因。

第三，稳态卡尔曼滤波可以离线计算增益。如果系统是时不变的，$P_{k|k-1}$ 会收敛到一个稳态值，卡尔曼增益也会收敛。这时候可以离线算出稳态增益，在线只需要做矩阵乘法和加法，计算量大幅降低。很多嵌入式系统就是这么做的。

第四，数值稳定性不是小事。卡尔曼滤波的递推形式在长时间运行时可能积累数值误差，导致协方差矩阵失去正定性。除了用Joseph形式，还可以用平方根滤波或者UD分解滤波，这些方法在数值上更稳定，但实现复杂度更高。如果项目对可靠性要求高，值得投入时间做这些优化。

第五，新息是免费的诊断工具。不要只输出滤波结果，把新息也记录下来。新息的统计特性可以告诉你滤波器是否健康，比看滤波结果本身更有信息量。我在一个导航项目里就是靠新息序列发现了一个传感器间歇性偏置的问题，如果只看滤波输出，这个问题很难被发现。