从贝叶斯推断到SLAM的数学模型

SLAM的全称是Simultaneous Location And Mapping，即同时定位与建图。机器人要在未知的环境中实时的估算自己的运动、定位并建图。 想象一个移动机器人在未知环境中移动，使用位于机器人上的传感器对一些未知的地标进行观察，就如下图所示。

要描述SLAM问题，还得从贝叶斯推断说起。

贝叶斯推断

贝叶斯定理，实际上是从条件概率衍生而来的。所以我们先从条件概率公式说起。

首先我们要知道三个概念：

1. 条件概率，表示事件A在事件B发生的条件下发生的概率，用符号$P(A|B)$表示；
2. 联合概率，表示两个事件共同发生的概率，可表示为$P(A\cap B)$ 或者$P(A,B)$、$P(AB)$；
3. 边缘概率，表示某个事件发生的概率，事件A的边缘概率可表示为$P(A)$，事件B的边缘概率可表示为$P(B)$

设 A 与 B 为样本空间 Ω 中的两个事件，其中 P(B)>0。那么在事件 B 发生的条件下，事件 A 发生的条件概率为：
$$P(A|B)=\frac{P(A\cap B)}{P(B)}$$

用文氏图表达：

通过条件概率公式，有：
$$P(A\cap B)=P(A)P(B|A)=P(B)P(A|B)$$

故得到贝叶斯公式：
$$P(A|B)=\frac{P(B|A)P(A)}{P(B)}$$

贝叶斯定理（英语：Bayes’ theorem）是概率论中的一个定理，描述在已知一些条件下，某事件的发生概率。比如，如果已知某癌症与寿命有关，使用贝叶斯定理则可以通过得知某人年龄，来更加准确地计算出他罹患癌症的概率。

我们可以把各项称之为：
$P(A|B)$：后验概率
$P(A)$：先验概率
$P(B|A)$：似然率
$P(b)$：边缘似然率

一般来说，直接求后验概率会很难求，那么怎么办呢？我们可以求得右侧的概率来推断后验概率是多少。

举个例子，隔壁班有个妹子经常看见你就对你笑，你很喜欢这个妹子，但又担心表白失败，作为工科生，肯定要理智一点，想判断她是否喜欢你，那么产生了一个后验概率：$$P(妹子喜欢你|妹子经常对你笑)$$

直接得出这个概率是很难的，这个时候你需要通过贝叶斯公式：

$P(妹子喜欢你|妹子经常对你笑)=P(妹子喜欢你)\times \frac{P(妹子经常对你笑|妹子喜欢你)}{P(妹子经常对你笑)}$

假设妹子喜欢你的概率$P(妹子喜欢你)$是50%。

这时候你还不能表白，因为这个表白的话很容易被拒绝，这样就没朋友做了。这时候你就想，她天天对我笑，那她对其他人是不是也一直笑呢？于是你就偷偷打听，终于，你打听到了她很少对别人笑，这个打听到的信息即$\frac{P(妹子经常对你笑|妹子喜欢你)}{P(妹子经常对你笑)}$，我们又把这个称为可能性函数，假设这个数值为1.8。

那么，$P(妹子喜欢你|妹子经常对你笑)=P(妹子喜欢你)\times \frac{P(妹子经常对你笑|妹子喜欢你)}{P(妹子经常对你笑)}=0.5\times 1.8=0.9$

妹子在经常对你笑的前提下，喜欢你的概率是90%，还不快去表白？

贝叶斯推断的本质是求解某事的条件概率，我们可以把先验信息看成是旧的看法，可能性函数看成是新的信息，后验概率看成是新的看法。

有同学可能要问了……这个可能性函数是要怎么来的？说实话，我也在这里想了很久。假设我们要解决的是机器学习中的分类问题（朴素贝叶斯），那么我们往往会掌握一些数据，而可能性函数就是通过这些数据来计算的，朴素贝叶斯的核心就是求解贝叶斯公式右侧的所有概率，进而分类出最有可能发生的事件。

贝叶斯推断和朴素贝叶斯的区别在于，贝叶斯推断只是单纯推断某件事发生的概率，朴素贝叶斯是把所有可能的事件概率计算出来，取概率最大的当成分类的结果。

比如一共有$P(喜欢你|对你笑)$和$P(不喜欢你|对你笑)$两种后验概率，贝叶斯推断是计算其中一种后验概率发生的概率，而朴素贝叶斯是把两种都算了，然后返回概率大的事件给你，在上述的例子中，朴素贝叶斯有种决策的效果，它告诉你应该去表白。好了，貌似扯远了……
关于朴素贝叶斯可看此处

那这样我们就介绍完贝叶斯推断了。

SLAM数学问题简单描述

在SLAM问题上，可以用贝叶斯框架来进行描述（不然也不会费这么多话来解释贝叶斯推断了）。在SLAM中，我们可以为相机（或机器人）定义两个变量，一个是状态量，一个是观测量。分别用以下字母来进行表示：

状态变量：$x_{1:k}=(x_1,x_2\dots \dots ,x_k)$
观测量：$z_{1:k}=(z_1,z_2\dots \dots ,z_k)$

其中，状态量指的是每个时刻下相机的姿态、位置速度和三维点坐标，$x_k$专指$k$时刻的状态。
而观测量指的是在当前状态下所看到的特征点的像素坐标，同样，$z_k$专指$k$时刻的观测。

我们的任务是把相机的状态估计出来，这是一个状态估计问题，因此，我们可以将SLAM问题转化为求解后验概率的问题：
$$P(x_{1:k}|z_{1:k},x_0)$$

刚刚贝叶斯推断时已经说了，直接求后验概率比较困难，但是求一个状态最优估计，使得在该状态下，后验概率最大化（Maximize a Posterior，MAP）是可行的：

$$x_{MAP}^{\ast }=argmaxP(x_{1:k}|z_{1:k},x_0)=argmaxP(z_{1:k},x_0|x_{1:k})P(x_{1:k})$$

由于贝叶斯公式中右侧的分母与$x$状态无关，所以使后验概率最大化无需考虑其分母。

这里最大化后验概率意思就是最可能的状态是什么。

但由于我们一般只关心当前时刻的状态，所以又可以写成：
$$x_{MAP}^{\ast }=argmaxP(x_k|z_{1:k},x_0)=argmaxP(z_{1:k},x_0|x_k)P(x_k)$$

三句话概括SLAM问题：

SLAM问题是状态的估计问题，就是说我们要估计相机或空间点的状态。那怎么估计呢？
理论上要求的：在这个观测值的前提下，相机的状态最有可能是怎样的？
实际上求的：在这个相机状态下，最可能观测到什么。

可以看到从概率学角度来看，SLAM问题本质上是求使用贝叶斯公式求最大后验概率。

关于求解SLAM的问题，世间产生了两种较为主流的方式。

滤波方式

回到上述所说，，我们要求的是：$$P(x_{1:k}|z_{1:k},x_0)$$

由于我们只关心当前时刻的状态，又可写为：$$P(x_k|z_{1:k},x_0)=\iiint ...\int P(x_{1:k}|z_{1:k},x_0)d{x}_{1}d{x}_{2}d{x}_3...d{x}_{k-1}$$
这是一个边缘化的过程，相当于把之前的状态信息当成一个先验信息融合进来，把时刻1到k-1时刻的状态都marginalization掉。

但是这样会产生一个问题，计算量很大，于是我们可以在马尔科夫假设下，用迭代贝叶斯滤波去求解。

马尔卡夫的假设下，$P(x_k|x_{1:k-1})=P(x_k|x_{k-1})$

根据贝叶斯公式：$$P(x_k|z_{1:k},x_0)=\frac{P(z_k,x_k|z_{1:k-1},x_0)}{\int P(z_k,x_k|z_{1:k-1},x_0)d{x}_k}$$

由于分子是一个归一化常量，我们用不着管它。

于是又可以写成：
$$\begin{gathered} P(x_k|z_{1:k},x_0)=\frac{P(z_k,x_k|z_{1:k-1},x_0)}{\int P(z_k,x_k|z_{1:k-1},x_0)d{x}_k}\propto P(z_k,x_k|z_{1:k-1}) \\ =P(z_k|x_k)P(x_k|z_{1:k-1},x_0)=P(z_k|x_k){\int }_{x_{k-1}}P(x_{k-1}|z_{1:k},x_0)d{x}_{k-1} \\ =P(z_k|x_k){\int }_{x_{k-1}}P(x_k|x_{k-1})P(x_{k-1}|z_{1:k-1},x_0)d{x}_{k-1} \end{gathered}$$

$P(z_k|x_k)$表示观测模型
$P(x_k|x_{k-1})$表示预测模型
$P(x_{k-1}|z_{1:k-1},x_0)$表示上一时刻的状态分布

上述的式子就是贝叶斯滤波的公式。

贝叶斯滤波步骤总结：

第一步预测：从上一时刻状态分布预测当前时刻状态分布：
$$P(x_k|z_{1:k-1},x_0)={\int }_{x_{k-1}}P(x_k|x_{k-1})P(x_k|z_{1:k-1},x_0)d{x}_{k-1}$$

$P(x_k|z_{1:k-1},x_0)$表示预测值。

第二步更新：使用当前时刻观测量更新后验概率：
$$P(x_k|z_{1:k},x_0)\propto P(z_k|x_k)P(x_k|z_{1:k-1},x_0)$$

$P(x_k|z_{1:k},x_0)$表示后验概率
$P(z_k|x_k)$ 表示观测值
$P(x_k|z_{1:k-1},x_0)$表示预测值。

反复迭代。

有小伙子可能想问了，贝叶斯滤波和卡尔曼滤波有什么关系？卡尔曼滤波是贝叶斯滤波的特例，当状态分布、预测模型和观测模型均服从高斯分布，那么这样的滤波称为卡尔曼滤波。

为什么高斯分布会和SLAM扯上关系，就是因为有时候我们把这些模型假设成高斯分布。

关于卡尔曼滤波的工作方式可见：卡尔曼滤波

非线性优化方式

非线性优化方式的核心就在于批量优化和多次迭代。

与滤波方式不太一样的是，要求解：$P(x|z_{1:k},x_0)$，这里的状态x一般是多个时刻的，比如我们定义了很多关键帧，进行一次优化的时候是优化这多个时刻的状态，并且进行一次优化的时候会迭代很多次。

而滤波方式每一次优化只迭代一次。

下面直接放出我做的ppt好了- -，实在懒得敲latex。
可以看到我们把贝叶斯公式乘上一个-log，从而达到由最大化转最小化的作用。。

这时候，假设模型服从高斯分布。关于高斯分布的一些性质，可看此处：高斯分布

可以看到我们已经把SLAM问题转化成一个最小二乘问题了，接下来，我们使用某种非线性优化方法去解它，比如高斯牛顿法：

重复以上多次，直到增量接近于0，非线性优化就完成了，SLAM问题也就解决了。

参考文献：
https://zhuanlan.zhihu.com/p/81728490?utm_source=wechat_session
https://games-cn.org/games-webinar-20180426-43/
视觉SLAM十四讲

如果我的文章对你有帮助，欢迎关注，点赞，评论。