符号对照表
| t时刻机器人位姿 | 线性运动噪声 | ||
|---|---|---|---|
| {
|
测量模型 | ||
| 第n各地标的位置 | 线性测量噪声 | ||
| 地图。包括n个地标的位置 | 第n_t个地标t时刻的期望观测 | ||
| t时刻传感器观测 | 量测更新 | ||
| {
|
更新协矩阵 | ||
| t时刻控制量 | 地标位姿的雅可比测量模型 | ||
| {
|
机器人位姿的雅可比测量模型 | ||
| t时刻所有的观测关联数据 | t时刻的FastSLAM粒子 | ||
| {
|
t时刻第m个FastSLAM粒子 | ||
| 运动模型 |
联合估计
SLAM，Simultaneous Localization and Mapping，实时定位与构图，既没有准确的机器人路径，也没有先验地图信息。
因为运动噪声，导致机器人的位姿是不准确的。
又因为测量噪声和机器人位姿的不确定性，导致与外界环境的相对位置不准，即构图不准确。
因此机器人位姿和地图应当同时进行估计。
后验估计
通过控制信息对运动噪声进行预测，即通过控制信息进行概率约束得到正确位姿。
通过观测信息对环境特征噪声进行测量，即通过观测信息进行概率约束得到机器人与环境的关系。
当获得当前时刻的观测信息时，不仅要更新当前的地图特征，还应当修正之前的地图特征。
后验概率的优点：
①增强噪声环境下实时定位与构图的鲁棒性
②可以比较不同环节之间的不确定性，即比较不同环节之间的后验概率分布
SLAM中的贝叶斯滤波器：
其中的联合后验概率为：
但实际上我们无法获得观测数据与地标之间的关联数据，即该观测数据是相对于哪一个地标的观测数据（地标一致性问题）。因此在上式中写作：
扩展卡尔曼滤波
EKF除了给出状态量的方差还需要给出各状态量之间的关系（用协方差矩阵表示）
EKF的两个缺点：
①复杂度
需要大量的计算量和存储空间
任何传感器信息的输入都需要对所有状态量进行更新
②单假定数据关联
我们给定的协方差阵和状态量之间的关联都是不准确的
EKF没有关于数据关联不确定性方面的机制，EKF所给出的数据关联是单一且确定的，因此上述误差不可逆转
我们可以通过同时考虑多个数据关联来提高EKF中数据关联的准确性，但需要付出一定的计算代价
SLAM的结构性及稀疏性
对于一个观测量来说，与其关联较大的状态量较少，因此可以根据这个特征进行稀疏矩阵的分解计算。
FastSLAM
SLAM就如同一个动态贝叶斯网络
若已知机器人的路径，则各个地标点的位置估计是相互独立的（原理参考d—分离）
则：
为路径后验
为地标估计
粒子滤波的递归更新
①采样新的机器人路径，给定新的控制
②根据观测对地标进行更新（EKF）
③为每个粒子分配权重
④粒子重采样
我们根据机器人路径进行后验估计，那么估计的位数应当随路径增加而增加
多假设数据关联
使用粒子滤波可以让每个粒子用不用的数据关联模型，铜鼓哦粒子滤波控制地标的增加与减少
计算权重时应考虑每个粒子间地标的不同，剔除个别粒子