第1章：状态估计基础——贝叶斯滤波框架
大家好，欢迎来到粒子滤波实战课程。
今天咱们聊点基础的东西——贝叶斯滤波。说实话，很多做SLAM的朋友一上来就撸代码，结果滤波器调参调到怀疑人生。我个人习惯是，先把框架理清楚，再动手。
1.1 为什么需要状态估计？
你想想看，机器人身上装了一堆传感器——激光雷达、IMU、轮式里程计。每个传感器都有噪声。激光雷达测距有高斯噪声？不一定。IMU有零偏漂移？肯定的。轮式里程计打滑？经常的。
所以问题来了：我们怎么从这些带噪声的数据里，还原出机器人的真实状态？
这就是状态估计要干的事。
核心思想：用概率分布来描述状态的不确定性。不是猜一个值，而是给出一堆可能的值，以及每个值的可信度。
我在做AGV项目时遇到过这种情况：激光雷达在仓库里被货架遮挡，数据断断续续。如果只用单帧数据做定位，那结果简直没法看。后来我改用贝叶斯滤波框架，把历史信息也融合进来，效果就好多了。
1.2 贝叶斯滤波框架
贝叶斯滤波说白了就两件事：预测和更新。
咱们用数学语言描述一下。设状态为 \(x_k\)，观测为 \(z_k\)。贝叶斯滤波的核心公式是：
p(x_k | z_{1:k}) ∝ p(z_k | x_k) · p(x_k | z_{1:k-1})这个公式看着简单，但内涵很深。我来拆解一下：
- 先验分布 \(p(x_k | z_{1:k-1})\)：根据上一时刻的信息，猜当前状态可能在哪
- 似然函数 \(p(z_k | x_k)\)：如果状态是某个值，观测到当前数据的可能性有多大
- 后验分布 \(p(x_k | z_{1:k})\)：融合了先验和观测后的最终结果
嗯，这里要注意：后验正比于先验乘以似然。那个归一化常数 \(p(z_k | z_{1:k-1})\) 被我省略了，因为它只负责让概率加起来等于1，不影响分布的形状。
我的经验：在实际工程中，归一化常数往往是最难算的。粒子滤波之所以受欢迎，就是因为它巧妙地绕过了这个计算。
1.3 预测与更新步骤
咱们把贝叶斯滤波拆成两步走。
预测步骤
预测就是根据运动模型，从上一时刻的后验推出当前时刻的先验。公式是：
p(x_k | z_{1:k-1}) = ∫ 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})\)：如果机器人从某个位置出发，下一步可能去哪
- 积分：把所有可能性加起来
我曾经犯过一个错误：在预测步骤里用了太简单的运动模型，结果机器人转弯时预测完全跟不上。后来我加了IMU数据做辅助，预测才靠谱起来。
更新步骤
更新就是拿到观测数据后，修正预测的结果。公式是：
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.4 递归贝叶斯估计
递归贝叶斯估计的精髓就四个字：迭代递推。
你想想看，如果每来一帧数据，我们都要从头算一遍所有历史数据，那计算量会爆炸。递归的思想是：只保留上一时刻的后验分布，然后结合当前观测，递推出当前的后验。
流程是这样的：
- 初始化：\(p(x_0)\) —— 初始状态分布
- 预测：\(p(x_k | z_{1:k-1}) = ∫ p(x_k | x_{k-1}) · p(x_{k-1} | z_{1:k-1}) dx_{k-1}\)
- 更新：\(p(x_k | z_{1:k}) ∝ p(z_k | x_k) · p(x_k | z_{1:k-1})\)
- 回到第2步，继续下一时刻
这个框架有个巨大的优势：计算量恒定。不管跑了多久，每次迭代的计算量都一样。这在嵌入式系统里特别重要。
关键点：递归贝叶斯估计假设系统是马尔可夫的——当前状态只依赖于上一时刻的状态，与更早的历史无关。这个假设在大多数SLAM场景下是合理的。
1.5 为什么需要粒子滤波？
讲到这里，你可能要问了：既然贝叶斯滤波框架这么完美，为什么还要搞粒子滤波？
原因很简单：积分算不出来。
你看预测步骤里的那个积分，还有更新步骤里的归一化常数。这些积分在以下情况下根本没法解析求解：
- 运动模型是非线性的（比如带转弯的机器人）
- 观测模型是非线性的（比如激光雷达的极坐标到笛卡尔坐标转换）
- 噪声是非高斯的（比如有离群点的传感器数据）
卡尔曼滤波只能处理线性高斯的情况。扩展卡尔曼滤波虽然能处理非线性，但噪声还得是高斯。一旦遇到非高斯噪声，就得请出粒子滤波了。
我的建议：如果你的系统是线性高斯的，用卡尔曼滤波，又快又准。但如果你遇到非高斯噪声、多模态分布、或者强非线性，别犹豫，上粒子滤波。
1.6 本章小结
咱们捋一捋这章的核心：
| 概念 | 说明 |
|---|---|
| 贝叶斯滤波 | 用概率分布描述状态不确定性，融合先验和观测 |
| 预测步骤 | 根据运动模型，从上一时刻后验推出当前时刻先验 |
| 更新步骤 | 根据观测模型，用当前观测修正先验得到后验 |
| 递归贝叶斯 | 只保留上一时刻后验，递推计算，计算量恒定 |
| 粒子滤波动机 | 处理非线性、非高斯情况，解析积分算不出来时用 |
下一章，咱们正式进入粒子滤波的世界。我会从最基础的蒙特卡洛方法讲起，一步步推导出粒子滤波的完整流程。到时候，你会看到粒子滤波是怎么用一堆「粒子」来近似那些算不出来的分布的。
嗯，今天就到这儿。有问题欢迎在评论区交流。