概率机器人（Probability Robotics）笔记 Chapter 10: SLAM

1. 简介

SLAM问题的输入只有测量和控制，没有地图和位姿估计。

有两种同等重要的SLAM问题：

1. 在线SLAM（online SLAM）:
   同时估计地图和即时位姿这两个后验，即$p(x_t,m|z_{1:t},u_{1:t})$。
   称为在线SLAM的原因是，只估计时间$t$存留的变量。
   很多在线SLAM算法是递增的，即只处理当前一帧观测和控制。
2. 完整SLAM（full SLAM）:
   估计整个位姿轨迹与地图，即$p(x_{1:t},m|z_{1:t},u_{1:t})$。
   对之前的状态KaTeX parse error: Can't use function '\.' in math mode at position 5: x_1,\̲.̲.. x_{t-1}在状态空间上进行积分，既可以得到在线SLAM。

SLAM问题的另一个特点与估计问题的本质相关，即包含一个连续和一个离散问题：
连续问题与地图中物体的位置和机器人的位姿相关；
离散问题与物体的关联相关。
有时，显式估计地标的关联是有利的，在线后验为$p(x_t,m,c_t|z_{1:t},u_{1:t})$。

不管是哪种SLAM问题，估计全部后验分布都是最好的，但通常不可行：

1. 连续参数空间维度太高（地图）；
2. 离散关联变量的数量太大，很多算法的地图包括上万个特征。

本章介绍EKF算法，解决在线SLAM问题。

2. EKF SLAM

2.1 设定与假设

最早、最有影响力的SLAM算法就是基于EKF的。
EKF SLAM算法使用最大可能性数据关联，将EKF用于在线SLAM问题。
包含了多个近似和有限制的假设：

1. 特征地图：地标数量较少（如一千个），且EKF本来就希望特征的模糊度小。
   EKF SLAM通常需要在特征探测上下功夫。
2. 高斯噪声: 假设运动和测量有高斯噪声。
   后验的不确定性尽量小，否则线性化误差大。
3. 正测量: 只能处理观测到地标这种正信息，而不能处理没看到地标的负信息。

2.2 已知关联下的SLAM

即只解决SLAM问题的连续部分。
同时估计地标的位置和机器人的位姿，需要把地标的坐标加入状态向量中，即
$y_t=(xy\theta m_{1,x}{m}_{1,y}{s}_1...m_{N,x}{m}_{N,y}{S}_N)$
$N$是地图中地标总数，EKF估计在线后验$p(y_t|z_{1:t},u_{1:t})$。

算法 EKF_SLAM_known_correspondences(${\mu }_{t-1},{\Sigma }_{t-1},u_t,z_t,c_t$):

02························
$$F_x=\left(\begin{array}{cccccc}1& 0& 0& 0& \cdots & 0\\ 0& 1& 0& 0& \cdots & 0\\ 0& 0& 1& 0& \cdots & 0\end{array}\right)$$
矩阵右边是2N列个0。
是系数矩阵。

03························
$${\bar{\mu }}_t={\mu }_{t-1}+F$$