4.1 Belief state in 2D
4.1.1 Introduction
How robots can keep track of where they are in space and time using these particle filter algorithms to represent their uncertainty.
1) explore how robots represent the pose, that is where they're located and how they can estimate this over time using a localization algorithm.
确定机器人的初始位姿
2) given a map, how can we use the information in the map to help us build up this registration of the pose to the map;
求取传感器测量结果，又称为地图配准，配准的作用是将测量结果与地图结果相匹配。
3) explore a canonical algorithm called the particle filter which robots can use to estimate their localization information using sensors.
利用粒子滤波器基于当前位姿与机器人测量结果估计机器人下一步的位姿。
4.1.2 Odometry Modeling
几种常见的定位方式
里程计更新方式与机器人模型相关。
以差速小车为例子，介绍一种简单的里程计实现方法。
从编码器可以读取内、外侧轮子的运动距离，即机器人转弯的内、外弧长；结合已知的内外侧轮子距离，可以计算转弯角度。
计算内、外弧长的平均值作为机器人坐标系的弧长，并近似为扇形边的长度。
x、y的计算公式善存疑问。
由于轮子滑移，结合陀螺仪计算机器人的转弯角度
4.2 Map Registration
二维雷达。雷达数据如下图中的离散黑色点。
地图以占据珊格形式表示。珊格地图提供我们期望的机器人观测获得的准确信息。
激光雷达的测量提供机器人当前时刻观测到的信息。
有了地图信息和激光雷达的观测数据，目标在于找到与最优观测结果相匹配的机器人姿态。
在所有可能姿态中搜索机器人位姿是困难的。结合4.1节的内容，我们已知上一时刻机器人的位姿，通过编码器读数获取机器人从上一时刻到当前时刻的里程信息，预测机器人当前时刻的位姿。因此，可以将搜索范围限制在预测位姿附近，对搜索结果也具有更高的置信度。
评估每一个预测位姿的观测信息的配准质量。
第一项为观测信息。对于每一个位姿p，其观测到的珊格的log-odd为正。
第二项为地图信息。通过珊格索引，它能够提供groundtruth与观测结果做对比。
对于给定位姿，如果观测结果与实际地图匹配成功则给正分，否则给负分。
此外，还可以考虑被激光穿过的点来进一步提高配准质量。不足之处在于增大计算量。
使用简单的高斯模型来描述位置和姿态的不确定性是不够的，下一节将介绍一种能描绘双模不确定性和非线性模型的姿态滤波器和一种可行的计算方法。
4.3 Particle Filters
粒子滤波是贝叶斯滤波的一种非参数实现。其主要思想是用一系列从后验得到的随机状态样本表示后验bel(x)。
不同于完整定义的概率模型，粒子滤波器用粒子群表示一组采样的分布。
采样点的统计特性与分布的统计特性是一致的，例如均值和协方差，甚至还可以是更复杂的统计特性。
一群粒子表征一个高斯混合模型
方差为0的情况下，高斯分布变成Dirac Delta函数？？？（不懂）
初始化粒子群
一群粒子表征了当前时刻的状态分布。
里程计更新
粒子群根据里程计提供的位姿变化数据进行移动，里程计的噪声使粒子位姿产生发散。
配准更新
里程计更新之后，还可以将观测数据配准地图来更新粒子群分布。
使用当前粒子的权值wi作为先验，
粒子群重采样
经过里程计更新和配准更新以后，粒子群分散，很多粒子的权值下降。因此需要剔除这部分权值很小的噪声点，重新采样。
所有粒子的累计权值为1，重采样方法原理类似轮盘赌
4.4 ICP
问题描述
ICP算法
给定R和t初始值，得到两个点集的初始匹配，构造两个点集之间的距离公式，通过迭代，寻找使距离为最小值的R和t。
