4. 状态估计基础：贝叶斯滤波、卡尔曼滤波(KF)原理
各位同学，今天我们聊点硬核的——状态估计。说白了，就是机器人怎么在不知道自己到底在哪的情况下，猜出自己大概在哪。
我刚开始做导航那会儿，总觉得传感器数据是可靠的。直到有一次，GPS信号被高楼遮挡，IMU（惯性测量单元）又疯狂漂移，小车直接冲着花坛就去了。嗯，从那以后，我彻底明白了：没有完美的传感器，只有靠谱的估计算法。
4.1 为什么需要状态估计？
你想想看，在GPS拒止环境下，我们有什么？
- IMU：短时间准，长时间漂
- 激光雷达/视觉：特征匹配有误差
- 轮式里程计：打滑就完蛋
每个传感器都有噪声，每个测量都不完美。我们需要一个方法，把所有这些不完美的信息融合起来，得到一个相对靠谱的估计。这就是状态估计要干的事。
4.2 贝叶斯滤波——一切的基础
贝叶斯滤波，说白了就是一套递归的猜谜游戏。我们有两个关键步骤：
- 预测（Predict）：根据上一时刻的状态，猜一下现在在哪
- 更新（Update）：拿到传感器数据后，修正这个猜测
数学上，我们关心的是后验概率：
P(x_k | z_{1:k}) ∝ P(z_k | x_k) · P(x_k | z_{1:k-1})这个公式看着吓人，其实意思很简单：
后验 ∝ 似然 × 先验
我在项目中遇到过一个问题：直接用贝叶斯滤波做全状态空间搜索，计算量爆炸。因为真实世界的状态空间是连续的，你不可能枚举所有可能的位置。这时候就需要卡尔曼滤波出场了。
4.3 卡尔曼滤波——线性高斯下的最优解
卡尔曼滤波做了两个关键假设：
- 系统是线性的
- 噪声是高斯分布的
在这两个假设下，卡尔曼滤波给出了解析解，而且计算量极小。我当年第一次手写KF代码时，看着那5个公式，觉得这玩意儿也太优雅了。
4.3.1 卡尔曼滤波的五个核心公式
预测阶段：
1. 状态预测：x̂_k|k-1 = A · x̂_k-1|k-1 + B · u_k
2. 协方差预测：P_k|k-1 = A · P_k-1|k-1 · A^T + Q更新阶段：
3. 卡尔曼增益：K_k = P_k|k-1 · H^T · (H · P_k|k-1 · H^T + R)^(-1)
4. 状态更新：x̂_k|k = x̂_k|k-1 + K_k · (z_k - H · x̂_k|k-1)
5. 协方差更新：P_k|k = (I - K_k · H) · P_k|k-1看着公式多，其实逻辑很清晰：
- 先猜（预测），再量（测量），最后折中（更新）
- 卡尔曼增益K决定了你更相信模型还是更相信传感器
4.4 卡尔曼滤波的直观理解
我习惯用一个比喻来解释KF：
想象你在黑屋子里走路。你每走一步，会根据自己的步长猜一下位置（预测）。但你的步长不准，所以猜测有误差。突然你摸到了墙（测量），你知道墙的真实位置，于是修正自己的猜测。卡尔曼滤波就是帮你算出「该信自己多少，该信墙多少」的那个数学工具。
为什么叫「滤波」？因为它在有噪声的测量中，把真实信号「滤」出来了。
4.5 卡尔曼滤波的局限性
KF虽好，但不是万能的。我总结了几点：
| 问题 | 表现 | 怎么办 |
|---|---|---|
| 系统非线性 | KF假设线性，实际系统大多非线性 | 用EKF（扩展卡尔曼滤波）或UKF |
| 噪声非高斯 | 实际噪声可能有长尾或偏态 | 用粒子滤波或鲁棒卡尔曼滤波 |
| 高维状态空间 | 协方差矩阵太大，计算慢 | 用稀疏化或信息滤波形式 |
4.6 本章知识体系
下面这张图，是我自己梳理的本章知识脉络。你看完应该能明白贝叶斯滤波和卡尔曼滤波的关系：
4.7 代码示例：一维卡尔曼滤波
光说不练假把式。我写了个最简单的一维KF例子，帮你理解整个过程：
import numpy as np
# 初始化
x_hat = 0.0 # 初始状态估计
P = 1.0 # 初始协方差
Q = 0.1 # 过程噪声协方差
R = 0.5 # 测量噪声协方差
A = 1.0 # 状态转移矩阵（一维匀速模型）
H = 1.0 # 观测矩阵
# 模拟测量数据
measurements = [1.2, 1.8, 2.5, 3.1, 3.9]
for z in measurements:
# 预测
x_pred = A * x_hat
P_pred = A * P * A + Q
# 更新
K = P_pred * H / (H * P_pred * H + R)
x_hat = x_pred + K * (z - H * x_pred)
P = (1 - K * H) * P_pred
print(f"测量值: {z:.2f}, 估计值: {x_hat:.2f}, 协方差: {P:.4f}")运行这段代码，你会发现估计值逐渐收敛到真实轨迹附近。这就是KF的魔力——用不完美的测量，得到完美的估计。
4.8 本章小结
贝叶斯滤波给了我们一个理论框架，卡尔曼滤波给了我们一个工程工具。在GPS拒止环境下，KF及其变种是状态估计的基石。你想想看，没有它们，我们连小车在哪都不知道，还谈什么自主导航？
嗯，这一章就到这。记住：估计的本质，是在不确定中寻找确定。
公众号:蓝海资料掘金营，微信deep3321