之前一直在做移動機器人定位算法。查來查去，發覺粒子濾波算法（又叫MC算法）應該算是最流行的了。因此開始學習使用之。入手的是本英文書叫 “probalistic robotic” 很不錯，我所見到的講得最好的一本書。花了大量時間去研讀。在這里我想談談我對粒子濾波的一點認識。因為在這一領域算是個新手。希望有前輩或者達人來指正 我的想法。也希望我的這篇文章對新手有理解他有所幫助（當初我就很是苦于它難于理解）在這里我不想談粒子濾波的理論基礎和推到，這點大家可以去自己翻書。 我只談下我的體會。

粒子濾波算法。他源于Montecarlo的思想，即以某事件出現的頻率來指代該事件的概率。因此在濾波過程中，需要用到概率如P(x)的地方，一 概對變量x采樣，以大量采樣的分布近似來表示P(x)。因此，采用此一思想，在濾波過程中粒子濾波可以處理任意形式的概率，而不像Kalman濾波只能處 理高斯分布的概率問題。他的一大優勢也在于此。

再來看對任意如下的狀態方程

x(t)=f(x(t-1),u(t),w(t))

y(t)=h(x(t),e(t))

其中的x(t)為t時刻狀態，u(t)為控制量，w(t) 和e(t)分別為模型噪聲和，觀測噪聲。前一個當然是狀態轉移方程，后一個是觀測方程。那么對于這么一個問題粒子濾波怎么來從觀測y(t)，和x(t-1),u(t) 濾出真實狀態x(t)呢？

看看濾波的預估階段：粒子濾波首先根據x(t-1) 和他的概率分布生成大量的采樣，這些采樣就稱之為粒子。那么這些采樣在狀態空間中的分布實際上就是x(t-1) 的概率分布了。好，接下來依據狀態轉移方程加上控制量可以對每一粒子得到一個預測粒子。所有的預測粒子就代表了涉及哪些參數化的東西）。

進入校正階段來：有了預測粒子，當然不是所有的預測粒子都能得到我們的時間觀測值y對不，越是接近真實狀態的粒子，當然獲得越有可能獲得觀測值y對吧。于是我們對所有的粒子得有個評價了，這個評價就是一個條件概率P(y|xi ),直白的說，這個條件概率代表了假設真實狀態x(t)取第i個粒子xi 時獲得觀測y的概率。令這個條件概率為第i個粒子的權重。如此這般下來，對所有粒子都進行這么一個評價，那么越有可能獲得觀測y的粒子，當然獲得的權重越高。好了預測信息融合在粒子的分布中，觀測信息又融合在了每一粒子的權重中。

哈哈最后采用重采樣算法（不知道什么是重采樣算法，那就只好翻書去吧），去除低權值的粒子，復制高權值的粒子。所得到的當然就是我們說需要的真實狀態x(t)了，而這些重采樣后的粒子，就代表了真實狀態的概率分布了。

下一輪濾波，再將重采樣過后的粒子集輸入到狀態轉移方程中，直接就能夠獲得預測粒子了。。

初始狀態的問題： 咱們一開始對x(0)一無所知對吧，那咱們就認為x(0)在全狀態空間內平均分布唄。于是初始的采樣就平均分布在整個狀態空間中。然后將所有采樣輸入狀態 轉移方程，得到預測粒子。嘿嘿再評價下所有預測粒子的權重，當然我們在整個狀態空間中只有部分粒子能夠獲的高權值咯。馬上重采樣算法來了，去除低權值的， 將下一輪濾波的考慮重點立馬縮小到了高權值粒子附近。哈哈就這么簡單。也很實用。

明白了沒？沒看糊涂吧哈哈。

如果大家看得還不過癮，后面有根據精彩的論述。

另外lishuai在文中也提到Particle filter的以下特點：

如果跟kalman濾波相比，那確實。畢竟kalman濾波可以直接得到狀態的解析估計，計算量很小。如果跟Markov定位相比，恰恰與 ricky所說相反，粒子濾波計算量小很多，而事實上，粒子濾波被用于定位的背景就是為了降低普通的Markov定位計算量相當大并且隨著維數的增長計算 量迅速增長的缺陷。（Sebastian Thrun, Wolfram burgard, Dieter fox等在90年代做的一個圖書館機器人導航的項目，其中很多當時他們的工作都成了現今機器人研究領域的熱點，比如粒子濾波，SLAM等）。

大家可能有幾個疑問，

1. Kalman濾波或者EKF都可以做定位并且運算量小，為什么還要用什么Markov定位呢？

2. 為什么Markov定位計算量大并且隨著空間維數的增長而增長劇烈？

3.為什么粒子濾波這么神奇，讓計算量如此之大的Markov定位運算量驟降？

4.到底粒子濾波實質是什么？

好，現在就一一談一下我的看法

1. Kalman濾波或者EKF都可以做定位并且運算量小，為什么還要用什么Markov定位呢？

燢alman濾波是有適用條件的，a。初始狀態必須是符合正態分布。b。必須是線性系統。（當然，EKF通過將非線性系統線性化的方法處理非線性系統）。而真實的定位問題很多時候不滿足以上兩個條件。為什么不滿足呢？

先說為什么a不滿足：首先舉個正態分布無法描述的反例，大家都知道，正態分布是單峰函數，也就是說機器人初始時位于工作空間中某個位置的初始概 率最大，其他地方都比這小。如果是采用地圖匹配進行絕對定位，上面描述的單峰高斯函數可能就無法精確的描述事實了，比如有十個一模一樣的房間。初始時把機 器人放在其中一個里面，機器人根據絕對測量傳感器獲得局部地圖，與他攜帶的先驗地圖匹配后他發現，他現在呆的位置在他的工作空間中有10個峰值，每個房間 一個，因為十個房間一模一樣，他無法區分。顯然，此時a假設不成立。

再說b為什么不滿足：這取決于真實機器人的物理特性，系統的狀態更新方程是由里程計或者是dead reckon 得到的，系統的觀測方程是由絕對定位系統（或者地圖匹配）得到的。對于一般的移動機器人，無論是兩個主動輪的形式還是一個主動輪一個steering wheel的形式，由此得到的狀態更新方程都是非線性的。

2. 為什么Markov定位計算量大并且隨著空間維數的增長而增長劇烈？

Markov定位的基本原理很簡單，就是用條件概率描述狀態更新，所有的可能的狀態都枚舉個遍，對于機器人的每一次更新，所有的可能的狀態遷移 都要被更新一遍，假設我們用柵格描述工作空間，假設t時刻機器人可能的位置為p1,p2,p3，在二維情況下采用正方形柵格劃分那么p1有8個鄰居，記為 p11，p12，p13，…,p18.在三維情況下，采用立方體劃分那么鄰居就更多了，有26個。如果維數繼續增加，那么鄰居增加的更厲害。這里我們以二 維情況為例來闡述問題。同理，我們記p2的鄰居為p21，p22，。。。p28。p3的鄰居為p31，p32，。。。，p38。在t時刻可能的位置只有3 個，然而t+1時刻，所有的三個的鄰居，以及p1，p2，p3都有可能成為當前位置，但是根據dead reckon的結果，我們可以排除一些小概率的鄰居，減少計算量。但是隨著時間的推移，整個空間中的所有點都有可能成為估計的當前位置（只不過各個位置的 概率不同而已）。這樣，如果不采取措施，那狀態的更新會是一件巨大的工程。并且，空間維數越大，節點數越多，計算量增長越厲害。

3.為什么粒子濾波這么神奇，讓計算量如此之大的Markov定位運算量驟降？

粒子濾波強就強在它用統計的基于采樣的方法來描述整個空間中的概率分布。Markov的思想是你既然當前位置的分布概率是個特殊分布，我就干脆 把你的樣本空間離散化（把空間劃分為柵格），計算每一個樣本的概率（計算每一個柵格的概率）。但是這帶來了問題2.為了解決這個問題，粒子濾波采用了另一 種思想：現在我不再均勻的把樣本空間離散化了，而是根據我當前所掌握的概率分布對空間進行采樣（重要性采樣），顯然，概率小的地方少采幾個樣(反正概率 小，即使采多了，每個樣本差別也不大，完全可以由附近的其他樣本反映）；概率高的地方應該多采幾個樣。這樣，我們可以規定，每次都采樣N個，對于大概率的 地方多采，小概率的地方少采。根據概率里的大數定律，可以證明即使在維數增加的時候依然保持采N個樣，仍然可以保持性能。這就是粒子濾波高的地方，當維數 非常高的情況，Markov定位都累的算不出來了，因為需要更新的狀態對實在是太多了，而人家粒子濾波依然只采N個樣，計算量還那樣，變化不大。

4.到底粒子濾波實質是什么？

事實上，我們完全可以換一種思維來認識粒子濾波。就是基于獎勵懲罰機制（強化學習）的優化的思想。

首先，根據狀態轉移方程，對于每個粒子的位置進行更新。但是這個更新只是基于dead reckon得到的，我們要融合絕對定位與相對定位，絕對定位的信息還沒有融合進去呢。根據狀態轉移方程得到的新狀態到底行不行？能有多大的概率？這還取 決于絕對定位的結果也就是輸出方程。把狀態轉移方程的到的結果代入輸出方程，得到一個輸出，這個輸出是估計值，而根據絕對定位的觀測，這個值對應的觀測值 也是可以測量得到的，現在這兩個值之間有個差額，很明顯，這個差額越小，剛才的到的狀態越可信，這個差額越大，狀態越不可信。好，就把這個指標作為評估函 數（像GA，pso等優化算法里的evaluation function),來修正各個狀態的估計概率。

總結一下，粒子濾波就是一種基于動態系統前向模型利用獎懲機制估計狀態值的一種方法。

轉自:http://blog.csdn.net/volkswageos/article/details/6508047