第3章 状态估计基础：贝叶斯滤波、卡尔曼滤波原理与核心公式推导
各位同学，欢迎来到《融合跟踪算法实战教程》的第三讲。
今天我们要啃一块硬骨头——状态估计。说白了，就是怎么从一堆带噪声的观测数据里，把真实的状态给“猜”出来。你想想看，雷达测目标，传感器有噪声，GPS有漂移，我们怎么知道目标到底在哪？
嗯，这就是状态估计要解决的问题。我个人习惯把这一章看作是整个跟踪算法的“内功心法”。内功练好了，后面学什么融合、什么滤波，都顺风顺水。
3.1 贝叶斯滤波：一切估计的哲学基础
先聊贝叶斯滤波。别被名字吓到，它的核心思想其实就一句话：用先验知识 + 当前观测 = 后验估计。
我在项目中遇到过不少新手，上来就调卡尔曼参数，结果调了半天效果还是差。为什么？因为没理解贝叶斯滤波的本质——你是在用概率的思维做推理。
贝叶斯滤波的数学形式很简单：
P(x_k | z_{1:k}) = [P(z_k | x_k) * P(x_k | z_{1:k-1})] / P(z_k | z_{1:k-1})拆开来看：
- P(x_k | z_{1:k})：后验概率。这是我们最终想要的——知道所有观测后，对当前状态的信念。
- P(z_k | x_k)：似然。给定状态，观测有多大概率出现？说白了就是传感器的模型。
- P(x_k | z_{1:k-1})：先验。根据上一时刻的信息，预测当前状态会是什么样。
- P(z_k | z_{1:k-1})：归一化常数。保证概率加起来等于1。
核心理解：贝叶斯滤波就是一个“预测-更新”的循环。预测阶段用系统模型推先验，更新阶段用观测数据修正后验。周而复始，步步逼近真实状态。
我曾经犯过一个错误：在非线性系统中硬套线性贝叶斯滤波，结果发散得一塌糊涂。后来才明白，贝叶斯滤波只是一个框架，具体怎么实现，要看你的系统是线性还是非线性，噪声是高斯的还是非高斯的。
3.2 卡尔曼滤波：线性高斯世界里的最优解
卡尔曼滤波（KF）是贝叶斯滤波在线性系统 + 高斯噪声假设下的特例。为什么它这么出名？因为它有闭式解，计算效率高，而且在线性高斯条件下是最优的。
说白了，卡尔曼滤波就是贝叶斯滤波的“学霸版”——条件苛刻，但一旦满足，表现无敌。
卡尔曼滤波假设系统是这样的：
状态方程：x_k = F * x_{k-1} + B * u_k + w_k
观测方程：z_k = H * x_k + v_k其中：
- F：状态转移矩阵。描述状态怎么随时间演变。
- B：控制输入矩阵。如果有外部控制（比如方向盘转角），就用它。
- u_k：控制向量。
- w_k：过程噪声，服从 N(0, Q)。
- H：观测矩阵。把状态空间映射到观测空间。
- v_k：观测噪声，服从 N(0, R)。
个人经验：Q和R这两个协方差矩阵，是卡尔曼滤波里最玄学的地方。我刚开始做项目时，Q设得太小，滤波器反应迟钝；R设得太小，又跟着噪声乱跳。后来我总结了一个经验：先用物理模型估算Q，再用传感器标定数据估算R，最后在实际数据上微调。
3.3 卡尔曼滤波的五个核心公式推导
好，重头戏来了。卡尔曼滤波的五个公式，我建议你手推一遍。为什么？因为只有自己推过，才能真正理解每个变量在干什么。
我们一步步来。
3.3.1 预测阶段（两个公式）
公式1：状态预测
x̂_k|k-1 = F * x̂_{k-1|k-1} + B * u_k这个公式很简单：用上一时刻的最优估计，乘以状态转移矩阵，加上控制输入，就得到当前时刻的预测状态。
公式2：协方差预测
P_k|k-1 = F * P_{k-1|k-1} * F^T + Q协方差代表不确定性。预测会让不确定性变大——因为过程噪声Q的存在。你想想看，预测得越远，不确定性越大，这是符合直觉的。
3.3.2 更新阶段（三个公式）
公式3：卡尔曼增益
K_k = P_k|k-1 * H^T * (H * P_k|k-1 * H^T + R)^{-1}这是整个卡尔曼滤波的灵魂。卡尔曼增益K决定了：你更相信预测，还是更相信观测？
- 如果观测噪声R很小（传感器很准），K会变大，滤波器更相信观测。
- 如果过程噪声Q很小（模型很准），P会变小，K会变小，滤波器更相信预测。
公式4：状态更新
x̂_k|k = x̂_k|k-1 + K_k * (z_k - H * x̂_k|k-1)括号里的 z_k - H * x̂_k|k-1 叫新息（innovation），也就是实际观测和预测观测的差异。用K乘以这个差异，加到预测状态上，就得到更新后的状态。
公式5：协方差更新
P_k|k = (I - K_k * H) * P_k|k-1更新后，不确定性降低了。因为观测提供了新信息，让我们的估计更确定。
五个公式一句话总结：先预测状态和协方差，再算增益，然后用观测修正状态，最后更新协方差。循环往复，这就是卡尔曼滤波的全部。
3.4 核心逻辑流程图
为了让你更直观地理解整个流程，我画了一张图。嗯，这张图我当年学的时候也画过，贴在工位上看了整整一周。
3.5 一个简单的数值例子
光讲理论太枯燥，我们来看个具体例子。假设我们要估计一维匀速运动物体的位置和速度。
系统参数：
F = [[1, dt], [0, 1]] # dt = 0.1s
H = [[1, 0]] # 只观测位置
Q = [[0.01, 0], [0, 0.01]]
R = [[0.1]]
初始状态：x̂₀ = [0, 1]ᵀ, P₀ = [[1, 0], [0, 1]]假设真实位置是匀速运动，初始位置0，速度1 m/s。我们模拟一下：
import numpy as np
# 参数设置
dt = 0.1
F = np.array([[1, dt], [0, 1]])
H = np.array([[1, 0]])
Q = np.array([[0.01, 0], [0, 0.01]])
R = np.array([[0.1]])
# 初始化
x_hat = np.array([[0], [1]]) # 位置, 速度
P = np.eye(2)
# 模拟10步
for k in range(10):
# 预测
x_pred = F @ x_hat
P_pred = F @ P @ F.T + Q
# 模拟观测（真实位置 + 噪声）
z_true = k * dt # 真实位置
z = z_true + np.random.normal(0, np.sqrt(R[0,0]))
# 更新
K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R)
x_hat = x_pred + K @ (z - H @ x_pred)
P = (np.eye(2) - K @ H) @ P_pred
print(f"Step {k}: 位置估计={x_hat[0,0]:.3f}, 速度估计={x_hat[1,0]:.3f}")避坑指南：我曾经在写代码时忘记转置矩阵，结果卡尔曼增益算出来维度不对，程序直接报错。嗯，矩阵运算的维度检查，一定要养成习惯。另外，P矩阵必须保持对称正定，数值误差可能导致它不对称，必要时可以强制对称化：P = (P + P.T) / 2。
3.6 卡尔曼滤波的适用条件与局限
| 条件 | 说明 | 不满足怎么办 |
|---|---|---|
| 线性系统 | 状态方程和观测方程都是线性的 | 用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF） |
| 高斯噪声 | 过程噪声和观测噪声都服从高斯分布 | 用粒子滤波（PF）或高斯和滤波 |
| 噪声统计已知 | Q和R矩阵已知且不随时间变化 | 用自适应卡尔曼滤波（AKF）在线估计Q和R |
| 初始状态已知 | 需要合理的初始估计x̂₀和P₀ | 用多模型方法或增大初始P₀ |
说白了，卡尔曼滤波不是万能的。它在线性高斯条件下是最优的，但现实世界往往是非线性、非高斯的。这时候就需要我们后面要讲的扩展卡尔曼滤波、无迹卡尔曼滤波，甚至粒子滤波。
我的建议：初学者先把标准卡尔曼滤波吃透。我见过太多人一上来就搞EKF、UKF，结果连基本的协方差更新都写不对。先把五个公式手推三遍，再用代码实现一个一维例子，最后再扩展到多维。稳扎稳打，比什么都强。
好，这一章的内容就到这里。贝叶斯滤波给了我们哲学框架，卡尔曼滤波给了我们工程工具。下一章我们会把这些知识应用到实际的目标跟踪场景中。