之前一直在做移動(dòng)機(jī)器人定位算法。查來(lái)查去，發(fā)覺(jué)粒子濾波算法（又叫MC算法）應(yīng)該算是最流行的了。因此開(kāi)始學(xué)習(xí)使用之。入手的是本英文書(shū)叫 “probalistic robotic” 很不錯(cuò)，我所見(jiàn)到的講得最好的一本書(shū)?；舜罅繒r(shí)間去研讀。在這里我想談?wù)勎覍?duì)粒子濾波的一點(diǎn)認(rèn)識(shí)。因?yàn)樵谶@一領(lǐng)域算是個(gè)新手。希望有前輩或者達(dá)人來(lái)指正 我的想法。也希望我的這篇文章對(duì)新手有理解他有所幫助（當(dāng)初我就很是苦于它難于理解）在這里我不想談粒子濾波的理論基礎(chǔ)和推到，這點(diǎn)大家可以去自己翻書(shū)。 我只談下我的體會(huì)。
粒子濾波算法。他源于Montecarlo的思想，即以某事件出現(xiàn)的頻率來(lái)指代該事件的概率。因此在濾波過(guò)程中，需要用到概率如P(x)的地方，一 概對(duì)變量x采樣，以大量采樣的分布近似來(lái)表示P(x)。因此，采用此一思想，在濾波過(guò)程中粒子濾波可以處理任意形式的概率，而不像Kalman濾波只能處 理高斯分布的概率問(wèn)題。他的一大優(yōu)勢(shì)也在于此。
再來(lái)看對(duì)任意如下的狀態(tài)方程
x(t)=f(x(t-1),u(t),w(t))
y(t)=h(x(t),e(t))
其中的x(t)為t時(shí)刻狀態(tài)，u(t)為控制量，w(t) 和e(t)分別為模型噪聲和，觀測(cè)噪聲。前一個(gè)當(dāng)然是狀態(tài)轉(zhuǎn)移方程，后一個(gè)是觀測(cè)方程。那么對(duì)于這么一個(gè)問(wèn)題粒子濾波怎么來(lái)從觀測(cè)y(t)，和x(t-1),u(t) 濾出真實(shí)狀態(tài)x(t)呢？
看看濾波的預(yù)估階段：粒子濾波首先根據(jù)x(t-1) 和他的概率分布生成大量的采樣，這些采樣就稱(chēng)之為粒子。那么這些采樣在狀態(tài)空間中的分布實(shí)際上就是x(t-1) 的概率分布了。好，接下來(lái)依據(jù)狀態(tài)轉(zhuǎn)移方程加上控制量可以對(duì)每一粒子得到一個(gè)預(yù)測(cè)粒子。所有的預(yù)測(cè)粒子就代表了涉及哪些參數(shù)化的東西）。
進(jìn)入校正階段來(lái)：有了預(yù)測(cè)粒子，當(dāng)然不是所有的預(yù)測(cè)粒子都能得到我們的時(shí)間觀測(cè)值y對(duì)不，越是接近真實(shí)狀態(tài)的粒子，當(dāng)然獲得越有可能獲得觀測(cè)值y對(duì)吧。于是我們對(duì)所有的粒子得有個(gè)評(píng)價(jià)了，這個(gè)評(píng)價(jià)就是一個(gè)條件概率P(y|xi ),直白的說(shuō)，這個(gè)條件概率代表了假設(shè)真實(shí)狀態(tài)x(t)取第i個(gè)粒子xi 時(shí)獲得觀測(cè)y的概率。令這個(gè)條件概率為第i個(gè)粒子的權(quán)重。如此這般下來(lái)，對(duì)所有粒子都進(jìn)行這么一個(gè)評(píng)價(jià)，那么越有可能獲得觀測(cè)y的粒子，當(dāng)然獲得的權(quán)重越高。好了預(yù)測(cè)信息融合在粒子的分布中，觀測(cè)信息又融合在了每一粒子的權(quán)重中。
哈哈最后采用重采樣算法（不知道什么是重采樣算法，那就只好翻書(shū)去吧），去除低權(quán)值的粒子，復(fù)制高權(quán)值的粒子。所得到的當(dāng)然就是我們說(shuō)需要的真實(shí)狀態(tài)x(t)了，而這些重采樣后的粒子，就代表了真實(shí)狀態(tài)的概率分布了。
下一輪濾波，再將重采樣過(guò)后的粒子集輸入到狀態(tài)轉(zhuǎn)移方程中，直接就能夠獲得預(yù)測(cè)粒子了。。
初始狀態(tài)的問(wèn)題： 咱們一開(kāi)始對(duì)x(0)一無(wú)所知對(duì)吧，那咱們就認(rèn)為x(0)在全狀態(tài)空間內(nèi)平均分布唄。于是初始的采樣就平均分布在整個(gè)狀態(tài)空間中。然后將所有采樣輸入狀態(tài) 轉(zhuǎn)移方程，得到預(yù)測(cè)粒子。嘿嘿再評(píng)價(jià)下所有預(yù)測(cè)粒子的權(quán)重，當(dāng)然我們?cè)谡麄€(gè)狀態(tài)空間中只有部分粒子能夠獲的高權(quán)值咯。馬上重采樣算法來(lái)了，去除低權(quán)值的， 將下一輪濾波的考慮重點(diǎn)立馬縮小到了高權(quán)值粒子附近。哈哈就這么簡(jiǎn)單。也很實(shí)用。
明白了沒(méi)？沒(méi)看糊涂吧哈哈。
如果大家看得還不過(guò)癮，后面有根據(jù)精彩的論述。
另外lishuai在文中也提到Particle filter的以下特點(diǎn)：
如果跟kalman濾波相比，那確實(shí)。畢竟kalman濾波可以直接得到狀態(tài)的解析估計(jì)，計(jì)算量很小。如果跟Markov定位相比，恰恰與 ricky所說(shuō)相反，粒子濾波計(jì)算量小很多，而事實(shí)上，粒子濾波被用于定位的背景就是為了降低普通的Markov定位計(jì)算量相當(dāng)大并且隨著維數(shù)的增長(zhǎng)計(jì)算 量迅速增長(zhǎng)的缺陷。（Sebastian Thrun, Wolfram burgard, Dieter fox等在90年代做的一個(gè)圖書(shū)館機(jī)器人導(dǎo)航的項(xiàng)目，其中很多當(dāng)時(shí)他們的工作都成了現(xiàn)今機(jī)器人研究領(lǐng)域的熱點(diǎn)，比如粒子濾波，SLAM等）。
大家可能有幾個(gè)疑問(wèn)，
1. Kalman濾波或者EKF都可以做定位并且運(yùn)算量小，為什么還要用什么Markov定位呢？
2. 為什么Markov定位計(jì)算量大并且隨著空間維數(shù)的增長(zhǎng)而增長(zhǎng)劇烈？
3.為什么粒子濾波這么神奇，讓計(jì)算量如此之大的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通過(guò)將非線性系統(tǒng)線性化的方法處理非線性系統(tǒng)）。而真實(shí)的定位問(wèn)題很多時(shí)候不滿(mǎn)足以上兩個(gè)條件。為什么不滿(mǎn)足呢？
先說(shuō)為什么a不滿(mǎn)足：首先舉個(gè)正態(tài)分布無(wú)法描述的反例，大家都知道，正態(tài)分布是單峰函數(shù)，也就是說(shuō)機(jī)器人初始時(shí)位于工作空間中某個(gè)位置的初始概 率最大，其他地方都比這小。如果是采用地圖匹配進(jìn)行絕對(duì)定位，上面描述的單峰高斯函數(shù)可能就無(wú)法精確的描述事實(shí)了，比如有十個(gè)一模一樣的房間。初始時(shí)把機(jī) 器人放在其中一個(gè)里面，機(jī)器人根據(jù)絕對(duì)測(cè)量傳感器獲得局部地圖，與他攜帶的先驗(yàn)地圖匹配后他發(fā)現(xiàn)，他現(xiàn)在呆的位置在他的工作空間中有10個(gè)峰值，每個(gè)房間 一個(gè)，因?yàn)槭畟€(gè)房間一模一樣，他無(wú)法區(qū)分。顯然，此時(shí)a假設(shè)不成立。
再說(shuō)b為什么不滿(mǎn)足：這取決于真實(shí)機(jī)器人的物理特性，系統(tǒng)的狀態(tài)更新方程是由里程計(jì)或者是dead reckon 得到的，系統(tǒng)的觀測(cè)方程是由絕對(duì)定位系統(tǒng)（或者地圖匹配）得到的。對(duì)于一般的移動(dòng)機(jī)器人，無(wú)論是兩個(gè)主動(dòng)輪的形式還是一個(gè)主動(dòng)輪一個(gè)steering wheel的形式，由此得到的狀態(tài)更新方程都是非線性的。
2. 為什么Markov定位計(jì)算量大并且隨著空間維數(shù)的增長(zhǎng)而增長(zhǎng)劇烈？
Markov定位的基本原理很簡(jiǎn)單，就是用條件概率描述狀態(tài)更新，所有的可能的狀態(tài)都枚舉個(gè)遍，對(duì)于機(jī)器人的每一次更新，所有的可能的狀態(tài)遷移 都要被更新一遍，假設(shè)我們用柵格描述工作空間，假設(shè)t時(shí)刻機(jī)器人可能的位置為p1,p2,p3，在二維情況下采用正方形柵格劃分那么p1有8個(gè)鄰居，記為 p11，p12，p13，…,p18.在三維情況下，采用立方體劃分那么鄰居就更多了，有26個(gè)。如果維數(shù)繼續(xù)增加，那么鄰居增加的更厲害。這里我們以二 維情況為例來(lái)闡述問(wèn)題。同理，我們記p2的鄰居為p21，p22，。。。p28。p3的鄰居為p31，p32，。。。，p38。在t時(shí)刻可能的位置只有3 個(gè)，然而t+1時(shí)刻，所有的三個(gè)的鄰居，以及p1，p2，p3都有可能成為當(dāng)前位置，但是根據(jù)dead reckon的結(jié)果，我們可以排除一些小概率的鄰居，減少計(jì)算量。但是隨著時(shí)間的推移，整個(gè)空間中的所有點(diǎn)都有可能成為估計(jì)的當(dāng)前位置（只不過(guò)各個(gè)位置的 概率不同而已）。這樣，如果不采取措施，那狀態(tài)的更新會(huì)是一件巨大的工程。并且，空間維數(shù)越大，節(jié)點(diǎn)數(shù)越多，計(jì)算量增長(zhǎng)越厲害。
3.為什么粒子濾波這么神奇，讓計(jì)算量如此之大的Markov定位運(yùn)算量驟降？
粒子濾波強(qiáng)就強(qiáng)在它用統(tǒng)計(jì)的基于采樣的方法來(lái)描述整個(gè)空間中的概率分布。Markov的思想是你既然當(dāng)前位置的分布概率是個(gè)特殊分布，我就干脆 把你的樣本空間離散化（把空間劃分為柵格），計(jì)算每一個(gè)樣本的概率（計(jì)算每一個(gè)柵格的概率）。但是這帶來(lái)了問(wèn)題2.為了解決這個(gè)問(wèn)題，粒子濾波采用了另一 種思想：現(xiàn)在我不再均勻的把樣本空間離散化了，而是根據(jù)我當(dāng)前所掌握的概率分布對(duì)空間進(jìn)行采樣（重要性采樣），顯然，概率小的地方少采幾個(gè)樣(反正概率 小，即使采多了，每個(gè)樣本差別也不大，完全可以由附近的其他樣本反映）；概率高的地方應(yīng)該多采幾個(gè)樣。這樣，我們可以規(guī)定，每次都采樣N個(gè)，對(duì)于大概率的 地方多采，小概率的地方少采。根據(jù)概率里的大數(shù)定律，可以證明即使在維數(shù)增加的時(shí)候依然保持采N個(gè)樣，仍然可以保持性能。這就是粒子濾波高的地方，當(dāng)維數(shù) 非常高的情況，Markov定位都累的算不出來(lái)了，因?yàn)樾枰碌臓顟B(tài)對(duì)實(shí)在是太多了，而人家粒子濾波依然只采N個(gè)樣，計(jì)算量還那樣，變化不大。
4.到底粒子濾波實(shí)質(zhì)是什么？
事實(shí)上，我們完全可以換一種思維來(lái)認(rèn)識(shí)粒子濾波。就是基于獎(jiǎng)勵(lì)懲罰機(jī)制（強(qiáng)化學(xué)習(xí)）的優(yōu)化的思想。
首先，根據(jù)狀態(tài)轉(zhuǎn)移方程，對(duì)于每個(gè)粒子的位置進(jìn)行更新。但是這個(gè)更新只是基于dead reckon得到的，我們要融合絕對(duì)定位與相對(duì)定位，絕對(duì)定位的信息還沒(méi)有融合進(jìn)去呢。根據(jù)狀態(tài)轉(zhuǎn)移方程得到的新?tīng)顟B(tài)到底行不行？能有多大的概率？這還取 決于絕對(duì)定位的結(jié)果也就是輸出方程。把狀態(tài)轉(zhuǎn)移方程的到的結(jié)果代入輸出方程，得到一個(gè)輸出，這個(gè)輸出是估計(jì)值，而根據(jù)絕對(duì)定位的觀測(cè)，這個(gè)值對(duì)應(yīng)的觀測(cè)值 也是可以測(cè)量得到的，現(xiàn)在這兩個(gè)值之間有個(gè)差額，很明顯，這個(gè)差額越小，剛才的到的狀態(tài)越可信，這個(gè)差額越大，狀態(tài)越不可信。好，就把這個(gè)指標(biāo)作為評(píng)估函 數(shù)（像GA，pso等優(yōu)化算法里的evaluation function),來(lái)修正各個(gè)狀態(tài)的估計(jì)概率。
總結(jié)一下，粒子濾波就是一種基于動(dòng)態(tài)系統(tǒng)前向模型利用獎(jiǎng)懲機(jī)制估計(jì)狀態(tài)值的一種方法。
轉(zhuǎn)自:http://blog.csdn.net/volkswageos/article/details/6508047