概述
粒子濾波，也就是Particle Filter，簡寫PF。
所有的定位程序被放在激光傳感器數據接收的事件中，每次接收到激光雷達的數據，都會判斷一下機器人是否移動了給定的距離或旋轉了給定的角度（下面簡稱機器人滿足移動調節）。如果機器人滿足移動調節，則處理激光數據，否則不處理，直接丟棄。感覺這裏如此浪費激光雷達數據，不是一個好的策略。
注意：下文中所說的粒子的概率和粒子的權重，是一回事。
基本流程
PF僞代碼（這裏沒有考慮機器人綁架，所以是MCL，不是AMCL）：
實際的處理流程跟這裏上面的僞代碼不太一樣。稍微解釋下上面的僞代碼。
函數輸入機器人上一個狀態，運動，測量和粒子數。利用運動採樣模型生成M個服從給定分佈的隨機粒子X，利用測量模型爲每個粒子評估產生測量zt的概率Pzt，並把X和Pzt組合到一起存放。
第8行開始篩選粒子，篩選的規則是，在上面產生的M個粒子中選出M個粒子，每次從中按概率Pzt去選擇一個粒子。將產生的M的新粒子作爲結果輸出。
實際的流程不太一樣，但是思想基本一致。流程如下：
1、獲取機器人當前位姿，計算移動增量delta，判斷機器人移動調節是否滿足
2、根據上一個位姿Xt-1和移動增量delta，用運動採樣模型產生（在原有粒子基礎上按給定分佈生成）若干數量（sample-count個）的粒子，也就是生成若干個當前狀態的可能位姿。
3、將激光雷達數據從激光座標系換算到baselink座標系，使用激光傳感器模型爲每一個粒子計算在其狀態下獲得測量Zt的概率，並計算總概率totalweight。
4、上面已經獲得了僞代碼中的粒子和概率組合，就進行採樣（可設定若干次數據更新採樣一次）。採樣後粒子數量不變，並重置所有粒子的概率爲平均值。（1/sample-count）
5、對粒子進行聚類，計算每個cluster的位姿均值、概率（權重）均值、協方差等參數 （實際中此步驟和上一步是同時進行的）
6、獲取平均概率最大的那一個聚類，認爲是粒子濾波所得的最終機器人位姿。根據此位姿（機器人在MAP座標系下的位置），修正ODOM座標系下機器人的位姿（編碼器累計值計算得到的機器人位姿）。完成一個PF濾波週期。
運動學模型和傳感器模型屬於另外兩個話題，下面重點分析粒子濾波的重採樣，聚類的方法這裏也不討論，後面另開文章敘述。
其實，代碼中採用的重採樣方法很簡單，原理依然是，按照每個粒子的權重對粒子進行採樣。在重採樣過程中，我們還需要提起機器人綁架問題。由於粒子聚集後，萬一機器人定位是錯誤的，或者機器人被搬動，粒子無法聚集到正確的範圍內。所以還需要在定位效果不好的情況下全局隨機的粒子，來模擬初始化時的情形。
定位效果的評價
方法描述：粒子X產生測量Zt的概率的含義是：在X的位姿下，能夠產生實際測量Zt的概率，其實也就是反演模型產生的虛擬測量與實際測量的吻合程度。顯然，定位越不正確，概率越小，定位越準確，概率越大。因此，可以監控全局平均概率，如果概率變小，說明定位越來越不準確了。
Alfa（slow）要遠遠小於Alfa（fast），Wavg是本次濾波得到的全局平均概率。顯然Wfast會很顯著地反應濾波效果，而Wslow則變化比較慢，更能反應濾波的長期效果。
如上圖公式（稱爲濾波評價值），當濾波效果越差，Wfast就變得越來越小於Wslow，那麼返回值就會越大。反之，濾波效果較好時候，或者比較穩定的時候，Wfast和Wslow比較接近，返回值就接近0。
考慮上述機器人綁架問題的蒙特卡羅濾波被稱爲增強的蒙特卡羅濾波，也就是AMCL。 
第12行開始，篩選粒子的過程也發生了變化，原來僅僅從生成的粒子裏面挑選粒子，現在還根據濾波評價值的大小隨機地添加一些全局隨機粒子。
重採樣小技巧
在前面提供的僞代碼中，都有一句draw i with probability的僞代碼，也就是根據粒子的權重，去挑選粒子。實際如何做呢？
我們知道，所有粒子的權重歸一化後求和爲1。如果把所有粒子排列成一排，權重越大，佔用位置越大，而他們佔用的總位置求和是1。那麼我們獲取一個（0,1）的隨機數，顯然，這個隨機數落在權重大的粒子上的可能性也更大啦。算法：
c[0] = 0.0;
for(i=0;i<sample_count;i++)
c[i+1] = c[i]+weight[i]; //用c來儲存權重的累加值
foreach(p in all particles)
{
double r;
r = drand48();
for(i=0;i<sample_count;i++)
{
if((c[i] <= r) && (r < c[i+1])) //權重越大的粒子，區間就越大，隨機數r落在其區間的可能性也越大
break;
}
//TODO: Select particle i
}
總結
粒子濾波的思想很簡單，是基於窮舉法的一種高效方法。若使用窮舉法，那麼就是對位姿空間中的每個可能去遍歷，當然從計算量上來說這是不可能的。所以粒子濾波就是足夠多的隨機粒子（位姿假設）放到整個位姿空間中，然後用本文的方法去選出定位效果好的粒子，不停迭代，最終所有的粒子收斂到正確的位姿附近。
優點：能夠得到機器人位姿的較優解（蒙特拉羅的），大大降低了窮舉法的計算量，能夠一定程度上解決機器人綁架問題
缺點：必須要機器人移動了一定距離才能執行一次濾波，機器人不運動時無法定位；粒子需要多次迭代才能收斂；計算量隨着粒子數量的增加而直線上升。