粒子滤波简介

粒子滤波基于蒙特卡洛方法,用后验概率中随机抽取的粒子集对目标概率密度函数进行近似。本文将简要介绍如何用粒子滤波进行定位并附上相关代码实例。

粒子滤波概述

粒子滤波,和卡尔曼滤波、一维马尔科夫定位都是贝叶斯滤波的一种方法。其最大特点是原理与实现特别简单。

其核心思想是:用很多个粒子代表定位物体,每个粒子有权重ww代表该粒子位置的可信度。在prediction阶段,根据物体的控制信息uu(速度、转角等)与motion model预测出每个粒子下个时刻的位置;在update阶段,根据物体的观测值zz与地图值zlz_l计算出每个粒子的权重ww;在resample阶段,根据粒子的ww重新采样粒子。这样,粒子的位置会越来越趋近真实的物体位置。

其步骤图如下,包含下面几个步骤:

  1. 初始化:用gps初始化粒子群的位置
  2. prediction:根据motion model与物体的控制信息uu预测下个时刻粒子群中粒子的位置
  3. update:根据物体的观测值zz与地图值zlz_l计算出每个粒子的权重ww
  4. resample:根据粒子的ww重新采样粒子

其伪代码如下:

下面,将分阶段具体介绍粒子滤波。

Motion Model

在介绍粒子滤波之前,需要对定位的物体进行建模,我们定位的物体是汽车,使用的模型是Bicycle Model

Bicycle Model假定汽车的四个轮子可以简化成一个轮子,类似自行车故得此名。

初始化

初始化较为简单,根据GPS信息对每个粒子初始化即可,需要注意的是初始化的时候需要考虑到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).
    cout<<"Init Start"<<endl;

    num_particles = 100;


    /*******************************************************
     * Set particles
     ******************************************************/
    double std_x = std[0];
    double std_y = std[1];
    double std_theta = std[2];

    // set random engine
    normal_distribution<double> dist_x(x, std_x);
    normal_distribution<double> dist_y(y, std_y);
    normal_distribution<double> dist_theta(theta, std_theta);

    for (int i = 0; i < num_particles; ++i) {
        Particle curr_particle;
        curr_particle.id = i;
        curr_particle.x = dist_x(gen);
        curr_particle.y = dist_y(gen);
        curr_particle.theta = dist_theta(gen);
        curr_particle.weight = 1.0;
        particles.push_back(curr_particle);
    }

    /******************************************************
     * Set others
     ******************************************************/
    weights.assign(num_particles, 1.0);
    is_initialized = true;

    cout<<"Weights Size: "<<weights.size()<<endl;
    cout<<"Particles Size: "<<particles.size()<<endl;
    cout<<"Init Finished"<<endl;
}

Prediction

Prediction阶段,即:

此处根据yaw_rate是否为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/


    double std_x = std_pos[0];
    double std_y = std_pos[1];
    double std_theta = std_pos[2];
    normal_distribution<double> dist_x(0, std_x);
    normal_distribution<double> dist_y(0, std_y);
    normal_distribution<double> dist_theta(0, std_theta);


    for (int i = 0; i < num_particles; ++i) {
        // main
        if (fabs(yaw_rate)>0.001){
            double theta0 = particles[i].theta;
            particles[i].x += velocity/yaw_rate * ( sin(theta0+yaw_rate*delta_t) - sin(theta0) );
            particles[i].y += velocity/yaw_rate * ( -cos(theta0+yaw_rate*delta_t) + cos(theta0) );
            particles[i].theta += yaw_rate * delta_t;
        } else {
            particles[i].x += velocity * delta_t * cos(particles[i].theta);
            particles[i].y += velocity * delta_t * sin(particles[i].theta);
        }
        // add random noise
        particles[i].x += dist_x(gen);
        particles[i].y += dist_y(gen);
        particles[i].theta += dist_theta(gen);
    }

}

Update

更新粒子权重的依据是粒子的观测值与地图标志物相似度的高低,越高的话该粒子的权重越大。可是,该过程存在如下问题:

  1. 粒子观测值的坐标系是以粒子为中心的,粒子前进方向为x轴的坐标系,不同与地图标志物的坐标系。需要统一到地图坐标系。
  2. 地图的标志物太多,只需要考虑粒子传感器范围内的标志物即可。
  3. 粒子观测值与地图标志物需要建立一一匹配的关系。
  4. 更新粒子的权重。

下面,将针对每点分开介绍。

坐标变换

坐标变换,需要求解的问题如下:

在求解该问题之前,引入坐标变换的相关公式:

对于上图中的坐标x,y到坐标x′,y′,其变换公式如下:

对于粒子滤波器中的坐标变换,需要将小车的观测值变换到地图坐标系下粒子的观测值。

记粒子的方向顺时针离水平位置的角度是θ\theta,那么从粒子的坐标系到地图的坐标系变换的角度为−θ- \theta,所以有:

经过上述公式变换后的坐标是以粒子为圆心,在地图坐标系下的坐标,考虑到粒子的坐标,经过平移变换后可得:

以上图为例,OBS1变换后的坐标为:(6,3)。

选择传感器范围内的标识物

获得粒子在地图坐标系下的观测值后,需要对地图中的标志物进行筛选,筛选的标准是标志物到粒子的距离需要小于传感器的感受距离。

Association

经过上面对观测值的坐标变换以及传感器范围内的筛选,对于每个粒子可以获得其观测标志物在地图坐标系下的坐标值以及地图标志物本身的坐标值,下一个问题就是怎么将这两项坐标值两两匹配。

比较简单的方法是,对于每个观测值,选择离其最近的标志物进行匹配即可。该方法的优缺点如下:

Update

对观测值与地图真实的标志物两两匹配后,下一个阶段是计算匹配是否可信,也就是对每个粒子计算其权重ww。

一个粒子可能包含多个观测值,每个观测值服从以匹配标志物为中心的多元高斯分布。具体地:

代码

void ParticleFilter::dataAssociation(std::vector<LandmarkObs> predicted, std::vector<LandmarkObs>& observations) {
    // TODO: Find the predicted measurement that is closest to each observed measurement and assign the
    //   observed measurement to this particular landmark.
    // NOTE: this method will NOT be called by the grading code. But you will probably find it useful to
    //   implement this method and use it as a helper during the updateWeights phase.


    const double BIG_NUMBER = 1.0e99;

    for (int i = 0; i < observations.size(); i++) {

        int current_j, id_j;
        double current_smallest_error = BIG_NUMBER;

        for (int j = 0; j < predicted.size(); j++) {

            const double dx = predicted[j].x - observations[i].x;
            const double dy = predicted[j].y - observations[i].y;
            const int id_j = predicted[j].id;
            const double error = dx * dx + dy * dy;

            if (error < current_smallest_error) {
                current_j = j;
                current_smallest_error = error;
            }
        }
        observations[i].ii = current_j;
        observations[i].id = id_j;
    }

}


void ParticleFilter::updateWeights(double sensor_range, double std_landmark[],
                                   const std::vector<LandmarkObs> &observations, const 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

    // iterate particles
    weights.clear();
    for (int i = 0; i < num_particles; ++i) {

        // get particle infomation
        double x_p = particles[i].x;
        double y_p = particles[i].y;
        double theta = particles[i].theta;

        // clear
        particles[i].associations.clear();
        particles[i].sense_x.clear();
        particles[i].sense_y.clear();

        /*****************************************************
         * Step 1: coordinate system change from VEHICLE'S to MAP'S
         ****************************************************/
        vector<LandmarkObs> map_observations;
        for (int j = 0; j < observations.size(); ++j) {
            double x_o = observations[j].x;
            double y_o = observations[j].y;

            double x_m, y_m;
            x_m = x_p + cos(theta) * x_o - sin(theta) * y_o;
            y_m = y_p + sin(theta) * x_o + cos(theta) * y_o;

            LandmarkObs observation = {
                    observations[j].id,
                    0,
                    x_m,
                    y_m
            };

            map_observations.push_back(observation);
        }

        /*****************************************************
         * Step 2: choose landmarks which has the distance with particle less than sensor_range
         ****************************************************/
        vector<LandmarkObs> effect_landmark_list;
        for (int k=0; k < map_landmarks.landmark_list.size(); ++k) {
            float x_l_i = map_landmarks.landmark_list[k].x_f;
            float y_l_i = map_landmarks.landmark_list[k].y_f;
            int id_l_i = map_landmarks.landmark_list[k].id_i;
            double dist = sqrt( pow(x_p-x_l_i, 2) + pow(y_p-y_l_i, 2) );
            if (dist < sensor_range){
                LandmarkObs curr_landmark = {
                        id_l_i,
                        0,
                        x_l_i,
                        y_l_i
                };
                effect_landmark_list.push_back(curr_landmark);
            }
        }


        /*****************************************************
         * Step 3: associate landmarks in range to landmarks obeservations
         ****************************************************/
        ParticleFilter::dataAssociation(effect_landmark_list, map_observations);

        /*****************************************************
         * Step 4: update weight
         ****************************************************/
        double weight_p = 1.0;
        if (effect_landmark_list.size()==0){
            weight_p = 0.0;
        } else{
            for (int j = 0; j < map_observations.size(); ++j) {
                // obeseration im map coordinate
                double x_m = map_observations[j].x;
                double y_m = map_observations[j].y;
                // landmark im map coordinate
                double x_l = effect_landmark_list[map_observations[j].ii].x;
                double y_l = effect_landmark_list[map_observations[j].ii].y;

                double std_x = std_landmark[0];
                double std_y = std_landmark[1];
                double p_one_landmark = 1/(2*M_PI*std_x*std_y)*exp(-( pow(x_m-x_l,2)/(2*std_x*std_x) + pow(y_m-y_l,2)/(2*std_y*std_y) ));

                weight_p *= p_one_landmark;

                particles[i].associations.push_back(map_observations[j].id);

                // !!!!! I need help in follow two lines, please help me!!!!!
                // !!!!! I cannot run my program when I uncomment the follow two lines. Why and How can I solve it?
                //particles[i].sense_x.push_back(x_m);
                //particles[i].sense_y.push_back(y_m);
            }
        }

        // update particle
        particles[i].weight = weight_p;
        weights.push_back(weight_p);
    }
}

Resample

Update阶段获得每个粒子的权重后,根据权重重新筛选粒子即可,代码如下:

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
    discrete_distribution<int> index (weights.begin(), weights.end());

    std::vector<Particle> new_particles;
    for (int i = 0; i < num_particles; ++i) {
        new_particles.push_back(particles[index(gen)]);
    }
    particles = new_particles;

}

实例

相关代码及完整程序可见YoungGer的Github

我的博客即将同步至腾讯云+社区,邀请大家一同入驻。

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

发表于

我来说两句

0 条评论
登录 后参与评论

相关文章

来自专栏大数据文摘

主成分分析(PCA)在R 及 Python中的实战指南

3178
来自专栏深度学习之tensorflow实战篇

因子分析与主成分分析之间爱恨离愁。FA与FCA

主成分分析和因子分析无论从算法上还是应用上都有着比较相似之处,本文结合以往资料以及自己的理解总结了以下十大不同之处,适合初学者学习之用。 1.原理不同 主成分分...

3579
来自专栏生信技能树

一文看懂主成分分析

主成分分析法是数据挖掘中常用的一种降维算法,是Pearson在1901年提出的,再后来由hotelling在1933年加以发展提出的一种多变量的统计方法,其最主...

3K3
来自专栏小樱的经验随笔

快速傅里叶变换(FFT)算法【详解】

快速傅里叶变换(Fast Fourier Transform)是信号处理与数据分析领域里最重要的算法之一。我打开一本老旧的算法书,欣赏了JW Cooley 和 ...

4689
来自专栏机器学习算法与理论

最新姿态估计研究进展

最新姿态估计研究进展 自上而下:就是先检测包含人的框,即human proposal,然后对框子中的人进行姿态估计。一般RCNN(区域CNN就是这个思路) 自下...

7846
来自专栏Petrichor的专栏

深度学习: RPN (区域候选网络)

Note: two stage型的检测算法在RPN 之后 还会进行 再一次 的 分类任务 和 边框回归任务,以进一步提升检测精度。

1322
来自专栏CVer

谷歌CVPR 2018最全总结:45篇论文,Ian Goodfellow GAN演讲PPT下载

谷歌在今年的CVPR上表现强势,有超过200名谷歌员工将在大会上展示论文或被邀请演讲,45篇论文被接收。在计算机视觉领域,生成对抗网络GAN无疑是最受关注的主题...

2123
来自专栏AI科技大本营的专栏

谷歌大脑深度学习从入门到精通视频课程[6.4]:自动编码器——线性自动编码器

AI100 已经引入 Hugo Larochelle 教授的深度学习课程,会在公众号中推送,并且对视频中的 PPT 进行讲解。课后,我们会设计一系列的问题来巩...

3267
来自专栏CDA数据分析师

【从零开始学统计】3.置信度置信的到底是什么?

连载系列3:置信度置信的到底是什么? 前两期楼主分别作了均值和拟合优度的专题,今天就来说说置信度。 要说置信度,首先老师肯定会在此前已经介绍过了点估计了,那么引...

18310
来自专栏大数据文摘

21副GIF动图让你了解各种数学概念

1645

扫码关注云+社区