rbpf粒子滤波slam matlab程序_学习笔记（优达学城）- 车辆定位之粒子滤波器（整合版）...
1.代碼傳送門
首先，一如既往的，打開傳送門！
Fred159/CarND-Kidnapped-Vehicle-Project?github.com代碼， 很重要，但更重要的是從代碼的行與行之間探索他們的深層意義。
同時要學會如何寫代碼~~ 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上面有橫杠的是所有粒子的平均值。
之后的算法順序就是
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.cpp?github.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.cpp?github.com5.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 particlesnum_particles = 100;// normal distribution of distribution x with std_x normal_distribution<double> dist_x(x, std[0]);// normal distribution of distribution y with std_y normal_distribution<double> dist_y(y, std[1]);//normal distribution of distribution theta with std_theta normal_distribution<double> angle_theta(theta, std[2]);// 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 particlesParticle 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<Particle> particles;// 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<double> dist_x(0, std_pos[0]);// normal distribution of distribution y with std_ynormal_distribution<double> dist_y(0, std_pos[1]);//normal distribution of distribution theta with std_thetanormal_distribution<double> angle_theta(0, std_pos[2]);// it needs for loopfor (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 changeparticles[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<LandmarkObs> predicted, std::vector<LandmarkObs>& observations) {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 numberdouble min_dis = numeric_limits<double>::max();//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 functiondouble 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 rangedouble sensor_range_2 = sensor_range * sensor_range;vector<LandmarkObs> inRangeLandmarks;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<LandmarkObs> mappedObservations;//Rotationfor (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 weightdouble 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 updatedouble 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> weights;double max_weight = numeric_limits<double>::min();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<float> dist_float(0.0, max_weight);uniform_int_distribution<int> dist_int(0, num_particles - 1);// Generating index.int index = dist_int(gen);double beta = 0.0;// the wheelvector<Particle> resampled_particles;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就是通過預測值，更新值，權重值一起估算出來的車輛在地圖上的最終位置。
190909修改
上面三行寫的這段話，在我重新觀察驗證之后，發現并不是連加。其實是找了權重最高的粒子。至于為什么不是像特龍說的那樣，說要把所有的x和權重相乘后連加起來，還不清楚。以后有頭緒了再來更新
通過重采樣之后，通過上面寫的x = 連加符號（x_i*weight_i）
求出最終的x，y值。此時的x，y就是通過預測值，更新值，權重值一起估算出來的車輛在地圖上的最終位置。
這些都是在一個sample time 里面發生的事情。。。。
6. 總結
粒子濾波器他不需要建立詳細的模型，也不需要線性化之類的。
他只是單純的利用大量粒子的隨機性去近似實際問題。
據Sebastian Thrun總結，粒子濾波和其他的濾波器的比較就是，簡單，簡單，簡單。
Sebastian Thrun總結各類濾波器這個文章通過udacity的項目簡述了粒子濾波器的工作原理及在無人車上的應用。
涉及到了很多東西，希望對大家的學習有所幫助！
謝謝支持，各位看官的關注就是持續更新的動力~
看完就別吝嗇點贊加關注啦~
同時也希望朋友往咱們專欄投稿，讓我們在無人車算法的造詣上不停的成長~！
20180530 研究室 林明
《新程序員》：云原生和全面數字化實踐50位技術專家共同創作，文字、視頻、音頻交互閱讀總結
以上是生活随笔為你收集整理的rbpf粒子滤波slam matlab程序_学习笔记（优达学城）- 车辆定位之粒子滤波器（整合版）...的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： sql删除语句_Part 3 | SQL
- 下一篇： cumsum在matlab中,matla