粒子滤波概述

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

Motion Model

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

初始化

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.
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阶段，即：

xt+1=f(xt,ut)

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);
}
particles[i].x += dist_x(gen);
particles[i].y += dist_y(gen);
particles[i].theta += dist_theta(gen);
}

}

Update

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

坐标变换

(xmap,ymap)=f(xparticle,yparticle,θparticle,xobs,yobs)

xy=xcos(θ)+ysin(θ)=xsin(θ)+ycos(θ)

xy=xcos(θ)ysin(θ)=xsin(θ)+ycos(θ)

xy=xcos(θ)ysin(θ)+xp=xsin(θ)+ycos(θ)+yp

Update

P(x,y)=12πσxσyexp((xμx)22σ2x+(yμy)22σ2y)

w=P(x,y)

代码

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
// 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 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;

}

实例

• 本文已收录于以下专栏：

基于粒子滤波的跟踪算法 Particle Filter Object Tracking

• yueguanyun
• 2013年01月29日 15:41
• 2933

基于粒子滤波器的目标跟踪算法及实现

• jinshengtao
• 2014年06月15日 10:15
• 35322

粒子滤波（Particle Filter）的通俗解释

• yq_forever
• 2017年03月15日 11:06
• 2643

粒子滤波流程

• qq_27923041
• 2017年02月22日 10:56
• 679

目标跟踪之粒子滤波

• u014568921
• 2015年06月20日 16:02
• 6733

用俗话讲讲卡尔曼滤波与粒子滤波

• sac761
• 2016年07月24日 21:34
• 7362

从meanshift目标跟踪到粒子滤波

• s110500617
• 2016年07月10日 19:08
• 993

学习Opencv2——粒子滤波Condensation算法

• GDFSG
• 2016年03月04日 01:51
• 3205

基于粒子滤波的人脸跟踪实现------立项篇

• yanqingan
• 2010年06月11日 21:35
• 2233

基于粒子滤波的目标跟踪基本算法（Matlab）

function [hist]=FHist(temp,D) % 此函数为提取区域目标色彩直方图 % temp-目标区域 % D-颜色空间位数 % m_wei(i,j)-像素点权值贡献 % hist...
• u014234991
• 2014年09月07日 20:21
• 2426

举报原因： 您举报文章：粒子滤波简介 色情 政治 抄袭 广告 招聘 骂人 其他 (最多只允许输入30个字)