slam小白版（2）-- 贝叶斯滤波

1、原理

在SLAM问题上，可以用贝叶斯框架来进行描述。简单来说，SLAM是一个状态估计问题，就是要估计状态量（姿态、位置速度和三维点坐标） x 1 : k = { x 1 , … , x k } x_{1:k}=\left\{x_{1}, \ldots, x_{k}\right\} x1:k​={x1​,…,xk​}。

给定一组传感器观测量（即在当前状态下所看到的特征点的像素坐标） z 1 : k = { z 1 , … , z k } {z_{1:k}}=\left\{z_{1}, \ldots, z_{k}\right\} z1:k​={z1​,…,zk​}，有以下关系为

$\{\begin{array}{l}{\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)+{\mathbf{w}}_k\\ {\mathbf{z}}_k=h\left({\mathbf{x}}_k\right)+{\mathbf{v}}_k\end{array}k=1,\dots ,N$

其中$f(.)$为状态方程，$h(.)$为观测方程。$x_{k-1}$是上一时刻k-1的状态，$u_k$是传感器的读数，$w_k$是随机测量噪声，$v_k$是像素误差。

这里要先明白两个概念，

• 先验：根据0 ~ k-1时刻的各种数据来推测$k$时刻的状态
• 后验：结合了k时刻观测数据的k时刻状态

那我们希望的目的，就是根据0 ~ k时刻所有的数据，来观测当前的状态

既然是状态估计问题，我们就可以将SLAM问题转化为求解后验概率的问题：$P\left(x_{1:k}\mid z_{1:k},x_0\right)$

直接求后验概率比较困难，但是求一个状态最优估计，使得在该状态下，后验概率最大化（Maximize A Posterior，MAP）是可行的：

$x_{MAP}^{\ast }=\mathrm{argmax}P\left(x_{1:k}\mid z_{1:k},x_0\right)$

换句话说，就是求最可能的状态是什么

那么我们一般只关心当前时刻的状态，最大后验概率问题就变成了

$x_{MAP}^{\ast }=\mathrm{argmax}P\left(x_k\mid z_{1:k},x_0\right)$

概括一下，

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

目前SLAM方法大致可分为两类：

• 基于概率模型的方法：基于卡尔曼滤波的完全SLAM、压缩滤波、FastSLAM等。
• 非概率模型方法：SM-SLAM、扫描匹配、数据融合（dataassociation）、基于模糊逻辑等。

2、贝叶斯滤波

按照上面说的，$P\left(x_k\mid z_{1:k},x_0\right)$就是我们要求的，我们可以在马尔科夫假设下，用迭代贝叶斯滤波去求解

先回顾下贝叶斯法则：

$P\left(B_i\mid A\right)=\frac{P\left(A\mid B_i\right)P\left(B_i\right)}{{\sum }_{j=1}^{n}P\left(A\mid B_j\right)P\left(B_j\right)}\propto P\left(A\mid B_i\right)P\left(B_i\right)$

贝叶斯等式左边的正比于贝叶斯等式右边的分子，因为分母是在求和，不会有选择和改变的余地。

因此，式子写成：

$P\left(x_k\mid z_{1:k},x_0\right)\propto P\left(z_{1:k},x_0\mid x_k\right)\ast P(x_k)$

然后对于第一项$P\left(z_{1:k},x_0\mid x_k\right)$，显然k时刻的先验并不能决定除了$z_k$以外的在k时刻之前的数据，因此第一项就等于$P\left(z_k\mid x_k\right)$。那第二项单独一个$P(x_k)$是做什么呢？是没有$z_k$情况下的k时刻状态，它也就等于在前面的记录的$(z_{1:k-1},x_0)$发生情况下产生的数据，即$P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)P\left({\mathbf{x}}_k\mid {\mathbf{z}}_{1:k-1},{\mathbf{x}}_0\right)$。

因此，式子又可以写为$P\left(x_k\mid z_{1:k},x_0\right)\propto P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)P\left({\mathbf{x}}_k\mid {\mathbf{z}}_{1:k-1},{\mathbf{x}}_0\right)$

来看下每部分含义

• 等式左边是我们想要知道的，根据0~k时刻所有的数据，来得到当前的状态，即后验概率；

• 等式右边第一项，是给定了一个先验$x_k$的情况下，什么观测数据$z_k$最能符合这个先验的概率，即似然概率；

• 等式右边第二项，是根据0 ~ k-1时刻的观测数据，推测得知k时刻的先验状态，即先验概率；

• 把整个式子联合起来看，”根据0 ~ k时刻所有的数据得到的k时刻的状态”所服从的概率分布，是正比于”根据0 ~ k-1的观测数据，推测得知k时刻的先验状态”的概率分布与 “由k时刻产生的先验状态的基础上最符合的观测数据$z_k$”的概率分布的乘积！ 即：$后验=似然\ast 先验$ 。

再来看下这个先验概率公式$P\left({\mathbf{x}}_k\mid {\mathbf{z}}_{1:k-1},{\mathbf{x}}_0\right)$

根据全概率公式$P(B)={\sum }_{i=1}^{n}P\left(A_i\right)P\left(B\mid A_i\right)$，B就是先验$x_k$，把$x_{k-1}$当成是A。另外对离散的A求和，和对连续的状态求积分是一样的，所以先验概率公式有以下等式

$P\left({\mathbf{x}}_k\mid {\mathbf{z}}_{1:k-1},{\mathbf{x}}_0\right)=\int P\left({\mathbf{x}}_{k-1}\mid {\mathbf{z}}_{1:k-1},{\mathbf{x}}_0\right)P\left({\mathbf{x}}_k\mid {\mathbf{z}}_{1:k-1},{\mathbf{x}}_0,{\mathbf{x}}_{k-1}\right)\mathrm{d}{\mathbf{x}}_{k-1}$

然后可以把这个先验的展开式代回$后验=似然\ast 先验$的公式中，就可以得到很多论文中出现的马尔科夫链：

$P\left(X_k\mid Z_k\right)\propto P\left(Z_k\mid X_k\right)\int P\left(X_{k-1}\mid Z_{k-1}\right)P\left(X_k\mid X_{k-1}\right)d{X}_{k-1}$

这就说明，$后验=似然\ast 先验的展开$，似然意味着给定状态解释当前测量的概率，先验的展开意味着系统的动态变化。

总结一下，整体公式就是

$P\left(x_k\mid z_{1:k},x_0\right)=\frac{P\left(z_{1:k},x_0\mid x_k\right)\ast P(x_k)}{{\sum }_{j=1}^{n}P\left(z_{1:k},x_0\mid x_k\right)\ast P(x_k)}\propto P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)P\left({\mathbf{x}}_k\mid {\mathbf{z}}_{1:k-1},{\mathbf{x}}_0\right)=P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)\int P\left({\mathbf{x}}_{k-1}\mid {\mathbf{z}}_{1:k-1},{\mathbf{x}}_0\right)P\left({\mathbf{x}}_k\mid {\mathbf{z}}_{1:k-1},{\mathbf{x}}_0,{\mathbf{x}}_{k-1}\right)\mathrm{d}{\mathbf{x}}_{k-1}$

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

到这里为止，如果认为k时刻的状态只与k-1时刻有关，那就是用扩展卡尔曼滤波器（EKF）来做。如果觉得和所有状态都有关，那就是用非线性优化来做。目前主流是非线性优化。

最后总结下贝叶斯滤波步骤：

• 预测：从上一时刻状态分布预测当前时刻状态分布
  • $P\left({\mathbf{x}}_k\mid {\mathbf{z}}_{1:k-1},{\mathbf{x}}_0\right)=\int P\left({\mathbf{x}}_{k-1}\mid {\mathbf{z}}_{1:k-1},{\mathbf{x}}_0\right)P\left({\mathbf{x}}_k\mid {\mathbf{z}}_{1:k-1},{\mathbf{x}}_0,{\mathbf{x}}_{k-1}\right)\mathrm{d}{\mathbf{x}}_{k-1}$
  • $P\left({\mathbf{x}}_k\mid {\mathbf{z}}_{1:k-1},{\mathbf{x}}_0\right)$表示预测值
• 更新：使用当前时刻观测量更新后验概率
  • $P\left(x_k\mid z_{1:k},x_0\right)\propto P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)P\left({\mathbf{x}}_k\mid {\mathbf{z}}_{1:k-1},{\mathbf{x}}_0\right)$
  • $P\left(x_k\mid z_{1:k},x_0\right)$表示后验概率
  • $P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)$表示观测值
  • $P\left({\mathbf{x}}_k\mid {\mathbf{z}}_{1:k-1},{\mathbf{x}}_0\right)$表示预测值
• 最后是反复迭代求值

那贝叶斯滤波和卡尔曼滤波有什么关系？卡尔曼滤波是贝叶斯滤波的特例，当状态分布、预测模型和观测模型均服从高斯分布，那么这样的滤波称为卡尔曼滤波。卡尔曼滤波以后看懂了再写吧。