粒子滤波在车辆定位中的应用

粒子滤波算法流程

粒子滤波算是一个非常经典的算法了，具体的思想步骤不再详述，这里和这里讲的都比较详细了。
下图就是算法的伪代码，分为以下几个步骤：

• 第一行是算法的初始化，将在这一步根据GPS的数据进行一个粗略的位置估计。
• 第三行是预测，为所有的粒子添加yaw rate和velocity
• 四五行是更新，更新每个粒子的权重（权重就是之后resample的依据）
• 最后一个for循环就是重采样，权重大的点更有可能被选中

下图是粒子滤波车辆定位算法的流程图。

初始化

在车辆定位问题中，不能将所有点的分布视为平均分布，比较好的办法就是根据GPS的信息，并考虑到传感器的高斯噪声来随机撒点。具体的做法就是将GPS读取的信息作为均值，传感器高斯噪声作为方差，让所有点服从这两个参数下的正态分布，代码如下：

void ParticleFilter::init(double x, double y, double theta, double std[]) {
   
    default_random_engine gen;
    normal_distribution<double> dist_x(x, std[0]);
    normal_distribution<double> dist_y(y, std[1]);
    normal_distribution<double> dist_theta(theta, std[2]);
    
    num_particles = NUM_PARTICLES;  // TODO: Set the number of particles
    particles.resize(num_particles);
    
    for(auto &p : particles){
   
        p.x = dist_x(gen);
        p.y = dist_y(gen);
        p.theta = dist_theta(gen);
        p.weight = 1;
    }
    is_initialized = true;

}

预测

更新需要根据车辆的运动学方程，匀速运动的车不必多说，单位时间内的位置更新就是简单的速度*夹角正弦或者余弦。如果是变速运动，那么用如下公式计算：

当然，我们仍然需要叠加上高斯噪声，具体代码如下：

void ParticleFilter::prediction(double delta_t, double std_pos[], 
                                double velocity,