PF粒子滤波

概念

粒子滤波法是指通过寻找一组在状态空间传播的随机样本对概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差分布的过程。

这里的样本即指粒子，当样本数量 N→∝时可以逼近任何形式的概率密度分布。
不用KF中的均值和方差来表示随机变量，用数量居多的采样点来表示！！！
因为一般要用到的采样点很多，所以也被称作粒子云滤波。
粒子滤波和其他的滤波器的比较就是，简单，简单，简单。

伪代码

Particle Filter伪代码

输入：x_t-1, u_t, z_t，分别为上一个时间点的状态向量，这个时间点的控制向量，这个时间点的测量向量。x_t [m] 是第 m 个粒子的意思。 x_t bar 也就是 x 上面有横杠的是所有粒子的平均值。

流程与实现

PF流程

初始化

设置N个粒子，且权重分别为1；
设置dx,dy,theta为正态分布随机数，表示噪声；

预测

通过物理模型预测车辆在下一个 sample time 的位置。

本次项目中只是利用了简单的自行车模型（其实预测车辆位置的时候，用到的车辆模型越复杂越好，前提是各种参数设置的都是正确的）

$\begin{aligned} x_{f} &=x_{0}+\frac{v}{\dot{\theta}}\left[\sin \left(\theta_{0}+\dot{\theta}(d t)\right)-\sin \left(\theta_{0}\right)\right] \\ y_{f} &=y_{0}+\frac{v}{\dot{\theta}}\left[\cos \left(\theta_{0}\right)-\cos \left(\theta_{0}+\dot{\theta}(d t)\right)\right] \\ \theta_{f} &=\theta_{0}+\dot{\theta}(d t) \end{aligned}$

添加dx,dy,theta噪声，为正态分布随机数；

更新粒子状态及粒子权重

根据车辆的预测位置，车辆的传感器，地图里面的地标的坐标计算各个粒子的权重和当下时间点的最终的车辆具体位置。

车辆传感器测量的值的选取：利用了 Nearest Neighborhood（NN）的原则，选取预测最近的目标作为测量值；
坐标系变换：将车辆坐标转到Map坐标；
$\left[\begin{array}{c}{x_{m}} \\ {y_{m}} \\ {1}\end{array}\right]=\begin{bmatrix}{\cos \theta} & {-\sin \theta} & {x_{p}} \\ {\sin \theta} & {\cos \theta} & {y_{p}} \\ {0} & {0} & {1}\end{bmatrix} \times\left[\begin{array}{c}{x_{c}} \\ {y_{c}} \\ {1}\end{array}\right]$

判断（计算）粒子的权重

判断（计算）粒子权重在这里的直观意思就是，越是跟正确的结果（landmark点）相近的粒子，他就越重要。

车辆会通过传感器获得 x 方向和与 y 方向两个参数。而且两个参数各自有自己的不确定性。所以要通过同时评估 x，y 的值来获得实际该粒子的权重。为了同时评估两个值的概率分布，这里用到了多元高斯概率分布：

重采样

根据各个粒子的权重（也就是概率）来随机可放回式的抽取。那么权重大的粒子被选择的可能性就大，权重小的粒子被选中的几率就小。当然，并不是说权重小就一定不会被选上，权重大就一定会被选上，都是概率的问题。

求出最终的 x，y 值

此时的 x，y 就是通过预测值，更新值，权重值一起估算出来的车辆在地图上的最终位置。

详情见代码：优达程序

应用

车辆定位：由高精地图和车载感知传感器得到自身位置。

问题

在一般情况下，应用粒子滤波困难重重。

那些粒子云经过多次迭代，很容易发生聚集，这样就无法精准的描述随机变量了，所以一般要择机向粒子云里稀疏的地方添加粒子，在太过稠密的地方减去粒子，不过谁能保证这么操作不会带来均值与方差的偏差？
此外为了达到精度要求，粒子的数量需要很大，这很难算的过来啊。

参考

无迹卡尔曼到底是什么东西？
学习笔记（优达学城）- 车辆定位之粒子滤波器（整合版）

PF粒子滤波

概念

伪代码

流程与实现

初始化

预测

更新粒子状态及粒子权重

重采样

求出最终的 x，y 值

应用

问题

参考

你可能感兴趣的:(PF粒子滤波)