1.代码传送门
首先，一如既往的，打开传送门！
https://github.com/Fred159/CarND-Kidnapped-Vehicle-Project
代码， 很重要，但更重要的是从代码的行与行之间探索他们的深层意义。
同时要学会如何写代码~~ o(∩_∩)o
（当然，我的代码也借鉴了很多别人的，c++还没有学明白）
2. 粒子滤波器是什么东西？
来自百度百科
与卡尔曼滤波（Kalman Filter）相比较 [1]
粒子滤波(PF: Particle Filter)的思想基于蒙特卡洛方法(Monte Carlo methods)，它是利用粒子集来表示概率，可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率中抽取的随机状态粒子来表达其分布，是一种顺序重要性采样法(Sequential Importance Sampling)。简单来说，粒子滤波法是指通过寻找一组在状态空间传播的随机样本对概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差分布的过程。这里的样本即指粒子,当样本数量N→∝时可以逼近任何形式的概率密度分布。
百度百科里面，划下划线的词语依次是: 蒙特卡洛方法，状态空间模型，后验概率，重要性采样，概率密度函数，样本均值。
这里不打算，详细的介绍每个东西。
毕竟每个东西都可以写成一本书。
百度上有很多好资料，就不再详细解释一遍了~
本文里每当涉及到的时候，会简单的描述他们概念和应用。
（先说一下贝叶斯方法： A上次欠我100，上上欠我200，上上上次欠我300且都没有还钱。今天A又跟我借钱，我根据A以前的行为判断，A这次也不会不还钱。所以我拒绝借给A钱。
一毛都不借！~
蒙特卡洛方法的概念就是：B跟今天跟我借钱。以前我听说B人品有问题，总是借钱不还。所以，我今天也没有借钱给他。
粒子滤波器的基本理念就是基于蒙特卡洛方法的。蒙特卡洛又是基于贝叶斯理论的）
3. 这个project的目的-绑架车？
其实在Udacity的project名字是“Kidnapped Vehicle”。在我理解“Kidnapped”这个单词意思就是“被绑架”。所以这个project 的意思就是“被绑架的车辆”。
那么为什么要叫这个名字呢？ 因为车子被绑架之后，它需要知道“我是谁，我在哪？”这种哲学性的问题。
这时就需要我们的“粒子滤波器”来告诉车如何判断自己的位置。
这个过程就叫做，定位。
不过这里想说明一下，根据不同传感器提供的信息及处理的方法，我认为定位分为两种。一种是像SLAM这种，由高精地图和车载感知传感器得到自身位置的。还有一种是只根据自己车辆内部的传感器，也就是车辆速度，车辆转向角，车辆的惯性导航信息（短期内不需要GPS信号的）得到自己的短期定位。
当然，如果说，短时间定位也是为了填补GPS的低频率数据的短板的话，也是没有问题的。
只不过，我认为是两种完全不一样的东西。
一个大的，一个是小的。
不过，说实话，我暂时还是不确定到底是否该分为两个。
希望能从评论里找到答案。
根据我的定义，那么本篇所涉及到的问题是第一类问题。也就是根据高精地图和车载感知传感器的到自身位置的。 虽然也会用到车辆内部传感器信息，但是因为每个sample time 都会收到传感器的信息，所以并没有信息丢失的问题，所以不能归类于第二类。
本人拙见，如有问题，务必请赐教
4. 如何用粒子滤波器定位？（虚拟码）
在开始讲详细的步骤及代码之前，想简单阐述一下整个过程。
上udaicty 原图。
Pseudo code of particle filter
这是什么呢？是粒子滤波器的整个虚拟码。
现在简单了解一下。
首先，值得注意的是，最上面标题中有x_t-1, u_t, zt。 这说明，粒子滤波器的input就是这三个东西（当然还要有计算其他东西所需要的参数）。他们分别是，上一个时间点的状态向量，这个时间点的控制向量，这个时间点的测量向量。x_t[m] 是第m个粒子的意思。 x_t bar 也就是x上面有横杠的是所有粒子的平均值。
之后的算法顺序就是
初始化粒子。设置N个粒子，每个粒子权重相同。
for 的循环里面， 对每一个粒子根据状态转移矩阵进行预测。预测之后，根据式子4得到每一个粒子对应的权重（根据测量值和预测值），更新每一个粒子。
重采样。N个粒子中，根据每个粒子的权重，判断是否在下一个循环中是否要使用这个粒子。粒子对应权重越高（也就是根据状态转移矩阵得到的预测值和车子身上的传感器得到的值约相似），他被选取的概率就越大。也就是说，一个有99%权重的粒子，也有1%不被选择使用的可能性。
更新每个粒子的权重并根据重采样可放回的随机抽取，最终通过抽取出来的粒子求得实际车辆位置。
5. 粒子滤波器（c++）
5.1 仿真器
开始讲粒子滤波器代码之前，先介绍一下本次项目使用的Udacity的simulator 结构。
首先，Udacity的仿真器是用Unity做的。
仿真器提供Landmark（以下称为地标）的X, Y 坐标。 这些个地标的位置就是完全正确的位置，也就是Ground truth data。
仿真器里的车自身带有传感器，可以感知某个障碍物，并得到障碍物相对于我的位置信息。
那么从宏观上来看，本次项目的目的是让车辆知道自己在哪里。那么对于车辆来说，输入和输出就明确了。
输入： Map 数据。车辆传感器数据（包括速度，转向角，障碍物方位感知）
输出： 车辆在Map上的坐标
所以对于我们要做的PF代码的输入和输出也就明确了。
整个代码的流程请看一下链接。也就是这个项目的main.cpp。 通过阅读这个代码，可以看到很多以前没有想到的事情及有些东西到底如何实现的问题。
https://github.com/Fred159/CarND-Kidnapped-Vehicle-Project/blob/master/src/main.cppgithub.com
这里也可以看一下，udacity给出的代码树结构。需要修改的就是main.cpp, particle_filter.h 和particle_filter.cpp.
Udacity 给出的Simulator代码结构
5.2 PF C++的实现
首先整个PF的构建都围绕着一下内容展开。从下图的箭头顺序可以看出，依次是初始化，预测，更新粒子状态及粒子的权重，重采样。
简单解释一下，初始化，就是定义传感器的噪声也就是sigma等参数和获取第一批GPS信号（Groud Truth x，y坐标）。预测过程就是根据车辆的yaw rate ，velocity 数据 + re-sample之后的各个粒子的值和其权重等，预测下一个sample time 之后的自身位置。在update step，根据自身预测的位置，传感器数据更新自身位置，最后通过重采样对各个粒子的权重进行重新采样。
整个PF流程
代码传送门 （Particle_filter.cpp文件）
https://github.com/Fred159/CarND-Kidnapped-Vehicle-Project/blob/master/src/particle_filter.cppgithub.com
5.2.1 初始化
初始化，顾名思义也就是还没有开始递归的时候，根据第一个测量的值初始化之后要用到的各类数据。
注意，所有的数据都是有噪声的。
这里定义了以后要用到的粒子的个数，有噪声的GPS 信号（x，y），车辆航行角度及各个粒子。
值得关注的是，因为初始化的时候，车辆还没有获得什么额外的观测值，所以PF也不能分配各个粒子的权重。所以刚开始的时候，所有粒子的权重就是1.0 ，代表所有的粒子都同样重要。
最后通过push_back把所有的单个粒子通过particles这个Class整合在一起。以便以后好直接统一调取。
（每个粒子也都是一个class。）
最后，通过is_initialized = true 结束初始化。之后的步骤里面就再也不会用到初始化了。
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).
if (is_initialized) {
return;
}
// Initializing the number of particles
num_particles = 100;
// normal distribution of distribution x with std_x
normal_distribution
// normal distribution of distribution y with std_y
normal_distribution
//normal distribution of distribution theta with std_theta
normal_distribution
// Generate particles with normal distribution with mean on GPS values.
for (int i = 0; i < num_particles; ++i) {
//Using struct to make a particle structure and assign every information about each particles
Particle particle;
particle.id = i;
particle.x = dist_x(gen);
particle.y = dist_y(gen);
particle.theta = angle_theta(gen);
// assign weight=1 to each particle
particle.weight = 1.0;
// add particle to ParticleFilter class => std::vector
// with this method, every particle and vecotr particles can be generated.
//add structure into a vector.
particles.push_back(particle);
}
//after initialized, is_initialized should be true. If not, paricle fitler will always become initialized and uselessful.
is_initialized = true;
}
5.2.2 预测
预测阶段是通过物理模型预测车辆在下一个sample time的位置。
那么有时候会有疑问，为什么非要预测下一个阶段呢？ 因为，在t+1的情况下，如果没有自身位置的预测，那么从传感器获取地标距离的时候，就没有可以评估这个距离置信度的方法。
预测阶段的输入是：(double delta_t, double std_pos[], double velocity, double yaw_rate) 。依次是sample time， standard deviation of position，车辆速度和偏航角。
为什么就这么点输入？ 汽车车辆模型那么复杂，3自由度，6自由度，20好几的自由度，等等等等。
嗯， 其实预测车辆位置的时候，用到的车辆模型越复杂越好（前提是各种参数设置的都是正确的），但是本次项目中只是利用了简单的自行车模型而已。因为简单，好理解~ 就三行
自行车模型
噪声的添加用了normal_distribution这种库。
因为要更新每个particle的x，y，yaw angle，所以利用for循环和粒子class对每个粒子进行计算。因为每个粒子的x，y，yaw angle 都是基于初始化的值+random noise得到的，所以所有的粒子的预测值基本也都是不一样的。
值得注意的是，根据自行车模型，可以发现分母位置有个theta_dot。 这个值作为分母，如果=0的时候就会出现极值。这是我们不想看到的。而且theta_dot=0的时候，也没必要非要用上面的公式。因为theta_dot 是零的时候，可以导出更简单的自行车模型。具体参见下面代码中的else部分。
void ParticleFilter::prediction(double delta_t, double std_pos[], double velocity, double yaw_rate) {
// normal distribution of distribution x with zero mean and each std.
normal_distribution
// normal distribution of distribution y with std_y
normal_distribution
//normal distribution of distribution theta with std_theta
normal_distribution
// it needs for loop
for (int i = 0; i < num_particles; i++) {
//double theta = particles[i].theta;
if (fabs(yaw_rate) >= EPS) {
particles[i].x = particles[i].x + (velocity / yaw_rate)*(sin(particles[i].theta + yaw_rate * delta_t) - sin(particles[i].theta));
particles[i].y = particles[i].y + (velocity / yaw_rate)*(cos(particles[i].theta) - cos(particles[i].theta + yaw_rate * delta_t));
particles[i].theta = particles[i].theta + yaw_rate * delta_t;
}
else {// theta doesn't change
particles[i].x = particles[i].x + velocity * delta_t *cos(particles[i].theta);
particles[i].y = particles[i].y + velocity * delta_t *sin(particles[i].theta);
}
//add noise to each particle in particles.
particles[i].x = particles[i].x + dist_x(gen);
particles[i].y = particles[i].y + dist_y(gen);
particles[i].theta = particles[i].theta + angle_theta(gen);
}
}
5.2.3 更新粒子状态及粒子权重
这个部分根据车辆的预测位置，车辆的传感器，地图里面的地标的坐标计算各个粒子的权重和当下时间点的最终的车辆具体位置。
这里涉及到几个知识点
a. 车辆传感器测量的值的选取
b. 车辆传感器测量的值是基于车辆自身坐标系的。然而，地标在地图上的位置是基于地图坐标系的。那么中间，就需要有坐标变换的环节
c. 如何判断粒子的权重
a. 车辆传感器测量的值的选取
如何判断车辆通过传感器获取的数据是目标a？
本次项目里面利用了Nearest Neighborhood（NN）的原则，选取众多数据中，探测到目标a的数据。
啥东西是NN？ 就是找最近的。
如下图所示，车辆的传感器感知了周围的环境。每个地标周围都有很多传感器信息。我们根据NN选取离地标最近的测量数据作为有效感知数据。其他都过滤掉。只有这样，车辆才能通过相对位置找到自己的位置。
到底是那个？
如何选取众多数据中，最接近真实测量值数据
代码实现如下。概念就是： 利用（传感器）测量出的众多x，y坐标和地标的GT坐标，挨个计算他们之间的距离。其中距离最小的，就当作是最接近真实测量数据。那么最终，每个地标就会只匹配一个传感器数据，而不是很多个。
void ParticleFilter::dataAssociation(std::vector
int n_observation = observations.size();
int n_predictions = predicted.size();
for (int i = 0; i < n_observation; i++) {
//for each observation
//initializing the min distance as really big number
double min_dis = numeric_limits
//initializing the found map that is not in map , this is made for return the nearset measurement around GT.
int id_in_map = -100;
//complexity is o(ij);
for (int j = 0; j < n_predictions; j++) {
//distance calculation with helper function
double distance = dist(observations[i].x, observations[i].y, predicted[j].x, predicted[j].y);
// if distance is smaller than the distance, then save the id , then iterate all the predicted value
//finally find the most nearest precited to GT value.
if (distance < min_dis) {
min_dis = distance;
id_in_map = predicted[j].id;
}
}
//assign the observed measurement to this particular landmark.
//for vehicle, it means, this observation is belong to this landmark.
observations[i].id = id_in_map;
}
}
那么可以看下NN的优缺点。简单，容易是最大的优点。缺点就是只根据距离判断，所以难免会把最近的噪声当作最好的，然后就是当传感器数据比较多的时候，要挨个计算各个粒子和传感器数据之间的距离，所以慢！导致实时性降低。
NN的优缺点
这边还有额外的一步就是，计算传感器可感知范围。毕竟物理世界中，传感器的感知范围是有限的。具体代码实现如下。所以，我们只考虑传感范围里面的地标和感知数据。
for (int i = 0; i < num_particles; i++) {
double x = particles[i].x;
double y = particles[i].y;
double theta = particles[i].theta;
//find landmarks in vehicle sensing range
double sensor_range_2 = sensor_range * sensor_range;
vector
for (unsigned int j = 0; j < map_landmarks.landmark_list.size(); j++) {
float landmarkX = map_landmarks.landmark_list[j].x_f;
float landmarkY = map_landmarks.landmark_list[j].y_f;
int id = map_landmarks.landmark_list[j].id_i;
double dX = x - landmarkX;
double dY = y - landmarkY;
//in this step, in range is constructed. After this step, we only calculate the landmarks in the range.
if (dX*dX + dY * dY <= sensor_range_2) {
inRangeLandmarks.push_back(LandmarkObs{ id, landmarkX, landmarkY });
}
}
b. 坐标系变换
为什么做坐标变换？ 很好理解。
比如，小明（人）站在空地上。前面2米处有个足球。对于小明来说，他知道距离自己两米的地方有一个球。但是他不知道这个球在GPS定义的地球坐标系里的位置。但是，只要小明只要知道自己在GPS定义的坐标系里的位置，他就可以根据自己的位置+2米算出球在GPS坐标系里的位置。
相反，如果小明不知道自己在GPS定义的坐标系里的位置，但是他知道距离自己2米的球在GPS定义的坐标系里的位置。那么小明就可以根据球的位置，算出自己在GPS定义的坐标系里位置。
更简单的用式子表达就是： 1 + 2 = 3
1知道自己是3-2
1也知道自己+2是3
1同样知道自己+2-3是0
2也一样，3也一样。
嗯，如果不能理解我想用式子表达的意思就跳过把。。
车辆坐标系测量的地标，到底在地图坐标系的哪里？又或者，地图坐标系的地标，应该在车辆坐标系的哪个地方？
同一个点，在不同坐标系的坐标是不一样的
说的挺复杂，其实就是一套公式。就是下面这个。不过需要注意的是，是把坐标从车辆转换到地图，还是地图转换到车辆。（公式里的thata代入的数据就是 yaw angle，因为每时每刻，汽车的yaw angle 都在变化，那么同一个点对于车辆坐标来说也是一直发生变化。）
旋转矩阵
具体代码实现如下。
// Transfrom observation coodinates from vehicle coordinate to map (global) coordinate.
vector
//Rotation
for (int j = 0; j< observations.size(); j++) {
double xx = x + cos(theta)*observations[j].x - sin(theta) * observations[j].y;
double yy = y + sin(theta)*observations[j].x + cos(theta) * observations[j].y;
//using struct defined in helperfunction.h LandmarkObs, to make a after transition and rotation transformed observation data.
//The @param observations is a noise mixed sensor measurement data.
mappedObservations.push_back(LandmarkObs{ observations[j].id, xx, yy });
}
c. 如何判断（计算）粒子的权重
判断（计算）粒子权重在这里的直观意思就是，越是跟正确的结果相近的粒子，他就越重要。为什么说更重要的？因为最终
x = 连加符号（x_i*weight_i）。（我猜你知道我写的连加符号是什么意思~ 哈哈）
所以，即使粒子值很大，但是其权重小，那还是对最终的影响很小。
我们的车辆会通过传感器获得x方向和与y方向两个参数。而且两个参数各自有自己的不确定性。所以要通过同时评估x，y的值来获得实际该粒子的权重。
为了同时评估两个值的概率分布，这里用到了多元高斯概率分布。
其实下面多元高斯概率分布的公式和单个高斯概率分布挺像的。
多元（2元）高斯概率分布
具体代码实现如下。
Note that x and y are the observations in map coordinates from the landmarks quiz and μx , μy are the coordinates of the nearest landmarks.
for (int j = 0; j < mappedObservations.size(); j++) {
double observationX = mappedObservations[j].x;
double observationY = mappedObservations[j].y;
int landmarkId = mappedObservations[j].id;
double landmarkX, landmarkY;
int k = 0;
int nLandmarks = inRangeLandmarks.size();
bool found = false;
while (!found && k < nLandmarks) {
if (inRangeLandmarks[k].id == landmarkId) {
found = true;
landmarkX = inRangeLandmarks[k].x;
landmarkY = inRangeLandmarks[k].y;
}
k++;
}
//calculating weight
double dX = observationX - landmarkX;
double dY = observationY - landmarkY;
//Since we assume the correlation between x direction and y direction is not exist, then rho in wiki is zero.
//weight update
double weight = (1 / (2 * M_PI*stdLandmarkRange*stdLandmarkBearing)) * exp(-(dX*dX / (2 * stdLandmarkRange*stdLandmarkRange) + (dY*dY / (2 * stdLandmarkBearing*stdLandmarkBearing))));
//if weight equal to zero. then multiply to the EPS.
if (weight == 0) {
particles[i].weight = particles[i].weight*EPS;
}
//if weight doesn't equal to zero, then weight should be multiply by i times. because it is multivariate define.
else {
particles[i].weight = particles[i].weight * weight;
}
}
5.2.4重采样
重采样的过程就是，我们根据各个粒子的权重（也就是概率）来随机可放回式的抽取。那么权重大的粒子被选择的可能性就大，权重小的粒子被选中的几率就小。当然，并不是说权重小就一定不会被选上，权重大就一定会被选上，都是概率的问题。
代码实现如下。其实就是定义一个变量之后，随机可放回的方式抽取粒子。
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
// Get weights and max weight.
vector
double max_weight = numeric_limits
for(int i = 0; i < num_particles; i++) {
weights.push_back(particles[i].weight);
if ( particles[i].weight > max_weight ) {
max_weight = particles[i].weight;
}
}
// Creating distributions.
uniform_real_distribution
uniform_int_distribution
// Generating index.
int index = dist_int(gen);
double beta = 0.0;
// the wheel
vector
for(int i = 0; i < num_particles; i++) {
beta += dist_float(gen) * 2.0;
while( beta > weights[index]) {
beta -= weights[index];
index = (index + 1) % num_particles;
}
resampled_particles.push_back(particles[index]);
}
particles = resampled_particles;
}
通过重采样之后，通过上面写的
x = 连加符号（x_i*weight_i）
求出最终的x，y值。此时的x，y就是通过预测值，更新值，权重值一起估算出来的车辆在地图上的最终位置。
这些都是在一个sample time 里面发生的事情。。。。
6. 总结
粒子滤波器他不需要建立详细的模型，也不需要线性化之类的。
他只是单纯的利用大量粒子的随机性去近似实际问题。
据Sebastian Thrun总结，粒子滤波和其他的滤波器的比较就是，简单，简单，简单。
Sebastian Thrun总结各类滤波器
这个文章通过udacity的项目简述了粒子滤波器的工作原理及在无人车上的应用。
涉及到了很多东西，希望对大家的学习有所帮助！
谢谢支持，各位看官的关注就是持续更新的动力~
看完就别吝啬点赞加关注啦~