自动驾驶-使用粒子滤波定位

前几章介绍了卡尔曼滤波是用来对其他车辆追踪和定位的，本章介绍的粒子滤波是用来定位自身的。通常我们用手机地图的gps功能已经能大致定位出自身位置，但是gps存在10m左右的偏差，这么大的偏差在自动驾驶时是无法承受的。本章介绍的粒子滤波是一种高精度定位自身位置的算法。

粒子滤波的原理概括来说是通过地图得到一些路标的坐标，假设自身车辆以某种概率分布在一定范围内，在这个范围内按概率分布假设一些粒子代表车辆位置，根据车辆运动情况预测每个粒子$\Delta t$以后的位置，车辆每隔$\Delta t$的时间还会用传感器感知路标相对自己的距离，用每个粒子周围路标的位置和传感器感知的路标位置做比较，传感器感知的路标和探测到的路标越接近则该粒子越有可能代表车辆位置。从预测到探测更新循环迭代，确认车辆的实际位置。

粒子滤波可以抽象为以下几步：

1. 初始化粒子
2. 预测
3. 探测更新
4. 重新分布粒子，重复2， 3步

下图是自动驾驶车辆使用粒子滤波的一般流程

粒子滤波是一种比卡尔曼滤波更加简单的算法，下面对每一步进行更详细的说明并给出源码实现加深理解。

初始化粒子

第一步是撒粒子，但是粒子应该撒到什么位置呢？又该按什么规律撒呢？和其他传感器一样，gps数据应该也满足正态分布。所以先通过gps得到一个位置不精确的位置坐标 ( x , y ) (x, y)