粒子濾波作為視覺SLAM中后端進(jìn)行狀態(tài)估計的主要算法之一，很好的完成了擴(kuò)展卡爾曼濾波無法有效處理的復(fù)雜狀態(tài)方程下的狀態(tài)估計任務(wù)。這篇文章詳細(xì)地描述了粒子濾波的思想歷程，即如何一步步從簡單的狀態(tài)估計、采樣、應(yīng)對多樣性缺失，最后到得到相對滿意的粒子濾波的算法的思路，最后簡單講解了粒子濾波的兩大應(yīng)用：狀態(tài)估計和目標(biāo)跟蹤。該文很好地符合了為解決問題而一步步演進(jìn)算法的思路，對為什么要使用粒子濾波技術(shù)給出了很好的解釋，現(xiàn)轉(zhuǎn)載如下：
原文地址
https://blog.csdn.net/piaoxuezhong/article/details/78619150
在論文中看到粒子濾波的知識點(diǎn)，在網(wǎng)上找到的幾篇講的很易的文章：
http://blog.csdn.net/heyijia0327/article/details/40899819
http://blog.csdn.net/heyijia0327/article/details/40929097
http://blog.csdn.net/heyijia0327/article/details/41122125
http://blog.csdn.net/heyijia0327/article/details/41142679
前言：
博主在自主學(xué)習(xí)粒子濾波的過程中，看了很多文獻(xiàn)或博客，不知道是看文獻(xiàn)時粗心大意還是悟性太低，看著那么多公式，總是無法把握住粒子濾波的思路，也無法將理論和實(shí)踐對應(yīng)起來。比如：理論推導(dǎo)過程中那么多概率公式，概率怎么和系統(tǒng)的狀態(tài)變量對應(yīng)上了？狀態(tài)粒子[圖片上傳失敗...(image-2b11e5-1526518178195)]
是怎么一步步采樣出來的，為什么程序里面都是直接用狀態(tài)方程來計算？粒子的權(quán)重是怎么來的？經(jīng)過一段時間的理解，總算理清了它的脈絡(luò)。同時也覺得，只有對理論的推導(dǎo)心中有數(shù)了，才能知道什么樣的地方可以用這個算法，以及這個算法有什么不足。因此，本文將結(jié)合實(shí)際程序給出粒子濾波的詳細(xì)推導(dǎo)，在推導(dǎo)過程中加入博主自己的理解，如有不妥，請指出，謝謝。
文章架構(gòu)：
由最基礎(chǔ)的貝葉斯估計開始介紹，再引出蒙特卡羅采樣，重要性采樣，SIS粒子濾波，重采樣，基本粒子濾波Generic Particle Filter，SIR粒子濾波，這些概念的引進(jìn)，都是為了解決上一個概念中出現(xiàn)的問題而環(huán)環(huán)相扣的。最后給出幾個在matlab和python中的應(yīng)用，例程包括圖像跟蹤，濾波，機(jī)器人定位。
再往下看之前，也可以看看《[卡爾曼濾波:從推導(dǎo)到應(yīng)用](http://blog.csdn.net/heyijia0327/article/details/17487467)》，好對這種通過狀態(tài)方程來濾波的思路有所了解。
一、貝葉斯濾波
假設(shè)有一個系統(tǒng)，我們知道它的狀態(tài)方程，和測量方程如下：
[圖片上傳失敗...(image-eb498-1526518178195)]
如：[圖片上傳失敗...(image-7b12bc-1526518178195)]
(1)
[圖片上傳失敗...(image-43d0c8-1526518178195)]
如：[圖片上傳失敗...(image-4d7a20-1526518178195)]
(2)
其中x為系統(tǒng)狀態(tài)，y為測量到的數(shù)據(jù)，f,h是狀態(tài)轉(zhuǎn)移函數(shù)和測量函數(shù)，v,n為過程噪聲和測量噪聲，噪聲都是獨(dú)立同分布的。上面對應(yīng)的那個例子將會出現(xiàn)在程序中。
從貝葉斯理論的觀點(diǎn)來看，狀態(tài)估計問題（目標(biāo)跟蹤、信號濾波）就是根據(jù)之前一系列的已有數(shù)據(jù)[圖片上傳失敗...(image-245ca6-1526518178195)]
（后驗知識）遞推的計算出當(dāng)前狀態(tài)[圖片上傳失敗...(image-114896-1526518178195)]
的可信度。這個可信度就是概率公式[圖片上傳失敗...(image-d4f7a-1526518178195)]
，它需要通過預(yù)測和更新兩個步奏來遞推的計算。
預(yù)測過程是利用系統(tǒng)模型(狀態(tài)方程1)預(yù)測狀態(tài)的先驗概率密度，也就是通過已有的先驗知識對未來的狀態(tài)進(jìn)行猜測，即p( x(k)|x(k-1) )。更新過程則利用最新的測量值對先驗概率密度進(jìn)行修正，得到后驗概率密度，也就是對之前的猜測進(jìn)行修正。
在處理這些問題時，一般都先假設(shè)系統(tǒng)的狀態(tài)轉(zhuǎn)移服從一階馬爾科夫模型，即當(dāng)前時刻的狀態(tài)x(k)只與上一個時刻的狀態(tài)x(k-1)有關(guān)。這是很自然的一種假設(shè)，就像小時候玩飛行棋，下一時刻的飛機(jī)跳到的位置只由當(dāng)前時刻的位置和骰子決定。同時，假設(shè)k時刻測量到的數(shù)據(jù)y(k)只與當(dāng)前的狀態(tài)x(k)有關(guān)，如上面的狀態(tài)方程2。
為了進(jìn)行遞推，不妨假設(shè)已知k-1時刻的概率密度函數(shù)[圖片上傳失敗...(image-dd2841-1526518178195)]
預(yù)測：由上一時刻的概率密度[圖片上傳失敗...(image-3f03c1-1526518178195)]
得到[圖片上傳失敗...(image-cf0395-1526518178195)]
，這個公式的含義是既然有了前面1：k-1時刻的測量數(shù)據(jù)，那就可以預(yù)測一下狀態(tài)x(k)出現(xiàn)的概率。
計算推導(dǎo)如下：
[圖片上傳失敗...(image-8f9ff4-1526518178195)]
[圖片上傳失敗...(image-af43c3-1526518178195)]
[圖片上傳失敗...(image-5f34a4-1526518178195)]
等式的第一行到第二行純粹是貝葉斯公式的應(yīng)用。第二行得到第三行是由于一階馬爾科夫過程的假設(shè)，狀態(tài)x(k)只由x(k-1)決定。
樓主看到這里的時候想到兩個問題：
第一：既然 [圖片上傳失敗...(image-38ec4c-1526518178195)]
，x(k)都只由x(k-1)決定了，即[圖片上傳失敗...(image-305260-1526518178195)]
，在這里弄一個[圖片上傳失敗...(image-9fb92b-1526518178195)]
是什么意思？
這兩個概率公式含義是不一樣的，前一個是純粹根據(jù)模型進(jìn)行預(yù)測，x(k)實(shí)實(shí)在在的由x(k-1)決定，后一個是既然測到的數(shù)據(jù)和狀態(tài)是有關(guān)系的，現(xiàn)在已經(jīng)有了很多測量數(shù)據(jù) y 了，那么我可以根據(jù)已有的經(jīng)驗對你進(jìn)行預(yù)測，只是猜測x(k)，而不能決定x(k)。
第二：上面公式的最后一行[圖片上傳失敗...(image-5ea95d-1526518178195)]
是假設(shè)已知的，但是[圖片上傳失敗...(image-7a8a9e-1526518178195)]
怎么得到呢？
其實(shí)[圖片上傳失敗...(image-452c95-1526518178195)]
它由系統(tǒng)狀態(tài)方程決定，它的概率分布形狀和系統(tǒng)的過程噪聲[圖片上傳失敗...(image-dbeb58-1526518178194)]
形狀一模一樣。如何理解呢?觀察狀態(tài)方程(1)式我們知道，x(k) = Constant( x(k-1) ) + v(k-1) 也就是x(k)由一個通過x(k-1)計算出的常數(shù)疊加一個噪聲得到?？聪旅娴膱D：如果沒有噪聲，x(k)完全由x(k-1)計算得到，也就沒由概率分布這個概念了，由于出現(xiàn)了噪聲，所以x(k)不好確定，他的分布就如同圖中的陰影部分，實(shí)際上形狀和噪聲是一樣的，只是進(jìn)行了一些平移。理解了這一點(diǎn)，對粒子濾波程序中，狀態(tài)x(k)的采樣的計算很有幫助，要采樣x(k)，直接采樣一個過程噪聲，再疊加上 f(x(k-1)) 這個常數(shù)就行了。
更新：由[圖片上傳失敗...(image-e21bd1-1526518178194)]
得到后驗概率[圖片上傳失敗...(image-e1ab07-1526518178194)]
。這個后驗概率才是真正有用的東西，上一步還只是預(yù)測，這里又多了k時刻的測量，對上面的預(yù)測再進(jìn)行修正，就是濾波了。這里的后驗概率也將是代入到下次的預(yù)測，形成遞推。
推導(dǎo)：
[圖片上傳失敗...(image-1e1e4a-1526518178194)]
[圖片上傳失敗...(image-8ff6fb-1526518178194)]
其中歸一化常數(shù)：
[圖片上傳失敗...(image-cd9593-1526518178194)]
等式第一行到第二行是因為測量方程知道, y(k)只與x(k)有關(guān)，[圖片上傳失敗...(image-9789e8-1526518178194)]
也稱之為似然函數(shù)，由量測方程決定。也和上面的推理一樣，[圖片上傳失敗...(image-cfdf54-1526518178194)]
, x(k)部分是常數(shù)，[圖片上傳失敗...(image-5750c0-1526518178194)]
也是只和量測噪聲n(k)的概率分布有關(guān)，注意這個也將為SIR粒子濾波里權(quán)重的采樣提供編程依據(jù)。
貝葉斯濾波到這里就告一段落了。但是，請注意上面的推導(dǎo)過程中需要用到積分，這對于一般的非線性，非高斯系統(tǒng)，很難得到后驗概率的解析解。為了解決這個問題，就得引進(jìn)蒙特卡洛采樣。
reference：
1.M. Sanjeev Arulampalam 《A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking》
2.ZHE CHEN 《Bayesian Filtering: From Kalman Filters to Particle Filters, and Beyond》
3.Sebastian THRUN 《Probabilistic Robotics》
3.百度文庫 《粒子濾波理論》
二、蒙特卡洛采樣
假設(shè)我們能從一個目標(biāo)概率分布p(x)中采樣到一系列的樣本（粒子）[圖片上傳失敗...(image-21ae5d-1526518178194)]
，（至于怎么生成服從p(x)分布的樣本，這個問題先放一放），那么就能利用這些樣本去估計這個分布的某些函數(shù)的期望值。譬如：
[圖片上傳失敗...(image-97b4be-1526518178194)]
[圖片上傳失敗...(image-7c1d6d-1526518178194)]
上面的式子其實(shí)都是計算期望的問題，只是被積分的函數(shù)不同。
蒙特卡洛采樣的思想就是用平均值來代替積分，求期望：
[圖片上傳失敗...(image-2983c9-1526518178194)]
這可以從大數(shù)定理的角度去理解它。我們用這種思想去指定不同的f(x)以便達(dá)到估計不同東西的目的。比如：要估計一批同齡人的體重，不分男女，在大樣本中男的有100個，女的有20個，為了少做事，我們按比例抽取10個男的，2個女的，測算這12個人的體重求平均就完事了。注意這里的按比例抽取，就可以看成從概率分布p(x)中進(jìn)行抽樣。
下面再看一個稍微學(xué)術(shù)一點(diǎn)的例子：
假設(shè)有一粒質(zhì)地均勻的骰子。規(guī)定在一次游戲中，連續(xù)四次拋擲骰子，至少出現(xiàn)一次6個點(diǎn)朝上就算贏?，F(xiàn)在來估計贏的概率。我們用[圖片上傳失敗...(image-5c360d-1526518178194)]
來表示在第n次游戲中，第k次投擲的結(jié)果，k=1...4。對于分布均勻的骰子，每次投擲服從均勻分布,即：
[圖片上傳失敗...(image-9a3742-1526518178194)]
這里的區(qū)間是取整數(shù)，1,2,3,4,5,6，代表6個面。由于每次投擲都是獨(dú)立同分布的，所以這里的目標(biāo)分布p(x)也是一個均勻分布[圖片上傳失敗...(image-5d0793-1526518178197)]
。一次游戲就是[圖片上傳失敗...(image-7077b8-1526518178197)]
空間中的一個隨機(jī)點(diǎn)。
為了估計取勝的概率，在第n次游戲中定義一個指示函數(shù):
[圖片上傳失敗...(image-4d4ba3-1526518178194)]
其中，指示函數(shù)I{x }是指，若x的條件滿足，則結(jié)果為1，不滿足結(jié)果為0?；氐竭@個問題，這里函數(shù) f()的意義就是單次游戲中，若四次投擲中只要有一個6朝上，f()的結(jié)果就會是1。由此，就可以估計在這樣的游戲中取勝的期望，也就是取勝的概率：
[圖片上傳失敗...(image-cf2a1e-1526518178194)]
當(dāng)抽樣次數(shù)N足夠大的時候，上式就逼近真實(shí)取勝概率了，看上面這種估計概率的方法，是通過蒙特卡洛方法的角度去求期望達(dá)到估計概率的目的。是不是就跟我們拋硬幣的例子一樣，拋的次數(shù)足夠多就可以用來估計正面朝上或反面朝上的概率了。
當(dāng)然可能有人會問，這樣估計的誤差有多大，對于這個問題，有興趣的請去查看我最下面列出的參考文獻(xiàn)2。（啰嗦一句：管的太多太寬，很容易讓我們忽略主要問題。博主就是在看文獻(xiàn)過程中，這個是啥那個是啥，都去查資料，到頭來粒子濾波是干嘛完全不知道了，又重新看資料。個人感覺有問題還是先放一放，主要思路理順了再關(guān)注細(xì)節(jié)。）
接下來，回到我們的主線上，在濾波中蒙特卡洛又是怎么用的呢？
由上面我們知道，它可以用來估計概率，而在上一節(jié)中，貝葉斯后驗概率的計算里要用到積分，為了解決這個積分難的問題，可以用蒙特卡洛采樣來代替計算后驗概率。
假設(shè)可以從后驗概率中采樣到N個樣本，那么后驗概率的計算可表示為：
[圖片上傳失敗...(image-1c11f1-1526518178194)]
其中，在這個蒙特卡洛方法中，我們定義[圖片上傳失敗...(image-37f665-1526518178194)]
,是狄拉克函數(shù)(dirac delta function)，跟上面的指示函數(shù)意思差不多。
看到這里，既然用蒙特卡洛方法能夠用來直接估計后驗概率，現(xiàn)在估計出了后驗概率，那到底怎么用來做圖像跟蹤或者濾波呢？要做圖像跟蹤或者濾波，其實(shí)就是想知道當(dāng)前狀態(tài)的期望值：
[圖片上傳失敗...(image-d13559-1526518178194)]
[圖片上傳失敗...(image-5bf8b3-1526518178194)]
[圖片上傳失敗...(image-b9ef2-1526518178194)]
(1)
也就是用這些采樣的粒子的狀態(tài)值直接平均就得到了期望值，也就是濾波后的值，這里的 f(x) 就是每個粒子的狀態(tài)函數(shù)。這就是粒子濾波了，只要從后驗概率中采樣很多粒子，用它們的狀態(tài)求平均就得到了濾波結(jié)果。
思路看似簡單，但是要命的是，后驗概率不知道啊，怎么從后驗概率分布中采樣！所以這樣直接去應(yīng)用是行不通的，這時候得引入重要性采樣這個方法來解決這個問題。
三、重要性采樣
無法從目標(biāo)分布中采樣，就從一個已知的可以采樣的分布里去采樣如 q(x|y)，這樣上面的求期望問題就變成了：
[圖片上傳失敗...(image-9e7307-1526518178194)]
[圖片上傳失敗...(image-3b1245-1526518178194)]
[圖片上傳失敗...(image-251675-1526518178194)]
(2)式
其中
[圖片上傳失敗...(image-f61a1c-1526518178194)]
[圖片上傳失敗...(image-11ed80-1526518178194)]
由于：
[圖片上傳失敗...(image-77d295-1526518178194)]
所以(2)式可以進(jìn)一步寫成：
[圖片上傳失敗...(image-14ce14-1526518178194)]
[圖片上傳失敗...(image-cee937-1526518178194)]
[圖片上傳失敗...(image-6c34e-1526518178194)]
[圖片上傳失敗...(image-9ca3e7-1526518178194)]
(3)式
上面的期望計算都可以通過蒙特卡洛方法來解決它，也就是說，通過采樣N個樣本[圖片上傳失敗...(image-4dac2c-1526518178194)]
,用樣本的平均來求它們的期望，所以上面的（3）式可以近似為：
[圖片上傳失敗...(image-dd6638-1526518178194)]
(4)式
其中：
[圖片上傳失敗...(image-e7bee4-1526518178194)]
這就是歸一化以后的權(quán)重，而之前在(2)式中的那個權(quán)重是沒有歸一化的。
注意上面的(4)式，它不再是（1）式中所有的粒子狀態(tài)直接相加求平均了，而是一種加權(quán)和的形式。不同的粒子都有它們相應(yīng)的權(quán)重，如果粒子權(quán)重大，說明信任該粒子比較多。
到這里已經(jīng)解決了不能從后驗概率直接采樣的問題，但是上面這種每個粒子的權(quán)重都直接計算的方法，效率低，因為每增加一個采樣，p( x(k) |y(1:k))都得重新計算，并且還不好計算這個式子。所以求權(quán)重時能否避開計算p(x(k)|y(1:k))？而最佳的形式是能夠以遞推的方式去計算權(quán)重，這就是所謂的序貫重要性采樣（SIS），粒子濾波的原型。
下面開始權(quán)重w遞推形式的推導(dǎo)：
假設(shè)重要性概率密度函數(shù)
，這里x的下標(biāo)是0:k，也就是說粒子濾波是估計過去所有時刻的狀態(tài)的后驗。假設(shè)它可以分解為：
后驗概率密度函數(shù)的遞歸形式可以表示為：
[圖片上傳失敗...(image-7c4ee9-1526518178194)]
其中，為了表示方便，將 y(1:k) 用 Y(k) 來表示，注意 Y 與 y 的區(qū)別。同時，上面這個式子和上一節(jié)貝葉斯濾波中后驗概率的推導(dǎo)是一樣的，只是之前的x(k)變成了這里的x(0:k)，就是這個不同，導(dǎo)致貝葉斯估計里需要積分，而這里后驗概率的分解形式卻不用積分。
粒子權(quán)值的遞歸形式可以表示為:
( 5)式
注意，這種權(quán)重遞推形式的推導(dǎo)是在前面（2）式的形式下進(jìn)行推導(dǎo)的，也就是沒有歸一化。而在進(jìn)行狀態(tài)估計的公式為這個公式中的的權(quán)重是歸一化以后的，所以在實(shí)際應(yīng)用中，遞推計算出w(k)后,要進(jìn)行歸一化，才能夠代入(4)式中去計算期望。同時，上面(5)式中的分子是不是很熟悉，在上一節(jié)貝葉斯濾波中我們都已經(jīng)做了介紹，p( y|x ),p( x(k)|x(k-1) )的形狀實(shí)際上和狀態(tài)方程中噪聲的概率分布形狀是一樣的，只是均值不同了。因此這個遞推的(5)式和前面的非遞推形式相比，公式里的概率都是已知的，權(quán)重的計算可以說沒有編程方面的難度了。權(quán)重也有了以后，只要進(jìn)行稍微的總結(jié)就可以得到SIS Filter。
四、Sequential Importance Sampling (SIS) Filter
在實(shí)際應(yīng)用中我們可以假設(shè)重要性分布q()滿足：
[圖片上傳失敗...(image-cd10d5-1526518178189)]
這個假設(shè)說明重要性分布只和前一時刻的狀態(tài)x(k-1)以及測量y(k)有關(guān)了，那么(5)式就可以轉(zhuǎn)化為：
[圖片上傳失敗...(image-c256d2-1526518178194)]
在做了這么多假設(shè)和為了解決一個個問題以后，終于有了一個像樣的粒子濾波算法了，他就是序貫重要性采樣濾波。
下面用偽代碼的形式給出這個算法：
----------------------pseudo code-----------------------------------
For i=1:N
(1)采樣：[圖片上傳失敗...(image-e934dd-1526518178196)]
；
(2)根據(jù)[圖片上傳失敗...(image-4ce46d-1526518178196)]
遞推計算各個粒子的權(quán)重；
End For
粒子權(quán)值歸一化。粒子有了，粒子的權(quán)重有了，就可以由(4)式,對每個粒子的狀態(tài)進(jìn)行加權(quán)去估計目標(biāo)的狀態(tài)了。
-----------------------end -----------------------------------------------
這個算法就是粒子濾波的前身了。只是在實(shí)際應(yīng)用中，又發(fā)現(xiàn)了很多問題，如粒子權(quán)重退化的問題，因此就有了重采樣( resample )，就有了基本的粒子濾波算法。還有就是重要性概率密度q()的選擇問題，等等。都留到[下一章](http://blog.csdn.net/heyijia0327/article/details/41122125) 去解決。
在這一章中，我們是用的重要性采樣這種方法去解決的后驗概率無法采樣的問題。實(shí)際上，關(guān)于如何從后驗概率采樣，也就是如何生成特定概率密度的樣本，有很多經(jīng)典的方法（如拒絕采樣，Markov Chain Monte Carlo，Metropolis-Hastings算法，Gibbs采樣），這里面可以單獨(dú)作為一個課題去學(xué)習(xí)了，有興趣的可以去看看《[統(tǒng)計之都 的一篇博文](http://cos.name/2013/01/lda-math-mcmc-and-gibbs-sampling/)》，強(qiáng)烈推薦，參考文獻(xiàn)里的前幾個也都不錯。
reference：
1. Gabriel A. Terejanu 《Tutorial on Monte Carlo Techniques》
- Taylan Cemgil 《A Tutorial Introduction to Monte Carlo methods, Markov Chain Monte Carlo and Particle Filtering》
3. M. Sanjeev Arulampalam 《A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking》
4. ZHE CHEN 《Bayesian Filtering: From Kalman Filters to Particle Filters, and Beyond》
5.百度文庫《粒子濾波理論》
6. Haykin 《Neural Networks and learning Machines 》Chapter 14
7. 統(tǒng)計之都 <LDA-math-MCMC 和 Gibbs Sampling>
五、重采樣
在應(yīng)用SIS 濾波的過程中，存在一個退化的問題。就是經(jīng)過幾次迭代以后，很多粒子的權(quán)重都變得很小，可以忽略了，只有少數(shù)粒子的權(quán)重比較大。并且粒子權(quán)值的方差隨著時間增大，狀態(tài)空間中的有效粒子數(shù)較少。隨著無效采樣粒子數(shù)目的增加，使得大量的計算浪費(fèi)在對估計后驗濾波概率分布幾乎不起作用的粒子上，使得估計性能下降，如圖所示。
通常采用有效粒子數(shù)來衡量粒子權(quán)值的退化程度，即
這個式子的含義是，有效粒子數(shù)越小，即權(quán)重的方差越大，也就是說權(quán)重大的和權(quán)重小的之間差距大，表明權(quán)值退化越嚴(yán)重。在實(shí)際計算中，有效粒子數(shù)可以近似為：
在進(jìn)行序貫重要性采樣時，若上式小于事先設(shè)定的某一閾值，則應(yīng)當(dāng)采取一些措施加以控制?？朔蜇炛匾圆蓸铀惴?quán)值退化現(xiàn)象最直接的方法是增加粒子數(shù)，而這會造成計算量的相應(yīng)增加，影響計算的實(shí)時性。因此，一般采用以下兩種途徑：(1)選擇合適的重要性概率密度函數(shù)；(2)在序貫重要性采樣之后，采用重采樣方法。
對于第一種方法：選取重要性概率密度函數(shù)的一個標(biāo)準(zhǔn)就是使得粒子權(quán)值的方差最小。關(guān)于這部分內(nèi)容，還是推薦百度文庫的那篇文章《[粒子濾波理論](http://wenku.baidu.com/view/88896d2b453610661ed9f4b4.html)》，他在這里也引申出來了幾種不同的粒子濾波方法。
這里著重講第二種方法，重采樣。
重采樣的思路是：既然那些權(quán)重小的不起作用了，那就不要了。要保持粒子數(shù)目不變，得用一些新的粒子來取代它們。找新粒子最簡單的方法就是將權(quán)重大的粒子多復(fù)制幾個出來，至于復(fù)制幾個？那就在權(quán)重大的粒子里面讓它們根據(jù)自己權(quán)重所占的比例去分配，也就是老大分身分得最多，老二分得次多，以此類推。下面以數(shù)學(xué)的形式來進(jìn)行說明。
前面已經(jīng)說明了求某種期望問題變成了這種加權(quán)和的形式：
[圖片上傳失敗...(image-71eff7-1526518178194)]
(1)
通過重采樣以后，希望表示成：
[圖片上傳失敗...(image-fae9dd-1526518178194)]
(2)
，注意對比(1)和(2)。[圖片上傳失敗...(image-2f8876-1526518178194)]
是第k時刻的粒子。[圖片上傳失敗...(image-78359a-1526518178194)]
是k時刻重采樣以后的粒子。其中n(i)是指粒子[圖片上傳失敗...(image-43b253-1526518178194)]
在產(chǎn)生新的粒子集[圖片上傳失敗...(image-6265a2-1526518178194)]
時被復(fù)制的次數(shù)。（2）式中第一個等號說明重采樣以后，所有的粒子權(quán)重一樣，都是1/N，只是有的粒子多出現(xiàn)了n(i)次。
思路有了，就看具體的操作方法了。在《On resampling algorithms for particle fi lters》 這篇paper里講了四種重采樣的方法。這四種方法大同小異。如果你接觸過遺傳算法的話，理解起來就很容易，就是遺傳算法中那種輪盤賭的思想。
在這里用個簡單的例子來說明：
假設(shè)有3個粒子，在第k時刻的時候，他們的權(quán)重分別是0.1, 0.1 ,0.8, 然后計算他們的概率累計和(matlab 中為cumsum() )得到： [0.1, 0.2, 1]。接著，我們用服從[0,1]之間的均勻分布隨機(jī)采樣3個值，假設(shè)為0.15 , 0.38 和 0.54。也就是說，第二個粒子復(fù)制一次，第三個粒子復(fù)制兩次。
在MATLAB中一句命令就可以方便的實(shí)現(xiàn)這個過程：
[?~, j] = histc(rand(N,1), [0 cumsum(w')]); 關(guān)于histc的用法可以點(diǎn)擊[histc用法](http://blog.csdn.net/carrie8899/article/details/8119650)。
對于上面的過程，還可以對著下面的圖加深理解：
將重采樣的方法放入之前的SIS算法中，便形成了基本粒子濾波算法。
重采樣的思路很簡單，但是當(dāng)你仔細(xì)分析權(quán)重的計算公式時：
[圖片上傳失敗...(image-9e4d35-1526518178188)]
[圖片上傳失敗...(image-5d5471-1526518178188)]
會有疑問，權(quán)重大的就多復(fù)制幾次，這一定是正確的嗎？權(quán)重大，如果是分子大，即后驗概率大，那說明確實(shí)應(yīng)該在后驗概率大的地方多放幾個粒子。但權(quán)重大也有可能是分母小造成的，這時候的分子也可能小，也就是實(shí)際的后驗概率也可能小，這時候的大權(quán)重可能就沒那么優(yōu)秀了。何況，這種簡單的重采樣會使得粒子的多樣性丟失，到最后可能都變成了只剩一種粒子的分身。在遺傳算法中好歹還引入了變異來解決多樣性的問題。當(dāng)然，粒子濾波里也有專門的方法：正則粒子濾波，有興趣的可以查閱相關(guān)資料。
至此，整個粒子濾波的流程已經(jīng)清晰明朗了，在實(shí)際應(yīng)用中還有一些不確定的就是重要性概率密度的選擇。在下一章中，首先引出 SIR 粒子濾波，接著用SIR濾波來進(jìn)行實(shí)踐應(yīng)用。
reference:
N. J. Gordon 《Beyond the Kalman Filter:Particle filters for tracking applications》
百度文庫《粒子濾波理論》
Jeroen D. Hol《On resampling algorithms for particle fi lters》
Gabriel A. Terejanu 《Tutorial on Monte Carlo Techniques》
六、Sampling Importance Resampling Filter (SIR)
SIR濾波器很容易由前面的基本粒子濾波推導(dǎo)出來，只要對粒子的重要性概率密度函數(shù)做出特定的選擇即可。在SIR中，選取：
[圖片上傳失敗...(image-13600e-1526518178193)]
p( x(k)|x(k-1) )這是先驗概率，在第一章貝葉斯濾波預(yù)測部分已經(jīng)說過怎么用狀態(tài)方程來得到它。將這個式子代入到第二章SIS推導(dǎo)出的權(quán)重公式中：
[圖片上傳失敗...(image-c8ef8a-1526518178193)]
得到：
[圖片上傳失敗...(image-eecc5a-1526518178193)]
(1)式
由之前的重采樣我們知道，實(shí)際上每次重采樣以后，有[圖片上傳失敗...(image-4ee395-1526518178193)]
。
所以(1)式可以進(jìn)一步簡化成：
[圖片上傳失敗...(image-fce9ff-1526518178193)]
這里又出來一個概率采樣[圖片上傳失敗...(image-759db7-1526518178188)] ，實(shí)際怎么得到這個概率，程序里面又怎么去采樣呢？
先搞清這個概率[[圖片上傳失敗...(image-8063dc-1526518178188)]](http://www.codecogs.com/eqnedit.php?latex=\dpi{100}&space;p\left&space;(&space;y_{k}|x_{k}^{i}&space;\right&space;)) 的含義，它表示在狀態(tài)x出現(xiàn)的條件下，測量y出現(xiàn)的概率。在機(jī)器人定位里面就是，在機(jī)器人處于位姿x時，此時傳感器數(shù)據(jù)y出現(xiàn)的概率。更簡單的例子是，我要找到一個年齡是14歲的男孩（狀態(tài)x），身高為170（測量y）的概率。要知道y出現(xiàn)的概率，需要知道此時y的分布。這里以第一篇文章的狀態(tài)方程為例，由系統(tǒng)狀態(tài)方程可知，測量是在真實(shí)值附近添加了一個高斯噪聲。因此，y的分布就是以真實(shí)測量值為均值，以噪聲方差為方差的一個高斯分布。因此，權(quán)重的采樣過程就是：當(dāng)粒子處于x狀態(tài)時，能夠得到該粒子的測量y。要知道這個測量y出現(xiàn)的概率，就只要把它放到以真實(shí)值為均值，噪聲方差為方差的高斯分布里去計算就行了：
[](http://www.codecogs.com/eqnedit.php?latex=\dpi{100}&space;w&space;=&space;\eta&space;(2\pi&space;\Sigma&space;)^{-1/2}exp\left&space;\{&space;-\frac{1}{2}\left&space;(&space;y_{true}-y&space;\right&space;)&space;\Sigma^{-1}\left&space;(&space;y_{true}-y&space;\right&space;)\right&space;\})
到這里，就可以看成SIR只和系統(tǒng)狀態(tài)方程有關(guān)了，不用自己另外去設(shè)計概率密度函數(shù)，所以在很多程序中都是用的這種方法。
下面以偽代碼的形式給出SIR濾波器：
----------------------SIR Particle Filter pseudo code-----------------------------------
[圖片上傳失敗...(image-ab9a96-1526518178188)]
-
FOR i = 1:N
(1)采樣粒子：[圖片上傳失敗...(image-52f8cc-1526518178193)] (2)計算粒子的權(quán)重：[圖片上傳失敗...(image-89759a-1526518178193)] END FOR
計算粒子權(quán)重和，t=sum(w)
對每個粒子，用上面的權(quán)重和進(jìn)行歸一化,w = w/t
-
粒子有了，每個粒子權(quán)重有了，進(jìn)行狀態(tài)估計 image
重采樣
-----------------------end -----------------------------------------------
在上面算法中，每進(jìn)行一次，都必須重采樣一次，這是由于權(quán)重的計算方式?jīng)Q定的。
分析上面算法中的采樣，發(fā)現(xiàn)它并沒有加入測量y(k)。只是憑先驗知識p( x(k)|x(k-1) )進(jìn)行的采樣，而不是用的修正了的后驗概率。所以這種算法存在效率不高和對奇異點(diǎn)(outliers)敏感的問題。但不管怎樣，SIR確實(shí)簡單易用。
七、粒子濾波的應(yīng)用
在這里主要以第一章的狀態(tài)方程作為例子進(jìn)行演示。
[圖片上傳失敗...(image-84090-1526518178188)]
[圖片上傳失敗...(image-f3de29-1526518178188)]
在這個存在過程噪聲和量測噪聲的系統(tǒng)中，估計狀態(tài)x(k)。
[plain] view plain copy
<embed id="ZeroClipboardMovie_1" src="https://csdnimg.cn/public/highlighter/ZeroClipboard.swf" loop="false" menu="false" quality="best" bgcolor="#ffffff" name="ZeroClipboardMovie_1" allowscriptaccess="always" allowfullscreen="false" type="application/x-shockwave-flash" pluginspage="http://www.macromedia.com/go/getflashplayer" flashvars="id=1&width=16&height=16" wmode="transparent" align="middle" width="16" height="16">
<embed id="ZeroClipboardMovie_2" src="https://csdnimg.cn/public/highlighter/ZeroClipboard.swf" loop="false" menu="false" quality="best" bgcolor="#ffffff" name="ZeroClipboardMovie_2" allowscriptaccess="always" allowfullscreen="false" type="application/x-shockwave-flash" pluginspage="http://www.macromedia.com/go/getflashplayer" flashvars="id=2&width=16&height=16" wmode="transparent" align="middle" width="16" height="16">
%% SIR粒子濾波的應(yīng)用，算法流程參見博客http://blog.csdn.net/heyijia0327/article/details/40899819
clear all
close all
clc
%% initialize the variables
x = 0.1; % initial actual state
x_N = 1; % 系統(tǒng)過程噪聲的協(xié)方差 (由于是一維的，這里就是方差)
x_R = 1; % 測量的協(xié)方差
T = 75; % 共進(jìn)行75次
N = 100; % 粒子數(shù)，越大效果越好，計算量也越大
%initilize our initial, prior particle distribution as a gaussian around
%the true initial value
V = 2; %初始分布的方差
x_P = []; % 粒子
% 用一個高斯分布隨機(jī)的產(chǎn)生初始的粒子
for i = 1:N
x_P(i) = x + sqrt(V) * randn;
end
z_out = [x^2 / 20 + sqrt(x_R) * randn]; %實(shí)際測量值
x_out = [x]; %the actual output vector for measurement values.
x_est = [x]; % time by time output of the particle filters estimate
x_est_out = [x_est]; % the vector of particle filter estimates.
for t = 1:T
x = 0.5x + 25x/(1 + x^2) + 8cos(1.2(t-1)) + sqrt(x_N)*randn;
z = x^2/20 + sqrt(x_R)*randn;
for i = 1:N
%從先驗p(x(k)|x(k-1))中采樣
x_P_update(i) = 0.5x_P(i) + 25x_P(i)/(1 + x_P(i)^2) + 8cos(1.2(t-1)) + sqrt(x_N)*randn;
%計算采樣粒子的值，為后面根據(jù)似然去計算權(quán)重做鋪墊
z_update(i) = x_P_update(i)^2/20;
%對每個粒子計算其權(quán)重，這里假設(shè)量測噪聲是高斯分布。所以 w = p(y|x)對應(yīng)下面的計算公式
P_w(i) = (1/sqrt(2pix_R)) * exp(-(z - z_update(i))^2/(2*x_R));
end
% 歸一化.
P_w = P_w./sum(P_w);
%% Resampling這里沒有用博客里之前說的histc函數(shù)，不過目的和效果是一樣的
for i = 1 : N
x_P(i) = x_P_update(find(rand <= cumsum(P_w),1)); % 粒子權(quán)重大的將多得到后代
end % find( ,1) 返回第一個 符合前面條件的數(shù)的 下標(biāo)
%狀態(tài)估計，重采樣以后，每個粒子的權(quán)重都變成了1/N
x_est = mean(x_P);
% Save data in arrays for later plotting
x_out = [x_out x];
z_out = [z_out z];
x_est_out = [x_est_out x_est];
end
t = 0:T;
figure(1);
clf
plot(t, x_out, '.-b', t, x_est_out, '-.r','linewidth',3);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time step'); ylabel('flight position');
legend('True flight position', 'Particle filter estimate');
濾波后的結(jié)果如下：
這是粒子濾波的一個應(yīng)用，還有一個目標(biāo)跟蹤(matlab)，機(jī)器人定位(python)的例子，我一并放入壓縮文件，供大家下載，[下載請點(diǎn)擊](http://download.csdn.net/detail/heyijia0327/8160645)。(下載需要1個積分，下載完評論資源你就可以賺回那1積分，相當(dāng)于沒損失)。請原諒博主的這一點(diǎn)點(diǎn)自私。兩個程序得效果如下：
粒子濾波從推導(dǎo)到應(yīng)用這個系列到這里就結(jié)束了。結(jié)合前面幾章的問題起來看，基本的粒子濾波里可改進(jìn)的地方很多，正由于此才誕生了很多優(yōu)化了的算法，而這篇博客只理順了基本算法的思路，希望有幫到大家。
另外，個人感覺粒子濾波和遺傳算法真是像極了。同時，如果你覺得這種用很多粒子來計算的方式效率低，在工程應(yīng)用中不好接受，推薦看看無味卡爾曼濾波（UKF）,他是有選擇的產(chǎn)生粒子，而不是盲目的隨機(jī)產(chǎn)生。
reference：
1.M. Sanjeev Arulampalam 《A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking》
其他參考：
https://en.wikipedia.org/wiki/Particle_filter
