4、卡尔曼滤波入门：状态估计问题、贝叶斯滤波、标准卡尔曼滤波推导

4.1 状态估计问题——我们在解决什么？

做多传感器融合，说白了就是回答一个问题：“我现在到底在哪儿？”

你拿着GPS、IMU、轮速计，每个传感器都在告诉你一个位置。但每个数据都有噪声。GPS可能飘了半米，IMU积分久了会漂，轮速计在打滑时完全不准。那到底信谁？

这就是状态估计问题的核心：从带噪声的观测数据中，反推出系统真实的状态。

我刚开始做定位时，总觉得“多传感器嘛，取个平均不就行了？”结果在真实场景里一跑，车在弯道上直接偏出去两米。后来才明白——不同传感器的噪声特性完全不同，简单平均等于把好数据和坏数据一视同仁。

状态估计的数学描述其实很简洁：

已知：
- 系统模型：x_k = f(x_{k-1}, u_k) + w_k
- 观测模型：z_k = h(x_k) + v_k
- 其中 w_k 和 v_k 是噪声

目标：
- 根据历史观测 z_{1:k}，估计当前状态 x_k

这里的 x_k 就是我们要估计的状态，比如位置、速度、姿态。z_k 是传感器观测，比如GPS坐标、IMU加速度。

4.2 贝叶斯滤波——一切滤波器的老祖宗

如果你只记住一件事，那就是：所有滤波器，本质上都是贝叶斯公式的递归应用。

贝叶斯公式长这样：

P(x|z) = P(z|x) * P(x) / P(z)

翻译成人话：

• 先验 P(x)：在拿到观测之前，我对状态的猜测
• 似然 P(z|x)：如果状态是x，观测到z的可能性有多大
• 后验 P(x|z)：结合观测后，我对状态的新认知

贝叶斯滤波就是把这个过程递归起来：

1. 预测步：用上一时刻的后验，预测当前时刻的先验
2. 更新步：用当前观测，修正先验得到后验

我在项目中遇到过一个问题：用贝叶斯滤波做纯视觉定位，结果状态空间太大（每个像素都要估计深度），计算量直接爆炸。后来才意识到——贝叶斯滤波是理论框架，实际用必须做近似。

4.3 标准卡尔曼滤波推导——从理论到代码

卡尔曼滤波是贝叶斯滤波在线性高斯系统下的特例。为什么它这么火？因为计算简单、效果又好。

假设系统是线性的：

状态方程：x_k = A * x_{k-1} + B * u_k + w_k
观测方程：z_k = H * x_k + v_k

其中：
- w_k ~ N(0, Q)  过程噪声
- v_k ~ N(0, R)  观测噪声

卡尔曼滤波的核心就5个公式，我建议你手抄一遍：

预测步（Time Update）

1. 状态预测：x_pred = A * x_prev + B * u
2. 协方差预测：P_pred = A * P_prev * A^T + Q

更新步（Measurement Update）

3. 卡尔曼增益：K = P_pred * H^T * (H * P_pred * H^T + R)^{-1}
4. 状态更新：x_est = x_pred + K * (z - H * x_pred)
5. 协方差更新：P_est = (I - K * H) * P_pred

嗯，这里要注意——卡尔曼增益 K 是整个滤波器的灵魂。它决定了你更相信预测还是更相信观测。

• 如果观测噪声 R 很大（传感器不准），K 会变小，更相信预测
• 如果过程噪声 Q 很大（模型不准），K 会变大，更相信观测

我曾经在一个AGV项目里，把Q设得太小，结果车子过减速带时IMU剧烈抖动，滤波器死活不更新，位置直接飞了。后来把Q调大一点，立马稳了。调参这事，真得靠经验。

4.4 卡尔曼滤波的直观理解

你想想看，卡尔曼滤波其实就是在做一件事：加权平均。

预测告诉你“我觉得状态在5”，观测告诉你“我看到状态在7”。你信谁？卡尔曼滤波说：我算一个权重，谁的噪声小就信谁多一点。

这个权重就是卡尔曼增益 K。它本质上是一个最优加权系数，使得估计误差的方差最小。

我习惯用一维的例子来理解：

假设：
- 预测值：5，方差 2
- 观测值：7，方差 1

卡尔曼增益 K = 2 / (2 + 1) = 0.667
最优估计 = 5 + 0.667 * (7 - 5) = 6.333

你看，观测方差小（更可信），所以估计值更靠近观测。

4.5 本章知识体系

下面这张图总结了本章的核心逻辑链条：

4.6 本章小结

这一章我们走完了从问题到算法的完整链路：

• 状态估计：在不确定中寻找确定，本质是概率推理
• 贝叶斯滤波：递归的“先猜后修”框架，是所有滤波器的理论基础
• 卡尔曼滤波：在线性高斯假设下，用5个公式实现最优估计

我个人觉得，理解卡尔曼滤波的关键不在于背公式，而在于理解它背后的概率思维。你不需要知道状态的确切值，你只需要知道它落在某个区间的概率有多大。然后，用观测来不断缩小这个区间。

下一章我们会把卡尔曼滤波真正用到多传感器融合中，看看GPS和IMU是怎么“吵架”又“和好”的。