4、概率论基础：贝叶斯公式、高斯分布、卡尔曼滤波思想、马尔可夫假设

做地图构建，说白了就是在跟不确定性打交道。

你拿到的传感器数据有噪声，机器人的轮子会打滑，甚至你放的二维码标签都可能被遮挡。那怎么办？概率论就是用来处理这些「不确定」的数学工具。我个人习惯，每次开始一个新项目，第一件事不是调代码，而是先把概率模型捋一遍——这能省下后面80%的调试时间。

贝叶斯公式是整个状态估计的基石。它解决了一个核心问题：有了新的观测数据，我该怎么修正之前的判断？

公式长这样：

P(A|B) = P(B|A) * P(A) / P(B)

拆开来看：

举个例子。你在建图时，机器人前方有个门。你事先觉得「门是开着的」概率是50%（先验）。然后激光雷达扫了一下，回波距离是2米。如果门开着，激光应该打到远处，2米这个值出现的概率很低（似然小）。如果门关着，2米就很合理（似然大）。

贝叶斯公式帮你算出来：后验概率 = 先验 × 似然 / 证据。证据P(B)就是个归一化常数，保证概率加起来等于1。

核心理解：贝叶斯公式不是一次性算完就拉倒。它是迭代的——这次的后验，就是下一次的先验。你每拿到一帧数据，就更新一次信念。这就是递归贝叶斯滤波的思想。

我在项目中遇到过一个问题：用贝叶斯更新时，如果先验设得太极端（比如99%），新数据很难把它拉回来。嗯，这里要注意——先验不要拍脑袋，最好用历史数据或经验值。

为什么到处都用高斯分布？两个原因：

一维高斯分布公式：

p(x) = (1 / sqrt(2πσ²)) * exp(-(x-μ)² / (2σ²))

其中μ是均值，σ²是方差。μ告诉你「大概在哪」，σ²告诉你「有多不确定」。

到了多维情况，我们用协方差矩阵Σ来描述各个维度之间的相关性。比如机器人的位置(x, y, θ)，x和y的误差可能是相关的——你往右走时，里程计误差同时影响x和y。

实战技巧：我建议你在调试SLAM时，把协方差矩阵可视化出来。如果某个维度的方差突然变大，说明传感器数据质量下降了，或者运动模型出问题了。我曾经靠这个发现了一个IMU的安装松动问题。

卡尔曼滤波不是黑魔法。它就是个两步走的过程：

第一步：预测 —— 根据运动模型，猜一下机器人现在该在哪。

x_pred = A * x_prev + B * u
P_pred = A * P_prev * A^T + Q

x是状态（位置、速度等），A是状态转移矩阵，u是控制输入，Q是过程噪声协方差。

第二步：更新 —— 拿到观测数据，修正预测结果。

K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_upd = x_pred + K * (z - H * x_pred)
P_upd = (I - K * H) * P_pred

K是卡尔曼增益，H是观测矩阵，R是观测噪声协方差。

你想想看，卡尔曼增益K决定了「你更相信预测还是观测」。如果观测噪声R很小（传感器很准），K就大，更新时更依赖观测。反过来，如果过程噪声Q很小（运动模型很准），K就小，更依赖预测。

一句话总结：卡尔曼滤波 = 高斯分布下的贝叶斯滤波。它假设所有噪声都是高斯分布，所有模型都是线性的。如果模型是非线性的，那就用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）。

我曾经在AGV小车上跑EKF，结果发现协方差矩阵发散——算出来的方差越来越大。排查了半天，发现是过程噪声Q设得太小了，模型过于自信。调大Q之后，滤波就稳定了。避坑指南：Q和R的调参，别靠猜，用实际数据去标定。

马尔可夫假设说：当前状态包含了预测未来所需的所有信息，过去的历史可以丢掉。

用公式表达：

P(x_t | x_{t-1}, x_{t-2}, ..., x_0) = P(x_t | x_{t-1})

说白了，就是「未来只跟现在有关，跟过去无关」。这个假设在SLAM里几乎无处不在：

注意：马尔可夫假设是个强假设。如果机器人的运动有长期相关性（比如轮胎磨损导致偏差逐渐增大），那这个假设就不成立。实际工程中，我们经常用「滑动窗口」来近似——保留最近N步的状态，丢掉更早的。这是一种折中。

我个人习惯，在写SLAM代码时，会在注释里明确标出「这里用了马尔可夫假设」。这样半年后回头看代码，能快速理解当时的建模思路。

下面这张图，把四个知识点的关系串起来了：

你看这张图就明白了：贝叶斯公式是顶层框架，高斯分布是底层工具，卡尔曼滤波是具体算法，马尔可夫假设是简化前提。四者缺一不可。

实际做地图构建时，你几乎每天都在跟这四样东西打交道。建图就是不断「预测→观测→更新」的循环，而每一步背后，都是概率论在支撑。

给新手的建议：别一上来就啃卡尔曼滤波的公式推导。先理解贝叶斯思想，再搞懂高斯分布的性质，然后看卡尔曼滤波的预测-更新流程。最后，记住马尔可夫假设的适用边界。按这个顺序学，事半功倍。

可可精品资料库