粒子群滤波

伪代码如图：

首先，值得注意的是，最上面标题中有x_t-1, u_t, z_t。 这说明，粒子滤波器的input就是这三个东西（当然还要有计算其他东西所需要的参数）。

他们分别是：

x_t-1上一个时间点的状态向量；

u_t这个时间点的控制向量；

z_t这个时间点的测量向量；

x_t^[m] 是第m个粒子的意思。

x_t bar 也就是x上面有横杠的是所有粒子的平均值。

算法顺序就是

1. 初始化粒子。设置N个粒子，每个粒子权重相同。
2. for 的循环里面， 对每一个粒子根据状态转移矩阵进行预测。预测之后，根据式子4得到每一个粒子对应的权重（根据测量值和预测值），更新每一个粒子。
3. 重采样。N个粒子中，根据每个粒子的权重，判断是否在下一个循环中是否要使用这个粒子。粒子对应权重越高（也就是根据状态转移矩阵得到的预测值和车子身上的传感器得到的值约相似），他被选取的概率就越大。也就是说，一个有99%权重的粒子，也有1%不被选择使用的可能性。
4. 更新每个粒子的权重并根据重采样可放回的随机抽取，最终通过抽取出来的粒子求得实际车辆位置。

流程如下：

第一步：初始化：

//使用GPS数据初始化滤波器
void ParticleFilter::init(double x, double y, double theta, double std[]) {
    // TODO: Set the number of particles. Initialize all particles to first position (based on estimates of 
    //   x, y, theta and their uncertainties from GPS) and all weights to 1. 
    // Add random Gaussian noise to each particle.
    // NOTE: Consult particle_filter.h for more information about this method (and others in this file).
    default_random_engine gen;
    
    //建立1000个粒子
    num_particles = 1000;
    
    // Creates a normal (gaussian) distribution for x, y, theta
    normal_distribution<double> dist_x(x, std[0]);
    normal_distribution<double> dist_y(y, std[1]);
    normal_distribution<double> angle_theta(theta, std[2]);

    // Resize the vector(weights & particles)
    weights.resize(num_particles);
    particles.resize(num_particles);

    // Initial weight value
    //均匀分布，都为1
    float weight_0 = 1.0 / num_particles;

    // Initialization of weights and particles
    for (int i = 0; i < num_particles; ++i)
    {
        particles[i].id = i;
        particles[i].x  = dist_x(gen);
        particles[i].y  = dist_y(gen);
        particles[i].theta  = angle_theta(gen);
        particles[i].weight = weight_0; 
    }

    // Initialization done
    is_initialized = true;
}

第二步：预测

当角速度不为0时，小车的位姿预测公式如下：

//根据运动模型预测例子的运动
void ParticleFilter::prediction(double delta_t, double std_pos[], double velocity, double yaw_rate) {
    // TODO: Add measurements to each particle and add random Gaussian noise.
    // NOTE: When adding noise you may find std::normal_distribution and std::default_random_engine useful.
    //  http://en.cppreference.com/w/cpp/numeric/random/normal_distribution
    //  http://www.cplusplus.com/reference/random/default_random_engine/

    default_random_engine gen;

    // Create a normal (gaussian) distribution for x, y, theta
    normal_distribution<double> dist_x(0.0, std_pos[0]);
    normal_distribution<double> dist_y(0.0, std_pos[1]);
    normal_distribution<double> angle_theta(0.0, std_pos[2]);

    // Prediction
    for (int i = 0; i < num_particles; ++i)
    {
        if (fabs(yaw_rate) > 0.01) // If yaw rate in not zero
        {
            float x_ = particles[i].x;
            float y_ = particles[i].y;
            float theta_ = particles[i].theta;

            particles[i].x = x_ + (velocity / yaw_rate) * (sin(theta_ + yaw_rate * delta_t) - sin(theta_));
            particles[i].y = y_ + (velocity / yaw_rate) * (cos(theta_) - cos(theta_ + yaw_rate * delta_t));
            particles[i].theta = theta_ + yaw_rate * delta_t;
        }
        else // If yaw rate is close to zero
        {
            float x_ = particles[i].x;
            float y_ = particles[i].y;
            float theta_ = particles[i].theta;

            particles[i].x = x_ + velocity * delta_t * cos(theta_);
            particles[i].y = y_ + velocity * delta_t * sin(theta_);
        }

        // Add Noise
        particles[i].x += dist_x(gen);
        particles[i].y += dist_y(gen);
        particles[i].theta += angle_theta(gen);
    }
}

第三步：更新权重

现在，我们已经完成了测量变换和关联，我们有了计算粒子最终重量所需的所有部分。粒子的最终重量将作为每次测量的多元高斯概率的乘积来计算.

多元高斯概率有两个维，即x和y，多元高斯的均值是与测量相关的地标位置，多元高斯标准差用x和y区间的初始不确定度来描述。在变换后的测量点对多元高斯函数进行了估计.多元高斯的公式如下所示.

 

void ParticleFilter::updateWeights(double sensor_range, double std_landmark[], 
        std::vector<LandmarkObs> observations, Map map_landmarks) {
    // TODO: Update the weights of each particle using a mult-variate Gaussian distribution. You can read
    //   more about this distribution here: https://en.wikipedia.org/wiki/Multivariate_normal_distribution
    // NOTE: The observations are given in the VEHICLE'S coordinate system. Your particles are located
    //   according to the MAP'S coordinate system. You will need to transform between the two systems.
    //   Keep in mind that this transformation requires both rotation AND translation (but no scaling).
    //   The following is a good resource for the theory:
    //   https://www.willamette.edu/~gorr/classes/GeneralGraphics/Transforms/transforms2d.htm
    //   and the following is a good resource for the actual equation to implement (look at equation 
    //   3.33
    //   http://planning.cs.uiuc.edu/node99.html
    
    for (int i = 0; i < num_particles; ++i)
    {
        // Particle values
        float id_ = particles[i].id;
        float x_  = particles[i].x;
        float y_  = particles[i].y;
        float theta_  = particles[i].theta;
        float weight_ = particles[i].weight; 
        
        // Transformation for OBS
        vector<LandmarkObs> obs_trans;

        // Observation transformation
        for (int j = 0; j < observations.size(); ++j)
        {
            int   obs_id = observations[j].id;
            float obs_x  = observations[j].x;
            float obs_y  = observations[j].y;

            float trans_obs_x = x_ + cos(theta_) * obs_x - sin(theta_) * obs_y;
            float trans_obs_y = y_ + sin(theta_) * obs_x + cos(theta_) * obs_y;

            // .push_back: add a new element at the end of the vector
            obs_trans.push_back(LandmarkObs{ obs_id, trans_obs_x, trans_obs_y });
        }

        // Predicted
        std::vector<LandmarkObs> predicted;
        
        // choose landmark which is in the sensor range
        for (int j = 0; j < map_landmarks.landmark_list.size(); ++j)
        {
            int   land_id = map_landmarks.landmark_list[j].id_i;
            float land_x  = map_landmarks.landmark_list[j].x_f;
            float land_y  = map_landmarks.landmark_list[j].y_f;

            if (sqrt((x_ - land_x) * (x_ - land_x) + (y_ - land_y) * (y_ - land_y)) < sensor_range)
            {
                // Add landmark
                predicted.push_back(LandmarkObs{land_id, land_x, land_y});
            }
        }

        // Data Association
        dataAssociation(predicted, obs_trans);

        // Weight Initialization
        particles[i].weight = 1;

        for (int j = 0; j < obs_trans.size(); ++j)
        {
            int   trans_obs_id = obs_trans[j].id;
            float trans_obs_x  = obs_trans[j].x;
            float trans_obs_y  = obs_trans[j].y;

            // find predicted which has same id with observation
            float predicted_x;
            float predicted_y; 

            for (int k = 0; k < predicted.size(); ++k)
            {
                if (trans_obs_id == predicted[k].id)
                {
                    predicted_x = predicted[k].x;
                    predicted_y = predicted[k].y;
                }
            }

            float sigma_x = std_landmark[0];
            float sigma_y = std_landmark[1];

            float x2 = (trans_obs_x - predicted_x) * (trans_obs_x - predicted_x);
            float y2 = (trans_obs_y - predicted_y) * (trans_obs_y - predicted_y);
            float sigma_x2 = sigma_x * sigma_x;
            float sigma_y2 = sigma_y * sigma_y;

            float P_ = (1 / (2 * M_PI * sigma_x * sigma_y)) * exp( - ((x2 / (2 * sigma_x2))  + (y2 / (2 * sigma_y2))));

            particles[i].weight *= P_;
        }
    }
}

第四步：重采样

void ParticleFilter::resample() {
    // TODO: Resample particles with replacement with probability proportional to their weight. 
    // NOTE: You may find std::discrete_distribution helpful here.
    //   http://en.cppreference.com/w/cpp/numeric/random/discrete_distribution
    
    default_random_engine gen;
    uniform_real_distribution<float> distribution(0.0, 1.0);
    
    float beta = 0.0;

    // Find max weight
    float w_max = 0;
    for (int i = 0; i < num_particles; ++i)
    {
        if (particles[i].weight > w_max) 
        {
            w_max = particles[i].weight;
        }
    }
    
    int index = int(distribution(gen) * num_particles);

    // Resampling wheel
    for (int i = 0; i < num_particles; ++i)
    {
        beta = beta + distribution(gen) * 2.0 * w_max;
        while (particles[index].weight < beta) 
        {
            beta = beta - particles[index].weight;
            index = (index + 1) % num_particles;
        }
        particles[i] = particles[index];
    }

}

整个流程

#include <iostream>
#include <math.h>
#include "particle_filter.h"

using namespace std;

#define GET_DATA 1  //改成自己获取数据的代码



int main()
{


    //Set up parameters here
    double delta_t = 0.1; // Time elapsed between measurements [sec]
    double sensor_range = 50; // Sensor range [m]

    double sigma_pos [3] = {0.3, 0.3, 0.01}; // GPS measurement uncertainty [x [m], y [m], theta [rad]]
    double sigma_landmark [2] = {0.3, 0.3}; // Landmark measurement uncertainty [x [m], y [m]]

    // Read map data
    Map map;
    if (!read_map_data("../data/map_data.txt", map)) {
        cout << "Error: Could not open map file" << endl;
        return -1;
    }

    // Create particle filter
    ParticleFilter pf;


    while(1) {


        if (!pf.initialized()) {

    // Sense noisy position data from the GPS
    //使用GPS数据初始化
            double sense_x = GET_DATA;
            double sense_y = GET_DATA;
            double sense_theta = GET_DATA;
            pf.init(sense_x, sense_y, sense_theta, sigma_pos);
        } else {
    // Predict the vehicle's next state from previous (noiseless control) data.
            double previous_velocity = GET_DATA;
            double previous_yawrate = GET_DATA;
            pf.prediction(delta_t, sigma_pos, previous_velocity, previous_yawrate);
        }

    //获取传感器数据
        vector<LandmarkObs> noisy_observations;

        std::vector<float> x_sense;
        std::back_inserter(x_sense);

        std::vector<float> y_sense;
        std::back_inserter(y_sense);

        for (int i = 0; i < x_sense.size(); i++) {
            LandmarkObs obs;
            obs.x = x_sense[i];
            obs.y = y_sense[i];
            noisy_observations.push_back(obs);
        }

    // Update the weights and resample
        pf.updateWeights(sensor_range, sigma_landmark, noisy_observations, map);
        pf.resample();

    // Calculate and output the average weighted error of the particle filter over all time steps so far.
        vector<Particle> particles = pf.particles;
        int num_particles = particles.size();
        double highest_weight = -1.0;
        Particle best_particle;
        double weight_sum = 0.0;
    //计算最大权重、总权重和对应的最优粒子
        for (int i = 0; i < num_particles; ++i) {
            if (particles[i].weight > highest_weight) {
                highest_weight = particles[i].weight;
                best_particle = particles[i];
            }
            weight_sum += particles[i].weight;
        }
        cout << "highest w " << highest_weight << endl;
    //    cout << "average w " << weight_sum / num_particles << endl;
    //
    //    cout << "best_particle_x" << best_particle.x << endl;
    //    cout << "best_particle_y" << best_particle.y << endl;
    //    cout << "best_particle_theta" << best_particle.theta << endl;
    //
    //    cout << "best_particle_associations" << pf.getAssociations(best_particle) << endl;
    //    cout << "best_particle_sense_x" << pf.getSenseX(best_particle) << endl;
    //    cout << "best_particle_sense_y" << pf.getSenseY(best_particle) << endl;

    }



}

 误差估计：