概述
粒子濾波，也就是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
}
總結
粒子濾波的思想很簡單，是基于窮舉法的一種高效方法。若使用窮舉法，那麼就是對位姿空間中的每個可能去周遊，當然從計算量上來說這是不可能的。是以粒子濾波就是足夠多的随機粒子（位姿假設）放到整個位姿空間中，然後用本文的方法去選出定位效果好的粒子，不停疊代，最終所有的粒子收斂到正确的位姿附近。
優點：能夠得到機器人位姿的較優解（蒙特拉羅的），大大降低了窮舉法的計算量，能夠一定程度上解決機器人綁架問題
缺點：必須要機器人移動了一定距離才能執行一次濾波，機器人不運動時無法定位；粒子需要多次疊代才能收斂；計算量随着粒子數量的增加而直線上升。