一 粒子滤波器简介

粒子叫作估计器estimator。估计过去叫平滑smoothing，估计未来叫预测prediction，估计当前值才叫滤波filtering。粒子滤波算法源于蒙特卡洛思想，即以某事件出现的频率来指代该事件的概率。通俗的讲，粒子滤波也是能用已知的一些数据预测未来的数据。我们知道，卡尔曼滤波限制噪声时服从高斯分布的，但是粒子滤波可以不局限于高斯噪声，原理上粒子滤波可以驾驭所有的非线性、非高斯系统。

二 形象比喻

在网上看到一个比较形象的比喻，很好的描述了粒子滤波的大体执行方法：

某年月，警方（跟踪程序）要在某个城市的茫茫人海（采样空间）中跟踪寻找一个罪犯（目标），警方采用了粒子滤波的方法。

1. 初始化：

警方找来了一批警犬（粒子），并且让每个警犬预先都闻了罪犯留下来的衣服的味道（为每个粒子初始化状态向量S0），然后将警犬均匀布置到城市的各个区（均匀分布是初始化粒子的一种方法，另外还有诸如高斯分布，即：将警犬以罪犯留衣服的那个区为中心来扩展分布开来）。

2. 搜索：

每个警犬都闻一闻自己位置的人的味道（粒子状态向量Si），并且确定这个味道跟预先闻过的味道的相似度（计算特征向量的相似性），这个相似度的计算最简单的方法就是计算一个欧式距离（每个粒子i对应一个相似度Di），然后做归一化（即：保证所有粒子的相似度之和为1）。

3. 决策：

总部根据警犬们发来的味道相似度确定罪犯出现的位置（概率上最大的目标）：最简单的决策方法为哪个味道的相似度最高，那个警犬处的人就是目标。

4. 重采样：

总部根据上一次的决策结果，重新布置下一轮警犬分布（重采样过程）。最简单的方法为：把相似度比较小的地区的警犬抽调到相似度高的地区。

上述，2,3,4过程重复进行，就完成了粒子滤波跟踪算法的全过程。

配合下面的图会更容易理解：

绿色的点就是罪犯，红色的点点都是粒子（也就是dog）。每一次狗都会获取到周围人的气味来判断周围有犯人的概率。然后通过resample来排除掉那些概率低的狗。

当然，犯人也不是傻子一直站在原地，他会移动，每一次的移动都会更新周围狗狗的概率。

这样，随着犯人的移动，就能最终准确的定位到这个家伙在哪里了，也就是粒子概率值最大的那些所处的位置。

三 粒子滤波实现步骤

从以上的粒子实现原理上很容易就能发现，在自动驾驶中（卡尔曼，无损卡尔曼）这些定位方法的核心步骤（predict(预测) update(更新)）

那么在自动驾驶上的实现步骤具体如何呢？

粒子滤波：

1. 初始化周围的粒子 （粒子的数量很有讲究，多了会影响定位效率，少了又不能准确表示物体准确的位置）
2. 更新（predict）
3. 2.1 雷达数据(车辆坐标 转 map坐标)

因为获取的雷达数据都是相对于汽车的坐标，为了之后的计算需要转换成粒子所在的地图坐标

公式：

2.2 根据周围标记物坐标获取范围内的坐标物 （欧几里得）

2.3 关联转换后的测量值和获取的坐标物 （近邻法，欧几里得）

2.4 根据关联的值计算每个粒子的权重（也就是概率值）

2.5 resample（对所有的粒子进行重采样，更新范围）

3. predict (预测)

根据物体的移动角速度 和移动速度等运动数据，预测每个粒子的位置

公式：

欢迎关注公众号 Self黑黑Driving 交流～～