粒子滤波学习记录

首先放原网址 https://zhuanlan.zhihu.com/p/107223012
此处仅作为个人学习记录所用

先验地图

首先，假设我们有一张局部的高精度电子地图，在这个地图上有很多绿色的路标，对应的是高精度地图上的感兴趣点POI（Point of Interest）。
【但是有很多情况是没有先验高精度地图的，比如SLAM过程中。个人理解应该是先根据自身位置和观测生成一张初始地图，然后把这张初始地图中的路标点作为参考开始后续的粒子滤波推算。】

我们将当前的场景简化成一个平面坐标系，在该坐标系下选定某个参考点为原点Om（m代指map），Xm轴指向东，Ym轴指向北，在XmOmYm坐标下下，每个路标相对于原点均有唯一的坐标和ID。

一般情况下，高精度电子地图上的POI位置都是以经纬度的形式存储的，通过引入平面坐标系的方法即可将经纬度转化为局部坐标，常用于转换坐标的开源库有geolib、proj.4等，关于经纬度转局部坐标的理论不是本次分享的重点，这里不做过多阐述。后续的讨论都基于平面坐标系展开。
初始化
无人车定位系统初始化时，会收到GPS的初始观测信息，这些信息包括无人车在XmOmYm坐标系下的位置XY（图中黑点所在位置）和航向角θ（图中黑色箭头与X方向的夹角）。由于传感器本身的存在测量误差，无人车位置的真值应该在初始观测位置附近，即上图中的观测误差圈内。
大部分传感器的测量误差是符合高斯分布的，我们分别在初始观测的位置X、Y和航向角θ上加上高斯噪声，创造出N个粒子（图中橙色点）。初始创造时，每个粒子的权重都相等，值为100%。理论上，当N足够大时，总会有一个粒子的位置和航向角与真值一致，如果我们能够找到这个粒子，就能找到真值。这就是粒子滤波的基本思想。

预测
根据GPS的位置完成所有粒子的初始化后，无人车开始运动。在运动过程中，定位模块能够收到自车的车速v和航向角变化率yaw_rate等信息，进而可以推测出运动△t时间后，自车所在的位置。

根据车速、航向角变化率推算（预测）出自车位置的过程叫做里程计（Odometry）。除了可以使用车速和航向角变化率外进行位置推算外，IMU等传感器也可以实现类似的功能。

由于每个粒子都有可能是无人车的真实位置，因此我们对所有的粒子都进行预测（里程计推算），如下图所示。每个粒子有自己的初始位置和航向角，执行预测后，粒子群的分布也不再那么集中。
更新粒子权重
粒子预测完成后，我们需要对所有粒子的权重进行更新（所有粒子的初始权重都相等）。权重更新的依据是无人车对路标位置的感知。由于车载传感器是安装在车上的，感知到的路标位置都是基于车体坐标系的。

如下所示，(x0,y0)和(x1,y1)为传感器在车体坐标系下，探测到的两个路标。

我们将感知结果绘制到平面坐标系上（此过程包含了车体坐标系到平面坐标系的坐标转换）。为了方便读者查看，我这里只绘制三个比较典型的粒子所观测到的路标，如下所示，绿色的为地图上的路标，橙色粒子的观测结果，用橙色绘制，紫色和蓝色的同理。

从图上可以很直观地看出，在粒子O1、O2和O3中，传感器感知到的路标与实际中地图路标最匹配的是O1（橙色），其次是O2（蓝色），最后是O3（紫色），其中蓝色是由于位置原因导致的匹配性降低，紫色是由于航向原因导致的匹配性降低。

这也意味着经过权重更新后，所有的粒子由相同的权重，更新为权重：粒子O1>粒子O2>粒子O3，可以选择粒子O1作为无人车当前所在的位置。可以想象，当粒子数量足够多时，权重越高的粒子，其位置和航向就越接近真值。

重采样

利用前面提到的方法更新每个粒子的权重后，我们就要进行删除粒子和新增粒子的操作，这个删除和新增的过程就叫做重采样（Resample）。重采样的目的是，保持粒子总数不变的情况下，删除掉那些权重较低的粒子，同时在权重较高的粒子附近抛洒新的粒子。

随着重采样次数的增加，粒子的集中度会越来越高，留下来的都是极其接近真值的粒子。在这些粒子中筛选出权重最高的粒子，即为无人车的位置。

【重采样不会生成新的粒子，只是复制高权重的粒子。相同的高权重的粒子在预测阶段会加入随机的高斯噪声，所以预测结果并不会生成完全一样的粒子】