README.md

SLAM - Simultaneous localization and mapping

Introduction

这份笔记是针对汉诺威大学Claus Brenner教授的课程SLAM 整理而来，笔记内的某些专业词汇可能是德语，该笔记仅供个人学习使用。课程所需的编程环境为python2.7. 在github页面正确显示公式需要安装一个插件: MathJax Plugin for Github, 教程视频： SLAM Lectures，Repo：SLAM，该课程分为以下几个模块：

• Unit A: Roboter, Sensor, Arena,Landmark, Motion Model,
• Unit B: LS-Square, correkt the trajektory, Feature Based Approach，Featureless Approach，Iterative-Closest-Point
• Unit C: Probability-Distribution, Bayes-Filter, Kalmann-Filter (One-Dimention)
• Unit D: Multivariate Normal Distribution, Kalmann-Filter (Muilti-Dimention), Extended-Kalmann-Filter(Non-Linear)
• Unit E: Particle Filter
• Unit F: Mapping, Extended-Kalmann-Filter SLAM
• Unit G: Particle Filter SLAM

Unit A

小车及环境构造

Unit A 的内容是对一个乐高小车的运动进行建模，首先利用乐高小车的电机编码器返回的数据计算出小车的运动轨迹，其次利用乐高小车搭载的LiDAR传感器获得周围环境的深度信息，检测出Landmark所在的位置。

Fig A-1 Lego Car

Fig A-2 Arena and Landmark

下图展示了乐高小车的运动模型，由两个电机带动左右履带，履带的位移差距可以让小车进行转向。

Fig A-3 Lego Car Structur

小车的编码器可以表示电机的角度，经过换算，1 tick 约为 0,349 mm 履带前进的距离。下图表示了一个电机的log-file中的内容。

Fig A-4 Motor log file

其中，第0列 'M' 表示Motor, 第1列表示时间戳 (time stamp), 第2列表示左轮Motor的编码， 第6列表示右轮Motor的编码，第10列表示第三个motor的编码，这里我们只需要左轮和右轮的motor即可。slam_01_a， slam_01_b，slam_01_c 这三个程序展示了如何通过lego_robot 这个class来读取robot4_motors.txt 。

小车运动模型

下一步是建立小车转弯时的运动模型。

Fig A-5 Motion Model

由下列公式可以得到小车绕中心点旋转时的旋转半径 $R$ 和旋转的角度 $\alpha$：

$$\begin{array}{}\text{(1.1)}& \begin{array}{rl}r& =\alpha \cdot (R+w)\\ l& =\alpha \cdot R\\ r-l& =\alpha \cdot w\\ \alpha & =(r-l)/w\\ R& =l/\alpha \end{array}\end{array}$$

p点和p'点之间的坐标关系可以通过中心点c联系起来：

Fig A-6 Motion Model

c点和p‘的坐标为：

$$\begin{array}{}\text{(1.2)}& \begin{array}{rl}\overrightarrow{c}& =\overrightarrow{p}-(R+\frac{w}{2})\cdot \left[\begin{array}{c}\sin \theta \\ -\cos \theta \end{array}\right]\\ \overrightarrow{p^{\prime }}& =\overrightarrow{c}+(R+\frac{w}{2})\cdot \left[\begin{array}{c}\sin \theta +\alpha \\ -\cos \theta +\alpha \end{array}\right]\end{array}\end{array}$$

slam_02_a 展示了如何通过读取乐高小车的电机logfile，将其转换成前进距离，并利用上述公式计算出下一时刻的位置。由于传感器LiDAR 的位置并不是小车轮子中心的位置，所以对读取的位置减去displacement (30mm)，计算好新的位置之后，再将displacement加上。slam_02_b 展示了相关操作，并将过滤过的位置输出到 poses_from_ticks.txt。

Fig A-7 Displacement

logfile_viewer.py 可以将 poses_from_ticks.txt 可视化。

Fig A-8 logfile_view

在可视化窗口中加入robot4_reference.txt，可以观察到小车和参考轨迹之间的差距，这是由于小车的宽度参数并不是实际参数，通过修改robot_width这一变量，可以调整计算得来的轨迹精度。

Fig A-9 logfile_view, left: robot_width=150, right: robot_width=171

基于LiDAR信息预测Landmark位置

下一步是引入传感器LiDAR得到的深度信息，以此来确定Arena中的Landmark位置。在可视化窗口载入robot4_scan.txt, 可以观察到小车的传感器信息，

Fig A-10 left: robot4\_scan.txt, middle: 可视化窗口, right: 单次时间点读取的数据

为了得到Landmark的数量，我们要对传感器数据进行微分，得到单次时间不同角度之间的差值 (derivative)，通过计数每次传感器数值的突变来得到Landmark的数量：

$$\begin{array}{}\text{(1.3)}& f^{\prime }(i)\approx \frac{f(i+1)-f(i-1)}{2}\end{array}$$

从上图右侧窗口可知，在某些角度，比如400附近，产生了一个传感器误差，此时传感器的值小于20，在程序slam_03_b我们对这样的数值进行过滤。最终得到该图像的微分图像：

Fig A-11 传感器LiDAR的数据及其微分

在程序slam_03_c中，通过对Landmark左边的edge和右边的edge进行计数，可以得到Landmark的数量，通过对左edge和右edge之间的数值求平均值，可以得到该Landmark相对于传感器的角度和深度信息，以此，我们可以确定该Landmark的位置。

Fig A-12 Landmark 相对于LiDAR的极坐标

在程序slam_03_d中，我们将Landmark的位置从相对LiDAR的极坐标位置转换成笛卡尔坐标系位置，并考虑到传感器接触Landmark表面与Lanmark中心点之间的差值大约为90mm, 最终结果输出到可视化窗口后如图，小绿点为Landmark通过建模估计得到的位置，灰点为Landmark实际位置：

Fig A-13 Landmark 相对于LiDAR的笛卡尔坐标

Unit B

基于Landmark预测建立相似变换公式

在转弯的时候，Landmark的估计结果有较大误差，通过比较估计的Landmark和实际的Landmark的差距，我们可以矫正对应的小车位置。

Fig B-1 Landmark 估计与实际位置的差距

首先通过 slam_04_a 可以将Landmark相对于地图的笛卡尔坐标计算出来，结果与上图通过 logfile_viewer 类似。 我们要把探测到的Landmark与实际存在的最近的Landmark进行匹配。程序 slam_04_b 实现了这样的效果。

Fig B-2 Landmark 估计位置与实际位置进行配准

下一步是将匹配好的点经过旋转位移等操作进行配准。对点云进行配准需要相似变换：

$$\begin{array}{}\text{(2.1)}& \lambda \overrightarrow{R}{\overrightarrow{l}}_i+\overrightarrow{t}={\overrightarrow{r}}_i\end{array}$$

其中需要确认以下3个变量：

$$\begin{array}{}\text{(2.2)}& \begin{array}{rl}\lambda & :scale\in \mathbb{R}\\ \overrightarrow{R}& :\left[\begin{array}{cc}\cos \alpha & -\sin \alpha \\ \sin \alpha & \cos \alpha \end{array}\right]\in {\mathbb{R}}^{2x2},\alpha \in \left[\begin{array}{cc}0& 2\pi \end{array}\right]\\ \overrightarrow{t}& :\left[\begin{array}{cc}{t}_x& t_y\end{array}\right]\end{array}\end{array}$$

利用最小二乘法确定相似变换公式

由于存在噪声误差，上述变换公式中，等式左边并不完全等于等式右边，我们需要用到最小二乘法来优化该问题。但由于旋转矩阵和scale变量的存在，该优化问题为非线性问题：

$$\begin{array}{}\text{(2.3)}& \sum _i=||\lambda \overrightarrow{R}{\overrightarrow{l}}_i+\overrightarrow{t}-{\overrightarrow{r}}_i|{|}^2\end{array}$$

所以接下来我们先计算两个点云的中心，通过每个点减去点云中心，得到基于点云中心的每个点的坐标：

$$\begin{array}{}\text{(2.4)}& \begin{array}{rl}{\overrightarrow{l}}_m& =\frac{1}{m}\sum _i{\overrightarrow{l}}_i\\ {\overrightarrow{r}}_m& =\frac{1}{m}\sum _i{\overrightarrow{r}}_i\\ {\overrightarrow{l}}_i^{\prime }& ={\overrightarrow{l}}_i-{\overrightarrow{l}}_m\\ {\overrightarrow{r}}_i^{\prime }& ={\overrightarrow{r}}_i-{\overrightarrow{r}}_m\end{array}\end{array}$$

并且满足：

$$\begin{array}{}\text{(2.5)}& \sum _i{\overrightarrow{l}}_i^{\prime }=0\end{array}$$

Fig B-3 点云基于中心点的坐标向量

由此可得：

$$\begin{array}{}\text{(2.6)}& \begin{array}{rl}\lambda \overrightarrow{R}{\overrightarrow{l}}_i-{\overrightarrow{r}}_i+\overrightarrow{t}& =\lambda \overrightarrow{R}({\overrightarrow{l}}_i^{\prime }+{\overrightarrow{l}}_m)-({\overrightarrow{r}}_i^{\prime }+{\overrightarrow{r}}_m)+\overrightarrow{t}\\ & =\lambda \overrightarrow{R}{\overrightarrow{l}}_i^{\prime }-{\overrightarrow{r}}_i^{\prime }+\lambda \overrightarrow{R}{\overrightarrow{l}}_m-{\overrightarrow{r}}_m+\overrightarrow{t}\\ & =\lambda \overrightarrow{R}{\overrightarrow{l}}_i^{\prime }-{\overrightarrow{r}}_i^{\prime }+{\overrightarrow{t}}^{\prime }\end{array}\end{array}$$

接下来我们要对上述模型进行优化，利用最小二乘法可得：

$$\begin{array}{}\text{(2.7)}& \begin{array}{rl}& \sum _i||\lambda \overrightarrow{R}{\overrightarrow{l}}_i^{\prime }-{\overrightarrow{r}}_i^{\prime }+{\overrightarrow{t}}^{\prime }|{|}^2\\ =& \sum _i||\lambda \overrightarrow{R}{\overrightarrow{l}}_i^{\prime }-{\overrightarrow{r}}_i^{\prime }|{|}^2+2{{\overrightarrow{t}}^{\prime }}^{\mathrm{T}}\sum _i(\lambda \overrightarrow{R}{\overrightarrow{l}}_i^{\prime }-{\overrightarrow{r}}_i^{\prime })+\sum _i||{\overrightarrow{t}}^{\prime }|{|}^2\\ =& \sum _i||\lambda \overrightarrow{R}{\overrightarrow{l}}_i^{\prime }-{\overrightarrow{r}}_i^{\prime }|{|}^2+m||{\overrightarrow{t}}^{\prime }|{|}^2⟶min\end{array}\end{array}$$

为了使上式达到最小值，首先第一项要达到最小，其次第二项可以达到零:

$$\begin{array}{}\text{(2.8)}& \begin{array}{rl}{\overrightarrow{t}}^{\prime }& =0\\ \lambda \overrightarrow{R}{\overrightarrow{l}}_m-{\overrightarrow{r}}_m+\overrightarrow{t}& =0\\ \overrightarrow{t}& ={\overrightarrow{r}}_m-\lambda \overrightarrow{R}{\overrightarrow{l}}_m\\ \end{array}\end{array}$$

通过分析上式可知，只需确认$\lambda$ 和$\vec{R}$ 即可使上式为零。接着我们分析第一项，通过对第一项等效变换可得：

$$\begin{array}{}\text{(2.9)}& \begin{array}{rl}& \sum _i||\lambda \overrightarrow{R}{\overrightarrow{l}}_i^{\prime }-{\overrightarrow{r}}_i|{|}^2⟶min\\ ⟹& \sum _i||\sqrt{\lambda }\overrightarrow{R}{\overrightarrow{l}}_i^{\prime }-\frac{1}{\sqrt{\lambda }}{\overrightarrow{r}}_i|{|}^2⟶min\\ =& \lambda \sum _i||\overrightarrow{R}{\overrightarrow{l}}_i^{\prime }|{|}^2-2\sum _i{{\overrightarrow{r}}_i^{\prime }}^{\mathrm{T}}\overrightarrow{R}{\overrightarrow{l}}_i^{\prime }+\frac{1}{\lambda }\sum _i||{\overrightarrow{r}}_i^{\prime }|{|}^2\\ ⟹& \lambda \sum _i||{\overrightarrow{l}}_i^{\prime }|{|}^2-2\sum _i{\overrightarrow{r}}_i^{\mathrm{T}}\overrightarrow{R}{\overrightarrow{l}}_i^{\prime }+\frac{1}{\lambda }\sum _i||{\overrightarrow{r}}_i^{\prime }|{|}^2\\ =& \lambda a-b+\frac{1}{\lambda }c\stackrel{min}{⟶}{\lambda }^2=\frac{c}{a}\\ ⟹& {\lambda }^2=\frac{\sum _i||{\overrightarrow{r}}_i^{\prime }|{|}^2}{\sum _i||{\overrightarrow{l}}_i^{\prime }|{|}^2}\\ ⟹& \lambda =\sqrt{\frac{\sum _i||{\overrightarrow{r}}_i^{\prime }|{|}^2}{\sum _i||{\overrightarrow{l}}_i^{\prime }|{|}^2}}\end{array}\end{array}$$

要使得上式达到最小，我们还需要使b项达到最大：

$$\begin{array}{}\text{(2.10)}& \begin{array}{rl}& \sum _i{{\overrightarrow{r}}_i^{\prime }}^{\mathrm{T}}\overrightarrow{R}{\overrightarrow{l}}_i^{\prime }⟶max\\ =& \left[\begin{array}{cc}{r}_x^{\prime }& r_y^{\prime }\end{array}\right]\left[\begin{array}{cc}\cos \alpha & -\sin \alpha \\ \sin \alpha & \cos \alpha \end{array}\right]\left[\begin{array}{c}{l}_x^{\prime }\\ l_y^{\prime }\end{array}\right]\\ =& \left[\begin{array}{cc}{r}_x^{\prime }& r_y^{\prime }\end{array}\right]\left[\begin{array}{ccc}{l}_x^{\prime }\cos \alpha & -l_y^{\prime }& \sin \alpha \\ l_x^{\prime }\sin \alpha & +l_y^{\prime }& \cos \alpha \end{array}\right]\\ =& r_x^{\prime }{l}_x^{\prime }\cos \alpha -r_x^{\prime }{l}_y^{\prime }\sin \alpha +r_y^{\prime }{l}_x^{\prime }\sin \alpha +r_y^{\prime }{l}_y^{\prime }\cos \alpha \\ =& \cos \alpha \sum _i(r_x^{\prime }{l}_x^{\prime }+r_y^{\prime }{l}_y^{\prime })+\sin \alpha \sum _i(-r_x^{\prime }{l}_y^{\prime }+r_y^{\prime }{l}_x^{\prime })⟶max\\ =& \left[\begin{array}{cc}\cos \alpha & \sin \alpha \end{array}\right]\left[\begin{array}{c}\sum _i(r_x^{\prime }{l}_x^{\prime }+r_y^{\prime }{l}_y^{\prime })\\ \sum _i(-r_x^{\prime }{l}_y^{\prime }+r_y^{\prime }{l}_x^{\prime })\end{array}\right]⟶max\end{array}\end{array}$$

上列式子由两个向量相乘，可以判断出，当两个向量具有相同方向时，其乘积达到最大，如下图所示：

Fig B-4 向量乘积

所以当满足以下条件时达到最大：

$$\begin{array}{}\text{(2.11)}& \left[\begin{array}{c}\cos \alpha \\ \sin \alpha \end{array}\right]=\frac{1}{||\left[\begin{array}{c}\sum _i(r_x^{\prime }{l}_x^{\prime }+r_y^{\prime }{l}_y^{\prime })\\ \sum _i(-r_x^{\prime }{l}_y^{\prime }+r_y^{\prime }{l}_x^{\prime })\end{array}\right]||}\left[\begin{array}{c}\sum _i(r_x^{\prime }{l}_x^{\prime }+r_y^{\prime }{l}_y^{\prime })\\ \sum _i(-r_x^{\prime }{l}_y^{\prime }+r_y^{\prime }{l}_x^{\prime })\end{array}\right]\end{array}$$

接下来整理以下编程的思路：

Given: $$ \vec{l}_i, \vec{r}_i, 1 \leq i \leq m \tag{2.12} $$

Compute: $$ \begin{aligned} &\vec{l}_m = \frac{1}{m}\sum_i\vec{l}_i,\quad \vec{r}_m = \frac{1}{m}\sum_i\vec{r}_i \\ &\vec{l}_i' = \vec{l}_i - \vec{l}_m,\quad \vec{r}_i' = \vec{r}_i - \vec{r}_m \\ \\ &cs, ss, rr, ll = 0.0 \\ &for \quad i\quad in\quad 1\quad \cdots m: \\ &\qquad sum\ of\ cos: cs += r_x'l_x' + r_y'l_y' \\ &\qquad sum\ of\ sins: ss += -r_x'l_y' + r_y'l_x' \\ &\qquad length\ of\ vectors: rr += r_x'r_x' + r_y'r_y' \\ &\qquad length\ of\ vectors: ll += l_x'l_x' + l_y'l_y' \\ \\ &\lambda = \sqrt{\frac{rr}{ll}} \\ &\begin{bmatrix}\cos{\alpha} \\ \sin{\alpha} \end{bmatrix} = \frac{\begin{bmatrix} cs\\ ss \end{bmatrix}}{\sqrt{cs^2 + ss^2}} \\ & \begin{bmatrix} t_x \\ t_y \end{bmatrix} = \vec{r}_m - \lambda\vec{R}\vec{l}_m = \begin{bmatrix} r_{m,x} \\ r_{m,y} \end{bmatrix} - \lambda \begin{bmatrix} \cos{\alpha} & -\sin{\alpha} \\ \sin{\alpha} & \cos{\alpha} \end{bmatrix} \begin{bmatrix} l_{m,x} \\ l_{m,y} \end{bmatrix} \\ \\ &return (\lambda, \cos{\alpha}, \sin{\alpha}, t_x, t_y) \end{aligned} \tag{2.13} $$

通过把$\lambda$ 设置为1，上述式子还能从相似变换（similar transformation）转换为 僵硬变换 (rigid transformation) 。值得注意的是，为了计算出上式中四个变量，我们需要至少四个观测值（Observation），否则无法执行最小二乘法（least square）。程序slam_04_c 实现了最小二乘法（least square）。

Fig B-5 利用最小二乘法匹配到的Landmark

利用相似变换公式矫正小车轨迹

下一步，在程序 >slam_04_d中 我们利用通过Landmark 确定下来的相似变换公式，同样可以用来矫正机器人的位置 (Pose) 和朝向 (Heading)，如下图所示：

Fig B-6 通过Landmark确定的相似变换公式矫正机器人的位置和朝向

$$\begin{array}{}\text{(2.14)}& \begin{array}{rl}\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\end{array}\right]& =\lambda \left[\begin{array}{cc}\cos \alpha & -\sin \alpha \\ \sin \alpha & \cos \alpha \end{array}\right]\left[\begin{array}{c}x\\ y\end{array}\right]+\left[\begin{array}{c}{t}_x\\ t_y\end{array}\right]\\ \alpha & =atan2(\sin \alpha ,\cos \alpha )\end{array}\end{array}$$

Fig B-7 通过Landmark确定的相似变换公式矫正机器人的位置和朝向，修正后的轨迹

将图 B-7与图 B-6 进行比较发现，在B-6中由于模型本身的参数误差造成的轨迹误差，通过观测Landmark的位置进行修正，在图 B-7 中有了很大的改善，但此时由于Feature点较少，轨迹修正不够顺滑，图中机器人的轨迹有较大突变，这不是我们想要的结果。利用雷达信息提取出Landmark信息，我们称之为 Feature Based Approach，但有时候雷达只能检测到一个 Landmark，此时便无法使用最小二乘法，但此时雷达仍然能获得墙壁的信息，这些信息没有特殊的Feature，但我们仍然可以利用它们，这种方法称为 Featureless Approach。

利用墙面信息进行 Featureless Approach， 更顺滑地矫正小车轨迹

Fig B-8 Feature Based Approach

Fig B-9 利用墙壁的信息进行Non Feature Based Approach

在程序slam_05_a 中，LiDAR获取的墙面数据与场地墙面坐标进行比较，找出墙面上与LiDAR数据最接近的坐标，下图展示了LiDAR数据点与最近的墙面坐标：

Fig B-10 匹配的墙面与LiDAR数据点

同样的，基于墙面信息同样能得到相似变化公式，从而来矫正机器人的轨迹，程序slam_05_b 中实现了利用墙面信息进行相似矩阵估计并矫正机器人轨迹的算法，下图展示了Featureless Approach的效果：

Fig B-11 Featureless Approach

ICP Iterative Closest Points Approach

该基于墙面信息的Featureless Approach 的缺点在于，每个墙面的数据点都想要进行修正，然而却互相牵制，无法直接达到最优化效果，如下图所示：

Fig B-12 Featureless Approach 的缺点

为了达到最优效果，我们采取迭代最近点的方法，即 Iterative Closest Point (ICP)，每次优化之后，更新相似变换矩阵，通过迭代的方式最终达到最优化状态：

Fig B-13 Iterative Closest Point (ICP)

程序slam_05_c 中实现了ICP的算法，伪代码如下：

# Init overall_trafoo	
overall_trafo = (1.0, 1.0, 0.0, 0.0, 0.0)
for j in xrange(iterations):
 	将world_points_init 变换成 world_points_new
	Call get_correspoinding_points_on_wall(...) 找到对应的墙上的点
	找出从world_points_new 变换成 墙上的点的trafo
	将旧的overall_trafo与新的trafo连接，
	overall_trafo = concatenate_transform(trafo, overall_trafo)
if trafo返回为none的时候，
	结束循环
返回最终的overrall_trafo

利用 ICP 算法矫正的机器人轨迹如下：

Fig B-14 利用 ICP 算法矫正的机器人轨迹

将图 B-14 与 图B-12 以及 B-13 相比较，可以看出，Feature Based Approach (利用Landmark信息) 的轨迹有较大突变，Feautureless Approach （利用墙面信息，无 ICP） 的轨迹比较顺滑，Featureless Approach with ICP 的算法，轨迹最为顺滑。

Unit C

误差的类型

从Unit A 中我们了解到，由于未对机器人的建模参数进行矫正 (calibration), 所以机器人在转弯的时候有较大的误差。通过对前面章节机器人的轨迹误差进行分析，可以将机器人的轨迹误差大致分为两类：

• Systematic Error 系统误差
  • use/add calibration parameters, 矫正过的参数可能和实际不匹配
  • can't capture all real world properties in the model，比如更换地面之后，摩擦力和位移会发生变化，模型不再适用
• Random Error 随机误差

Fig C-1 系统误差和随机误差

贝叶斯过滤器

我们来考虑以下机器人一维下的移动，其位置精度的概率分布可能如下：

Fig C-2 机器人位置的概率分布

在程序 distribution.py 中我们设置了一个关于离散分布的类，里面包含了一个离散分布可能用到的各种变量和各种方法，程序 slam_06_a 展示了如何设置一个 distribution 的 offset 和 values 。

接下来的问题是不确定性的传递，如下图所示，在初始位置我们很确定机器人的位置，到了第二步，机器人的位置有了不确定性，到了第三步，机器人的不确定性会在第二步之上继续叠加：

Fig C-3 机器人位置的概率分布

如上图所示，当A事件发生时B事件发生的概率为：

$$\begin{array}{}\text{(3.1)}& P(A,B)=P(B|A)P(A)\end{array}$$

第二步中机器人位于每个位置的可能性如下图所示：

Fig C-3 机器人位置的概率分布

$$\begin{array}{}\text{(3.2)}& \mathrm{T}\mathrm{o}\mathrm{t}\mathrm{a}\mathrm{l}\mathrm{p}\mathrm{o}\mathrm{s}\mathrm{i}\mathrm{b}\mathrm{i}\mathrm{l}\mathrm{i}\mathrm{t}\mathrm{y}:\mathrm{P}(\mathrm{A},\mathrm{B})=\sum _{\mathrm{i}}\mathrm{P}(\mathrm{B}|{\mathrm{A}}_{\mathrm{i}})\mathrm{P}({\mathrm{A}}_{\mathrm{i}})\end{array}$$

对于上图的概率分布，这实际上相当于对事件A的概率用事件B的概率进行卷积，程序 slam_06_b 实现了对概率分布进行卷积操作。下图展示了一个机器人的位置如何通过三次位移后达到一个不确定状态以及其每一步位移之后位置的概率分布：

Fig C-4 机器人经过三次位移之后的概率分布

机器人通过建模对自身位置进行估计时，我们要考虑到由于模型的局限性带来的随机误差。 机器人通过传感器对自身位置进行估计时，同样要考虑随机误差，模型的随机误差和传感器的随机误差通过相乘结合到一起。

Fig C-5 测量随机误差，模型随机误差，以及其总体随机误差

如上图，模型随机误差我们称之为 先验 (Prior)， 测量随机误差我们称之为 测量(Measurment), 他们的总体误差将通过这两者随机误差相乘得到。

首先我们来了解一下条件概率：

$$\begin{array}{}\text{(3.3)}& \begin{array}{rl}PosteriorProbability(后验概率)& =\frac{PriorProbability(先验概率)}{Evidence(证据)}\\ \\ P(X|Z)& =\frac{P(Z|X)\cdot P(X)}{P(Z)}\\ & =\frac{P(Z|X)\cdot P(X)}{\sum _{x^{\prime }}P(Z|x^{\prime })\cdot P(x^{\prime })}\end{array}\end{array}$$

程序 slam_06_c 实现了两个概率分布相乘的算法，效果图如下：

Fig C-6 概率分布相乘

通过对Measurement的位移，Posterior的改变如下：

Fig C-7 Measurement位移造成的Posterior的改变

对于机器人小车而言，基于模型的位置估计有随机误差，基于传感器的观测也有随机误差，我们不能只相信模型或只相信传感器，而是要对这两者的概率分布进行结合。对于每一步小车的移动，首先要把小车模型的概率分布与上一时间点小车总体的概率分布进行卷积，得到这一步小车模型的概率分布，其次通过与传感器的高铝分布相乘，得到此时小车最终的概率分布：

1. Motion - Convolution

$$\begin{array}{}\text{(3.4)}& P(X)=\sum _{y}P(X|y)\cdot P(y)\end{array}$$

2. Measurement - Multiplication

$$\begin{array}{}\text{(3.5)}& P(X|Z)=\alpha \cdot P(Z|X)\cdot P(X)\end{array}$$

Fig C-8 某一时间点小车的模型概率部分与传感器的概率分布

计算机器人位置整体概率分布的算法如下：

Fig C-9 贝叶斯过滤器，考虑小车模型的概率分布以及传感器的概率分布

下图展示了一个机器人通过贝叶斯过滤器，综合考虑模型概率分布以及传感器概率分布，并分别使用三角分布以及正态分布的结果：

Fig C-10 贝叶斯过滤器，左：三角分布，右：正态分布

###正态分布

下图展示了一些正态分布的基础知识：

Fig C-11 正态分布

正态分布的积分不是1，而是由 $\sigma$ 决定的。 正态分布的密度如下:

$$\mathcal{N}(x;\mu ,{\sigma }^2)=\frac{1}{\sqrt{2\pi }\cdot \sigma }\cdot {\mathrm{e}}^{-\frac{1}{2}(\frac{x-\mu }{\sigma }{)}^2}$$

mit

$$\begin{array}{rl}\mu :& mean\\ {\sigma }^2:& variance\\ \sigma :& standarddeviation=\sqrt{variance}\end{array}$$

基于模型预测以及传感器观测，我们可以得到一个belief，假设模型预测以及传感器观测的概率分布皆为正态分布，则得到的belief也为正态分布，我们只需计算其变量 $\mu$ 以及$\sigma^2$:

Fig C-12 两个正态分布相乘仍是正态分布

由以下公式我们可以确定两个正态分布相乘以后仍是正态分布:

$$\begin{array}{rl}bel(x)=& \alpha \cdot p(z|x)\cdot be{l}^{\prime }(x)\\ =& {\alpha }^{\prime }\cdot {\mathrm{e}}^{-\frac{1}{2}(\frac{z-cx}{{\sigma }_Q}{)}^2}\cdot {\mathrm{e}}^{-\frac{1}{2}(\frac{x-{\mu }^{\prime }}{{\sigma }^{\prime }}{)}^2}\\ =& {\alpha }^{\prime }\cdot {\mathrm{e}}^{-(\frac{1}{2}(\frac{z-cx}{{\sigma }_Q}{)}^2+\frac{1}{2}(\frac{x-{\mu }^{\prime }}{{\sigma }^{\prime }}{)}^2)}\\ =& {\alpha }^{″}\cdot {\mathrm{e}}^{-\frac{1}{2}(\frac{x-\mu }{\sigma }{)}^2}⟶Normalverteilung!\end{array}$$

接下来我们来推导变量 $\mu$ 以及$\sigma^2$:

由下述公式可得二次函数的系数:

$$\begin{array}{rl}f(x)=& \frac{1}{2}A(x-B{)}^2+C\\ \frac{\partial }{\partial x}f(x)=& A(x-B)\stackrel{!}{=}0\\ \frac{{\partial }^2}{\partial x^2}f(x)=& A\end{array}$$

从两个正态分布相乘的公式可得:

$$\begin{array}{rl}g(x)=& \frac{1}{2}(\frac{z-cx}{{\sigma }_Q}{)}^2+\frac{1}{2}(\frac{x-{\mu }^{\prime }}{{\sigma }^{\prime }}{)}^2\\ \frac{\partial }{\partial x}g(x)=& \frac{1}{{\sigma }_Q^2}(z-cx)\cdot (-c)+\frac{1}{{\sigma }^{\prime 2}}(x-{\mu }^{\prime })\stackrel{!}{=}0\end{array}$$