之前一直在做移動機(jī)器人定位算法。查來查去，發(fā)覺粒子濾波算法（又叫MC算法）應(yīng)該算是最流行的了。因此開始學(xué)習(xí)使用之。入手的是本英文書叫 “probalistic robotic” 很不錯，我所見到的講得最好的一本書。花了大量時間去研讀。在這里我想談?wù)勎覍αＷ訛V波的一點(diǎn)認(rèn)識。因?yàn)樵谶@一領(lǐng)域算是個新手。希望有前輩或者達(dá)人來指正 我的想法。也希望我的這篇文章對新手有理解他有所幫助（當(dāng)初我就很是苦于它難于理解）在這里我不想談粒子濾波的理論基礎(chǔ)和推到，這點(diǎn)大家可以去自己翻書。 我只談下我的體會。

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

再來看對任意如下的狀態(tài)方程

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

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

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

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

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

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

下一輪濾波，再將重采樣過后的粒子集輸入到狀態(tài)轉(zhuǎn)移方程中，直接就能夠獲得預(yù)測粒子了。。

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

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

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

另外lishuai在文中也提到Particle filter的以下特點(diǎn)：

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

大家可能有幾個疑問，

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

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

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

4.到底粒子濾波實(shí)質(zhì)是什么？

好，現(xiàn)在就一一談一下我的看法

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

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

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

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

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

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

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

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

4.到底粒子濾波實(shí)質(zhì)是什么？

事實(shí)上，我們完全可以換一種思維來認(rèn)識粒子濾波。就是基于獎勵懲罰機(jī)制（強(qiáng)化學(xué)習(xí)）的優(yōu)化的思想。

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

總結(jié)一下，粒子濾波就是一種基于動態(tài)系統(tǒng)前向模型利用獎懲機(jī)制估計狀態(tài)值的一種方法。

轉(zhuǎn)自:http://blog.csdn.net/volkswageos/article/details/6508047