从贝叶斯滤波到卡尔曼滤波：公式推导与代码实现

先说个题外话。我在做传感器融合和定位相关项目时，经常遇到这种情况：同学或同事一听到“卡尔曼滤波”这个名字，第一反应是“那是搞控制或无人机的人才会用的高级算法”，然后就绕道走了。但当我告诉他们“你看手机里的导航、手环里的计步、甚至你打游戏时鼠标的平滑移动，背后都有这个算法的影子”时，大家又往往露出“原来是它”的表情。

所以这篇系列开篇，我打算不画任何复杂的流程图，不搬一堆云里雾里的公式吓人，而是沿着一条最自然的思考路径，把“贝叶斯滤波”和“卡尔曼滤波”这两件事彻底讲清楚。尤其要把“为什么卡尔曼滤波是贝叶斯滤波的一种特例”这种听起来很抽象的话，落到可以手推、可以写代码、可以跑起来看效果的程度。这篇偏重梳理脉络和推导，下一篇会专门聊工程实现和参数调优。

1. 先把问题摆出来：我们到底在“滤”什么东西

先说个经典场景。你有一个无人机，想知道它现在飞在什么位置。GPS告诉你一个位置，但GPS有误差，可能有5到10米的偏移；惯性传感器告诉你一个位置，但它是通过对加速度积分算出来的，时间一长就会飘。两个来源都有噪声，都不完美，你该怎么判断无人机到底在哪？

这就是状态估计问题的雏形：系统里有一个我们看不见的真实状态（位置、速度、温度、电压等），我们手上只有一组被噪声污染过的观测数据，需要想办法把真实状态“估计”出来。“滤波”这个词听起来像是在说“滤掉噪声”，其实是这个意思——从有噪声的观测里把干净的状态提取出来。

假设我们从第1帧观测一直做到第k帧观测，拿到了 (z_{1:k})，希望求当前状态 (x_k) 的概率分布：

[
P(x_k | z_{1:k})
]

这个式子翻译成人话就是：在已经看到了到目前为止全部观测数据的前提下，状态是 (x_k) 的可能性有多大。在数学上，它叫“后验概率分布”。有了这个分布，我们不光能知道“状态最可能是多少”（取分布的均值），还能知道“我们有多确定”（看分布的方差）。这就是贝叶斯滤波的起点——把估计问题当成“算概率”的问题来做。

如果状态变量是离散的，那这个概率分布就是一堆离散值上的概率质量；如果状态变量是连续的，那就是一个概率密度函数。不管是离散还是连续，思路完全一样。

2. 贝叶斯滤波：一个朴素但“算不动”的框架

2.1 预测和更新：贝叶斯滤波的两板斧

贝叶斯滤波不指定系统长什么样，它只要求我们能用两个模型描述这个系统：

• 状态转移模型：描述状态随时间怎么演化，即 (x_k) 如何从 (x_{k-1}) 变化而来，记为 (P(x_k|x_{k-1}))。
• 观测模型：描述在某个状态下我们会观测到什么，即 (z_k) 与 (x_k) 的关系，记为 (P(z_k|x_k))。

有了这两个模型，贝叶斯滤波就是一个两步循环：

第一步，预测（也叫先验更新）： 在上一步得到后验 (P(x_{k-1}|z_{1:k-1})) 之后，利用状态转移模型预测当前时刻的状态：

[
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}
]

这其实就是全概率公式。它相当于在说：我不确定上一时刻的状态具体是哪一个，但我对上一时刻的状态有一个概率描述，把这些可能性全部“加权平均”地往前推进。

第二步，更新（也叫后验修正）： 当新的观测 (z_k) 到来，用贝叶斯公式修正预测结果：

[
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})}
]

分母是一个归一化常数，作用是保证右边积分出来等于1：

[
P(z_k | z_{1:k-1}) = \int P(z_k | x_k) P(x_k | z_{1:k-1}) , dx_k
]

这个框架在概念上非常优雅，每一步都有清晰的概率含义。它的缺点也一眼就能看出来：每一步都有一个积分，而一般函数的积分没有解析解，只能在离散网格上做数值近似。如果状态是连续的高维向量（比如位置+速度+朝向就是4到9维），离散网格的网格点数量会随维度指数爆炸，根本算不过来。

2.2 贝叶斯滤波的工程困境：积分这个拦路虎

我在实际课程和代码里，见过太多人在贝叶斯滤波这一步被劝退。因为一旦涉及连续状态，贝叶斯滤波需要数值积分，而这个数值积分的代价高到不可接受。

MIT 有一门经典课程叫“Underactuated Robotics”，里面就专门讲到：贝叶斯滤波是概率机器人学的灵魂框架，但实际实现时几乎没有人直接积分贝叶斯公式，所有人都在寻找能够绕过那个积分、同时保持问题结构的方式。卡尔曼滤波就是其中最成功的一种“特例”。

3. 从贝叶斯到卡尔曼：三个假设如何把积分变成算术

卡尔曼滤波做的事情，本质上是对贝叶斯滤波加上了三个强假设，使得所有积分都有解析解，从而把“数值积分”变成了“矩阵运算”。

3.1 假设一：线性高斯系统

第一个假设是：状态转移模型和观测模型都是线性的，且噪声都是加性高斯噪声。用公式写出来就是：

[
x_k = A x_{k-1} + B u_k + w_k
]

[
z_k = H x_k + v_k
]

其中 (w_k \sim \mathcal{N}(0, Q))，(v_k \sim \mathcal{N}(0, R))。这里 (A) 是状态转移矩阵，(H) 是观测矩阵，(B) 是控制输入矩阵，(u_k) 是控制量，(Q) 是过程噪声协方差，(R) 是观测噪声协方差。所有噪声都假设为相互独立的高斯白噪声。

为什么要加高斯假设？因为高斯分布有一个无与伦比的优点：**两个高斯分布相乘，结果仍然是高斯分布（只是未归一化）。对高斯分布积分，也只要记住均值和方差两个数字。**换句话说，只要初始分布是高斯，经过线性变换和贝叶斯更新之后，整个过程中分布一直保持高斯，我们只需要跟踪均值和协方差这两个量，其它信息不需要。这就像是概率世界里一个完美的“闭合回路”——进去的是高斯，出来的还是高斯。

3.2 假设二：初始状态服从高斯分布

(x_0 \sim \mathcal{N}(\mu_0, P_0))，初始状态本身的不确定性由 (\mu_0) 和 (P_0) 描述。这个假设配合前面两条，保证从第0步到第k步，每一个时刻的分布都保持高斯形态。如果初始分布不是高斯，哪怕系统是线性的，预测那一步的积分也会很快把分布扭曲成非高斯。

3.3 假设三：噪声互相独立

过程噪声和观测噪声不相关，并且各自在时间上也不相关（白噪声假设）。这保证了我们在做预测和更新两步时，噪声项不会引入跨时刻的耦合，积分才可能拆开简化。

这三个假设凑在一起，卡尔曼滤波的整个推导只需用到线性代数和多元高斯分布的概率密度函数，不再需要任何数值积分。

3.4 推导关键：多元高斯分布的条件密度公式

为了把卡尔曼滤波公式不打磕绊地推导出来，我得先复习一个东西——多元高斯分布的条件分布公式。

设 (x) 和 (y) 的联合分布是：

[
\begin{bmatrix} x \ y \end{bmatrix} \sim \mathcal{N} \left( \begin{bmatrix} \mu_x \ \mu_y \end{bmatrix}, \begin{bmatrix} \Sigma_{xx} & \Sigma_{xy} \ \Sigma_{yx} & \Sigma_{yy} \end{bmatrix} \right)
]

那么给定 (y) 时，(x) 的条件分布也是高斯分布，且：

[
x|y \sim \mathcal{N} \left( \mu_x + \Sigma_{xy} \Sigma_{yy}^{-1} (y - \mu_y), ; \Sigma_{xx} - \Sigma_{xy} \Sigma_{yy}^{-1} \Sigma_{yx} \right)
]

这个公式是卡尔曼滤波“更新步”的数学底座。

4. 手推卡尔曼滤波五公式：从预测到更新

下面我们把贝叶斯滤波的两步，分别套进线性高斯假设里，逐个推导。

4.1 预测步：推导状态和协方差的先验估计

假设上一步的后验分布是：

[
x_{k-1} \sim \mathcal{N}(\hat{x}{k-1}, P)
]

其中 (\hat{x}{k-1}) 是上一步的均值估计，(P) 是上一步的协方差矩阵。现在我们要用状态转移方程得到 (x_k)。由于 (w_{k-1}) 和 (x_{k-1}) 独立且都是高斯，我们可以直接写出预测分布：

[
\hat{x}k^- = A \hat{x} + B u_k
]

[
P_k^- = A P_{k-1} A^T + Q
]

上标“-”表示这是“先验估计”（没有用到当前观测的估计）。这里的逻辑就是协方差的线性传播公式：线性变换会左乘矩阵和右乘转置，同时加上过程噪声带来的不确定性。

4.2 更新步：推导卡尔曼增益的由来

现在来了一个观测 (z_k)，我们希望已知观测的条件下修正对 (x_k) 的估计。我们将 (x_k) 和 (z_k) 的联合分布写出来。第一步的预测已经告诉我们：

[
\mathbb{E}[x_k] = \hat{x}_k^-, \quad \mathrm{Cov}(x_k) = P_k^-
]

观测模型是 (z_k = H x_k + v_k)，所以：

[
\mathbb{E}[z_k] = H \hat{x}_k^- , \quad \mathrm{Cov}(z_k) = H P_k^- H^T + R
]

协方差部分则因为 (x_k) 和 (v_k) 独立而交叉项为零：

[
\mathrm{Cov}(x_k, z_k) = P_k^- H^T
]

把这三个量拼成联合高斯分布，再利用上面条件分布公式，就能得到更新后的后验分布。这里“求条件分布”这一步骤对应到贝叶斯滤波的“更新步”。为了少写一点矩阵符号，直接翻译成五条公式：

预测：

[
\hat{x}k^- = A \hat{x} + B u_k
]

[
P_k^- = A P_{k-1} A^T + Q
]

更新：

[
K_k = P_k^- H^T \left( H P_k^- H^T + R \right)^{-1}
]

[
\hat{x}_k = \hat{x}_k^- + K_k \left( z_k - H \hat{x}_k^- \right)
]

[
P_k = \left( I - K_k H \right) P_k^-
]

这五条公式就是卡尔曼滤波的全部核心。注意第3条里的 (K_k) 叫卡尔曼增益，它决定了我们多信任预测、多信任观测。这里 (z_k - H\hat{x}_k^-) 叫新息，表示“观测和预期观测的差异”。如果 (R) 很大说明观测噪声大，(K_k) 就会变小，滤波结果更偏向预测；如果 (R) 很小说明观测很准，(K_k) 变大，结果更偏向观测。卡尔曼增益在数学上等价于在预测和观测之间做一个最优加权平均。

提示：我看见不少人把卡尔曼滤波简写成“先预测、再更新”，这没有错，但欠缺了关键一步——计算卡尔曼增益。这个增益才是卡尔曼滤波里真正“聪明”的地方，它根据不确定性的比例自适应地决定信任谁。

4.3 五条公式背后的贝叶斯直觉

用贝叶斯的话说，预测步就是在求 (P(x_k|z_{1:k-1}))，更新步就是在算 (P(x_k|z_{1:k}))。卡尔曼滤波只是把这个贝叶斯框架在“线性高斯”前提下全部化简成了矩阵运算，并用均值和协方差两个数字代表了整个概率分布。

所以“卡尔曼滤波是贝叶斯滤波的特例”这句话，可以更准确地说成：卡尔曼滤波是贝叶斯滤波在线性高斯条件下的解析解。它没有抛弃概率视角，而是用概率视角推导出来的具体算法。这也解释了为什么卡尔曼滤波的前提那么严格——一旦系统非线性，或者噪声不是高斯，这套公式就不再是最优解，需要扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）来近似。系列后面的文章会详细对比两者。

5. 从公式到代码：一维示例的完整实现

5.1 核心实现架构

为了方便理解，我用一个最简单的一维位置估计问题来演示。假设你有一个一维匀速运动的物体，可以用两个状态表示：位置 (p) 和速度 (v)。那么状态向量是：

[
x = \begin{bmatrix} p \ v \end{bmatrix}
]

离散化的状态转移矩阵是：

[
A = \begin{bmatrix} 1 & \Delta t \ 0 & 1 \end{bmatrix}
]

观测矩阵是：

[
H = \begin{bmatrix} 1 & 0 \end{bmatrix}
]

即我们只能直接观测位置，测不到速度。下面这段代码用纯 Python 和 NumPy 实现，重点在于把五条公式原样翻译成矩阵运算，不引入任何现成的卡尔曼滤波库。

python复制import numpy as np

class SimpleKalmanFilter:
    def __init__(self, dt, initial_state, initial_cov, process_noise_std, measurement_noise_std):
        self.dt = dt
        self.x = np.array(initial_state, dtype=float)  # 状态 [位置, 速度]
        self.P = np.array(initial_cov, dtype=float)    # 协方差矩阵
        self.A = np.array([[1, dt],
                           [0, 1]], dtype=float)       # 状态转移矩阵
        self.H = np.array([[1, 0]], dtype=float)       # 观测矩阵，只观测位置
        self.Q = np.array([[0, 0],
                           [0, process_noise_std**2]], dtype=float)  # 过程噪声协方差
        self.R = np.array([[measurement_noise_std**2]], dtype=float) # 观测噪声协方差

    def predict(self):
        self.x = self.A @ self.x
        self.P = self.A @ self.P @ self.A.T + self.Q
        return self.x, self.P

    def update(self, z):
        # 卡尔曼增益
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)

        # 新息
        innovation = z - self.H @ self.x

        # 状态和协方差更新
        self.x = self.x + K @ innovation
        self.P = (np.eye(2) - K @ self.H) @ self.P
        return self.x, self.P

    def step(self, z):
        self.predict()
        self.update(z)
        return self.x, self.P

在这段简单的代码里，有几个细节值得说明。

设初始状态为 ([0, 0])，初始协方差矩阵为 (\text{diag}(1, 1))，表示我们不太确定起点在哪。process_noise_std 设置成 0.1，表示我们相信匀速模型，但0.1的速度噪声还是要的；measurement_noise_std 设置成 1.0，表示观测噪声较大。这样设置的好处是，滤波结果会明显表现出“平滑”观测的效果。

5.2 仿真实验：滤波效果对比

现在来做一个仿真实验。真实物体以每秒 1 米的速度匀速运动，采样间隔为 0.1 秒，观测噪声标准差设成 1 米。我们用上面滤波器跑 10 秒，共 100 步，来验证滤波效果。

python复制import matplotlib.pyplot as plt

# 仿真参数
dt = 0.1
true_velocity = 1.0
process_noise_std = 0.1
measurement_noise_std = 1.0

# 生成真值和带噪声的观测
true_positions = []
measurements = []
filtered_positions = []

# 状态: [位置, 速度]
true_state = np.array([0.0, true_velocity])
kf = SimpleKalmanFilter(
    dt=dt,
    initial_state=[0.0, true_velocity],
    initial_cov=[[1, 0], [0, 1]],
    process_noise_std=process_noise_std,
    measurement_noise_std=measurement_noise_std
)

np.random.seed(42)
for i in range(100):
    true_state[0] += true_velocity * dt
    true_positions.append(true_state[0])

    # 观测位置时加上高斯噪声
    z = true_state[0] + np.random.normal(0, measurement_noise_std, size=1)
    measurements.append(z[0])

    # 滤波
    filtered_state, _ = kf.step(z)
    filtered_positions.append(filtered_state[0])

# 画图比较真实位置、观测位置、滤波后位置
plt.figure(figsize=(10, 5))
plt.plot(true_positions, 'k-', label='True position')
plt.plot(measurements, 'r.', alpha=0.4, label='Noisy measurements')
plt.plot(filtered_positions, 'b-', label='Kalman filtered')
plt.legend()
plt.xlabel('Time step')
plt.ylabel('Position (m)')
plt.title('1D Kalman Filter: Estimating position from noisy measurements')
plt.grid(True, alpha=0.3)
plt.show()

运行代码出来的效果，通常是红点（观测值）上下乱跳，而滤波后的蓝线很平滑地贴近真实黑线。这正是卡尔曼滤波在起作用的直观证据。

5.3 速度估计：滤波器的“隐藏福利”

这个例子里容易被人忽略的一点是：我们虽然只观测了位置，但状态里同时还估计出了速度。也就是说，卡尔曼滤波通过模型耦合，自动帮你从带噪声的位置观测中“反推”出了速度变量。这在工程上非常实用——比如通过位置传感器估计速度时，不需要单独做微分（微分会放大噪声），直接用滤波器的速度估计就行。

我在实际项目里经常遇到类似需求：磁编码器只给出角度，但控制环需要角速度。如果直接对角度做差分，角度噪声会被高频放大；如果你在卡尔曼滤波状态里加入角速度变量，再用角度做观测，角速度估计会平滑得多。这种“间接测量”的效果是卡尔曼滤波一个极为好用的隐藏特性。

6. 实际工程中的常见坑与排查技巧

6.1 协方差发散问题

刚上手卡尔曼滤波的人，最常遇到的坑是滤波结果发散，也就是估计值越偏越远、完全不跟观测走。原因通常是 (Q) 和 (R) 设置不合理。

假如 (Q) 设得太小，滤波器就会非常“自信”地认为模型预测是准确的，于是几乎不理会观测。一旦模型有偏差（比如物体突然加速），滤波结果就会长期偏在真值外，看起来就是“发散”。反过来，(R) 设得太大也有类似效果。

排查思路很简单：先打印每一步的卡尔曼增益 (K) 和协方差矩阵 (P)。如果 (K) 接近 0，说明滤波器几乎不听观测，问题多半在 (Q) 太小；如果 (K) 接近 1，说明滤波器完全跟着观测走，本来就失去了滤波的意义。把 (K) 曲线画出来，问题一目了然。

6.2 初始协方差选多大合适

初始协方差 (P_0) 代表我们对初始状态的信任程度。设太大会导致前几步波动剧烈，设太小可能导致滤波器前期“过于自信”而收敛到错误位置。我的经验是，除非你有非常确定的初始位置和速度，否则可以适度高估 (P_0)，让滤波器在一开始快速修正。

举个例子，如果你用 GPS 初始化，位置误差可能有几米，那 (P_0) 的位置对角线至少设置成 (10^2) 的量级，而不是 (0.1^2)。前期几帧的抖动不要害怕，那是滤波器在“学习”。

6.3 数值稳定性细节

在实际工程代码中，乘法和求逆的数值稳定性也需要关注。协方差矩阵 (P) 因为浮点误差会慢慢失去对称正定性，所以很多工程实现每隔若干步会强制执行 (P \leftarrow (P + P^T)/2) 来保持对称。更高阶的做法是使用平方根卡尔曼滤波，用 Cholesky 分解来更新协方差，以保持数值稳定性。

在嵌入式C语言实现中，还需要注意：矩阵求逆要避免直接计算逆矩阵再相乘，尽量用求解线性方程的方式代替，这样既稳定又省时间。关于C语言实现的分步细节，后续系列会单独开一篇，这里先指出方向。

7. 卡尔曼滤波背后的“作者视角”：为什么这篇这样写

写这篇博客的时候，我心里一直记着一件小事：很多同学第一次学卡尔曼滤波，都是抱着“我马上就能用”的心态去搜教程，但搜到的内容要么是纯公式复制粘贴，要么是只给代码不讲原理的工具文。真正把两者接起来的内容很少。

所以这次我在组织系列时，刻意做了一件事：把“概率推导”和“矩阵运算”用一条主线索串起来。从贝叶斯滤波到卡尔曼滤波，不是一个知识跳跃，而是一个假设一个假设地去掉积分负担。整个过程中没有新概念，只是在做“线性化”和“高斯化”两个操作。看懂了这一点，后续学扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）、粒子滤波（Particle Filter）时，你会自然而然地知道它们各自放宽了哪些假设、代价是什么。

我在实际做传感器融合时也反复体会到这个道理：不懂原理直接调参，等于蒙着眼睛试；懂原理之后，哪怕代码里某个矩阵算错了，你也能从 P 的怪异变化倒推出来问题出在哪个环节。

最后分享一个我自己的小技巧：初学卡尔曼滤波时，千万别直接看多维矩阵形式。先在纸上把一维的情况从贝叶斯公式一步步推到五条公式，拿真实数据跑一跑，看看 K 的变化规律，然后再推广到多维。我在带人的时候，发现这条路比任何花哨的PPT都管用。