轉自 http://zhengjunliu360.blog.163.com/blog/static/53797102201032734617104/
之前一直在做移動機器人定位算法。查來查去，發覺粒子濾波算法（又叫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),來修正各個狀態的估計概率。
總結一下，粒子濾波就是一種基於動態系統前向模型利用獎懲機制估計狀態值的一種方法。