Filter-based SLAM：贝叶斯滤波

数学概念

理解基础的数学概念，是理解贝叶斯滤波的前提，由于公式推理中会经常用到这些公式，所以写在博客的开头以便后面翻看。

独立：
$$\begin{gathered} p(x,y)=p(x)p(y) \\ p(x|y)=p(x) \\ p(y|x)=p(y)\text{\hspace{1em}(1.1)} \end{gathered}$$

条件独立：
$$\begin{gathered} 表示当条件z存在时，x,y独立 \\ p(x,y|z)=p(x|z)p(y|z) \\ 当x与z相关，与y无关时 \\ p(x|y,z)=p(x|z) \\ p(y|x,z)=p(y|z)\text{\hspace{1em}(1.2)} \end{gathered}$$

全概率公式：
$$p(x)=\int p(x|y)p(y)\mathrm{d}y\text{\hspace{1em}(1.3)}$$

条件概率公式：
$$p(x,y)=p(x|y)p(y)=p(y|x)p(x)\text{\hspace{1em}(1.4)}$$

贝叶斯公式：
$$\begin{gathered} p(x,y)=p(x|y)p(y)=p(y|x)p(x) \\ \to p(x|y)=\frac{p(y|x)p(x)}{p(y)}=\eta p(y|x)p(x) \\ （但p(y)相对不重要或可以计算出来可用\eta 表示）\text{\hspace{1em}(1.5)} \end{gathered}$$

条件贝叶斯公式：
$$p(x|y,z)=\frac{p(y|x,z)p(x|z)}{p(y|z)}=\eta p(y|x,z)p(x|z)\text{\hspace{1em}(1.6)}$$

贝叶斯滤波

   gmapping是目前应用最广泛的基于滤波的SLAM算法，其算法实现是基于粒子滤波，而粒子滤波的本质就是贝叶斯滤波。在这篇文章中，我会结合机器人定位来讲解贝叶斯滤波。

介绍

1. 估计的是概率分布，不是具体的数值
2. 是一大类方法的统称
3. 是一个抽象的表达形式—对于不同问题有不同的实现方式(卡尔曼家族、粒子滤波)
4. 迭代估计形式

推导

1、符号说明：
$x_t$：表示$t$时刻机器人的位姿；
$z_t$：表示$t$时刻机器人的观测数据（激光雷达的数据）；
$u_t$：表示$t$时刻机器人的编码器里程计的数据。

2、贝叶斯滤波的更新公式：
$$\begin{gathered} bel(x_t)=p(x_t|z_{1:t},u_{1:t})=\eta p(z_t|x_t)\overline{bel}(x_t) \\ 其中：\overline{bel}=\int p(x_t|x_{t-1},u_t)p(x_{t-1}|z_{1:t-1},u_{1:t-1})\mathrm{d}{x}_{t-1} \\ \text{\hspace{1em}(2.1)} \end{gathered}$$
  该公式的作用是在已知$t-1$时刻的概率分布，在给定$t$时刻的观测数据$(z_t,u_z)$的情况下，估计出状态量在$t$时刻的概率分布。此处$bel(x_t)$表示$x_t$的后验概率分布，$\overline{bel}$表示$x_t$的预测概率分布（先验概率分布）。
　　
3、贝叶斯滤波的推导：
目标：在已知$p(x_{t-1}|z_{1:t-1},u_{1:t-1})、u_t、z_t$的情况下，得到$p(x_t|z_{1:t},u_{1:t})$的表达式。
$$\begin{gathered} p(x_t|z_{1:t},u_{1:t})=p(x_t|z_t,z_{1:t-1},u_{1:t}) \\ =\frac{p(z_t|x_t,z_{1:t-1},u_{1:t})p(x_t|z_{1:t-1},u_{1:t})}{p(z_t|z_{1:t-1},u_{1:t})} \\ =\eta p(z_t|x_t,z_{1:t-1},u_{1:t})p(x_t|z_{1:t-1},u_{1:t})\text{\hspace{1em}(2.2)} \end{gathered}$$
  因为当前时刻的激光观测数据$z_t$仅与位姿$x_t$有关，与历史观测$z_{1:t-1}$及历史到当前位姿$u_{1:t}$无关（已知$t$时刻位姿后，观测$z_t$就与$x_t$有关，与$u_t$无关），公式$(2.2)$第一步将$z_t$与$z_{1:t-1}$分开。然后先忽略$z_{1:t-1}$和$u_{1:t}$，将$p(x_t|z_t)$用贝叶斯公式展开，再把$z_{1:t-1}$和$u_{1:t}$代人展开的式子中。
  另外，我们还需要将公式(2.2)简化一下。
其中：
已知$t$时刻位姿后，观测$z_t$就与当前位姿$x_t$有关，与$u_{1:t}$和$z_{1:t-1}$无关：
$$p(z_t|x_t,z_{1:t-1},u_{1:t})=p(z_t|x_t)\text{\hspace{1em}(2.3)}$$
这里是将$p(x_t|z_{1:t-1},u_{1:t})$用全概率公式展开得到$x_t$与$x_{t-1}$的关系：
$$\begin{gathered} p(x_t|z_{1:t-1},u_{1:t})=\int p(x_t|x_{t-1},z_{1:t-1},u_{1:t})p(x_{t-1}|z_{1:t-1},u_{1:t})\mathrm{d}{x}_{t-1} \\ \text{\hspace{1em}(2.4)} \end{gathered}$$
将上面全概率公式再简化，得到：
$$\begin{gathered} p(x_t|x_{t-1},z_{1:t-1},u_{1:t})=p(x_t|x_{t-1},u_t) \\ p(x_{t-1}|z_{1:t-1},u_{1:t})=p(x_{t-1}|z_{1:t-1},u_{1:t-1})\text{\hspace{1em}(2.5)} \end{gathered}$$
上面公式$p(x_t|x_{t-1},z_{1:t-1},u_{1:t})=p(x_t|x_{t-1},u_t)$便是机器人的运动模型。
令：
$$\begin{gathered} bel(x_t)=p(x_t|z_{1:t},u_{1:t}) \\ \overline{bel(x_t)}=p(x_t|z_{1:t-1},u_{1:t})\text{\hspace{1em}(2.6)} \end{gathered}$$
  $bel(x_t)$是后验概率分布，也就是我们要求的实际分布；$\overline{bel(x_t)}$是先验概率分布，也就是根据里程计数据得到的预测(proposal)分布。最后得到的式子如下：
$$bel(x_t)=\eta p(z_t|x_t)\overline{b}el(x_t)\text{\hspace{1em}(2.7)}$$

实例

   在上图a中，机器人的位置的概率分布是一个均匀分布，所以机器人无法获知自己的位置。图b中，机器人通过传感器检测知道了自己的位置在门旁边，这时候概率分布就变成了在门对应的位置有一个较大的波峰，表示机器人可能在这三个位置上，但无法确定是哪一个门。图c较图b，机器人向前移动了一定距离，到了另一个门边上，此时机器人的位置概率分布也随之产生变化，波峰的位置改变。这时候，图c的概率分布是通过里程计计算预测得到的，为先验概率分布，图d的概率分布通过观测数据更新后，得到的后验概率分布就确定了机器人的位置。