粒子滤波定位

理论知识

除了线性状态估计的KF和非线性状态估计的EKF，还有一种可以解决非线性、非高斯问题的粒子滤波算法，粒子滤波主要基于蒙特卡洛方法，使用粒子集来表示概率。
粒子滤波主要分为四部分：初始化、预测、粒子权重更新、重采样，之后在重复的预测、更新、重采样，使得粒子逐渐向真实位置聚集。

1、初始化

粒子滤波初始化需要初始的位置（x,y），航向(yaw)，以及高斯噪声。

单纯用GPS的结果是有很大误差的，所以这边在初始化时，创造出N个粒子，并且所有粒子都是在GPS提供的初值附近的高斯分布。这是黑色圆圈出现的原因。每个粒子权重都相等，都是100%。
利用GPS的值初始化一定量粒子，粒子的**位置（x, y）, 航向角（yaw）**满足正态分布。GPS测量值为均值，上面黄框中的传感器噪音，即为分布方差。

2、预测

汽车的定位模块能够受到自车的车速v和航向角变化率yaw_rate，进而可以推测出运动△t时间后，自车所在的位置。由于每个粒子都可能是无人车的真实位置，当我们对所有粒子进行里程计预测（考虑运动噪音，也是满足正太分布），每个粒子都会有自己的位置和航向角。

接下来由于涉及了航向角，我们先介绍汽车的运动模型CTRV（恒定转向速率yaw_rate和速度v）。

状态向量为[px, py, v, yaw, yaw_rate]，位置(px,py)，速度(v)方向为运动轨迹切线，yaw为航向角，yaw_rate为航向角变化率。

由于行驶速度v和航向角的变化率φ’为常量，v的导数和φ’'均为0

根据航向角的变化率φ’是否为0，分为两种：
1、当φ’为0，即速度、航向角变化率、航向角均不发生变化，汽车此时沿直线行驶，只有位置发生变化。

2、当φ’不为0，是个常量，k和k+1时间段内汽车做圆周运动，状态方程为：

3、更新粒子权重

这些粒子哪些才能更好的代表汽车的位置了，所以要给它们相应的权重，权重越大，它被重采样的机会也就越大，这样随着不但迭代，最终会找到最能代表汽车位置的粒子了。

1、坐标系转换（从车系Oc到世界系Om）
汽车传感器会获得周围路标的位置(比如a,b,c三个路标)，将这些汽车坐标下的位置(比如a,b,c)转化为全局地图上的位置（a’, b’ ,c’）


2、数据关联、“临近法” 通过id绑定观测到的路标与地图中实际存在的路标

**3、将某粒子下观测到的所有路标的可信度累乘，就可以得到这个粒子的权重了 w=P(xa, ya)P(xb, yb)P(xc, yc)
利用联合高斯分布，求出每个路标的可信度P(x, y)。其中x, y为测量值，μx, μy 为对应路标地图上的位置。

4、重采样

重采样的目的是，保持粒子总数不变的情况下，删除掉那些权重较低的粒子，同时在权重较高的粒子附近抛洒新的粒子。对这些不同权重的粒子进行重新选取，权重大的粒子，代表的位置准确度更高，所以应该更多的重采样这些权重大的粒子。

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

代码讲解

1、初始化init()

粒子滤波初始化需要一个初始的位置、航向、测量误差。
利用GPS的值作为均值进行初始化。粒子的x,y,yaw都满足高斯分布，将均值和噪声作为已知量初始化N个粒子。
函数init()的声明以及各参数含义如下：

• @param x 初始的x（GPS估计值）
• @param y 初始的y
• @param theta 初始航向角[rad]
• @param std[] 上述3个的标准差

void init(double x, double y, double theta, double std[]);

/**
   * TODO: 设置粒子数目，初始化所有粒子在第一个位置的高斯分布，且所有权重都是1 
   * TODO: 对每个粒子在初始位置和初始航向的基础上加入一个随机的高斯分布测量误差
   */
void ParticleFilter::init(double x, double y, double theta, double std[]) {
   
    // 粒子数目N
    num_particles = 100;
    particles.resize(num_particles);

    // 根据均值和测量噪声建立呈现高斯分布的N个粒子
    std::default_random_engine gen;
    std::normal_distribution<double> dist_x(x, std[0]);// 均值和方差
    std::normal_distribution<double> dist_y(y, std[1]);
    std::normal_distribution<double> dist_theta(theta, std[2]);
    // 遍历所有粒子
    for (size_t i = 0; i < num_particles; ++i) {
   
        particles.at(i).x = dist_x(gen);
        particles.at(i).y = dist_y(gen);
        particles.at(i).theta = dist_theta(gen);
        particles.at(i).weight =