SLAM (Simultaneous Localization and Mapping)

SLAM stands for Simultaneous Localization and Mapping.

SLAM problems arise when the robot does not have access to a map of the environment, nor does it know its own pose. Instead, all it is given are measurements $z_{1:t}$ and controls $u_{1:t}$. The term "simultaneous localization and mapping" describes the resulting problem: In SLAM, the robot acquires a map of its environment while simultaneously localizing itself relative to this map.

SLAM 问题的输入是：控制量 $u_{1:t}$ 和测量值 $z_{1:t}$ ，想得到的输出是：地图 $m$ 和机器人路径 $x_{1:t}$ 。

参考：
Probabilistic Robotics, by Sebastian Thrun, 10 Simultaneous Localization and Mapping
Simultaneous Localisation and Mapping (SLAM): Part I The Essential Algorithms
视觉 SLAM 漫谈：http://www.cnblogs.com/gaoxiang12/p/3695962.html

The EKF SLAM has been the de facto method for SLAM, until the introduction of FastSLAM.

EKF SLAM 和 EKF Localization 算法类似，区别在于 EKF SLAM 除了估计机器人位姿外，还要估计 Landmark 的坐标。

我们知道，要使用 Kalman 滤波器进行估计，需要知道“理论估计值”（在“预测阶段”会使用它）和“测量值”（在“更新阶段”会使用它）。Kalman 滤波器可以看作混合“理论估计值”和“测量值”，从而得到一个更接近真实值的估计值。

EKF SLAM 要同时估计机器人位姿和 Landmark 的坐标。对于估计机器人位姿，它的“理论估计值”可以由机器人的 Motion Model 得到；对于估计 Landmark 的坐标，它的“理论估计值”可以由下式（摘自：SLAM for Dummies）得到：
$$\left[\begin{array}{c}\text{range}\\ \text{bearing}\end{array}\right]=\left[\begin{array}{c}\sqrt{({\lambda }_x-x{)}^2+({\lambda }_y-y{)}^2}+v_r\\ \text{atan2}({\lambda }_y-y,{\lambda }_x-x)-\theta +v_{\theta }\end{array}\right]$$
其中， ${\lambda }_x,{\lambda }_y$ 是 Landmark 的坐标， $x,y$ 是机器人坐标估计值， $\theta$ 是机器人 Bearing(Heading Direction)。

参考：
SLAM - Simultaneous Localization and Mapping, by Wolfram Burgard, Diego Tipaldi
SLAM for Dummies
Simulataneous localization and mapping with the extended Kalman filter

如果直接把粒子滤波器应用于 SLAM 算法中，则描述地图时维度太大，计算上不可行。比如，有一个 $20m\times 10m$ 的地图，且需要的分辨率为 $5cm\times 5cm$ ，则这个地图中共有 $400\times 200=80,000$ 个单元，那么有 $2^{80,000}$ 个可能的地图！参考：http://people.eecs.berkeley.edu/~pabbeel/cs287-fa12/slides/RBPF.pdf

FastSLAM 算法是 Rao-Blackwellised 粒子滤波器(RBPF)在机器人 SLAM 问题中的应用。

Rao-Blackwellised 粒子滤波器(RBPF, Rao-Blackwellized Particle Filtering)
在高维状态空间中采样时，PF 的效率很低。对某些状态空间模型，状态向量的一部分在其余部分的条件下的后验分布可以用解析方法求得，例如某些状态是条件线性高斯模型，可用 Kalman 滤波器得到条件后验分布，对另外部分状态用 PF，从而得到一种混合滤波器，降低了 PF 采样空间的维数，RBPF 样本的重要性权的方差远远低于 SIR 方法的权的方差，为使用粒子滤波器解决 SLAM 问题提供了理论基础。而 Montemerlo 等人在 2002 年首次将 Rao-Blackwellised 粒子滤波器应用到机器人 SLAM 中，并取名为 FastSLAM 算法。

摘自：http://baike.baidu.com/view/2238505.htm

The Rao-Blackwellized Particle Filtering (RBPF) is a combination of the particle filter and the Kalman filter, where each particle has a Kalman filter associated to it.

RBPF 参考：
Rao-Blackwellized Particle Filters and Localization: http://www2.informatik.uni-freiburg.de/~grisetti/teaching/ls-slam/lectures/pdf/ls-slam-06-rbpfloc.pdf
Thomas Schön, Fredrik Gustafsson, and Per-Johan Nordlund. “Marginalized Particle Filters for Mixed Linear/Nonlinear State-Space Models”. IEEE Transactions on Signal Processing, 53(7):2279-2289, Jul. 2005: http://user.it.uu.se/~thosc112/pubpdf/schongn2005.pdf

FastSLAM 参考：
Probabilistic Robotics, by Sebastian Thrun, 13 The FastSLAM Algorithm
http://ais.informatik.uni-freiburg.de/teaching/ss15/robotics/slides/14-slam-fastslam.pdf

SLAM 问题可以转换为图优化问题。

参考：
Probabilistic Robotics, by Sebastian Thrun, 11 The GraphSLAM Algorithm
Graph-based SLAM using Least Squares: http://ais.informatik.uni-freiburg.de/teaching/ws11/robotics2/pdfs/rob2-09-least-squares-slam.pdf
A Tutorial on Graph-Based SLAM: http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf
SLAM Back-End, by Giorgio Grisetti: http://kaspar.informatik.uni-freiburg.de/~kitutorial/pdfs/ki-pose-slam.pdf
g2o：A general Framework for (Hyper) Graph Optimization: https://svn.openslam.org/data/svn/g2o/trunk/g2o/doc/g2o.pdf

Visual SLAM (Vision-based SLAM) uses camera images to map out the position of a robot in a new environment.

参考：http://frc.ri.cmu.edu/~kaess/vslam_cvpr14/