粒子濾波作為視覺SLAM中后端進(jìn)行狀態(tài)估計(jì)的主要算法之一，很好的完成了擴(kuò)展卡爾曼濾波無法有效處理的復(fù)雜狀態(tài)方程下的狀態(tài)估計(jì)任務(wù)脓恕。這篇文章詳細(xì)地描述了粒子濾波的思想歷程析砸，即如何一步步從簡單的狀態(tài)估計(jì)、采樣燃异、應(yīng)對多樣性缺失坚嗜，最后到得到相對滿意的粒子濾波的算法的思路夯膀，最后簡單講解了粒子濾波的兩大應(yīng)用：狀態(tài)估計(jì)和目標(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)方程來計(jì)算否过？粒子的權(quán)重是怎么來的午笛？經(jīng)過一段時間的理解惭蟋，總算理清了它的脈絡(luò)苗桂。同時也覺得，只有對理論的推導(dǎo)心中有數(shù)了告组，才能知道什么樣的地方可以用這個算法煤伟，以及這個算法有什么不足。因此木缝，本文將結(jié)合實(shí)際程序給出粒子濾波的詳細(xì)推導(dǎo)便锨，在推導(dǎo)過程中加入博主自己的理解，如有不妥我碟，請指出放案，謝謝。
文章架構(gòu)：
由最基礎(chǔ)的貝葉斯估計(jì)開始介紹矫俺，再引出蒙特卡羅采樣吱殉，重要性采樣掸冤，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)估計(jì)問題（目標(biāo)跟蹤澳淑、信號濾波）就是根據(jù)之前一系列的已有數(shù)據(jù)[圖片上傳失敗...(image-245ca6-1526518178195)]
（后驗(yàn)知識）遞推的計(jì)算出當(dāng)前狀態(tài)[圖片上傳失敗...(image-114896-1526518178195)]
的可信度。這個可信度就是概率公式[圖片上傳失敗...(image-d4f7a-1526518178195)]
插佛，它需要通過預(yù)測和更新兩個步奏來遞推的計(jì)算杠巡。
預(yù)測過程是利用系統(tǒng)模型(狀態(tài)方程1)預(yù)測狀態(tài)的先驗(yàn)概率密度，也就是通過已有的先驗(yàn)知識對未來的狀態(tài)進(jìn)行猜測雇寇，即p( x(k)|x(k-1) )氢拥。更新過程則利用最新的測量值對先驗(yàn)概率密度進(jìn)行修正，得到后驗(yà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)的概率攀隔。
計(jì)算推導(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)驗(yàn)對你進(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)計(jì)算出的常數(shù)疊加一個噪聲得到澈驼。看下面的圖：如果沒有噪聲韧衣，x(k)完全由x(k-1)計(jì)算得到盅藻，也就沒由概率分布這個概念了，由于出現(xiàn)了噪聲畅铭，所以x(k)不好確定，他的分布就如同圖中的陰影部分勃蜘，實(shí)際上形狀和噪聲是一樣的硕噩，只是進(jìn)行了一些平移。理解了這一點(diǎn)缭贡，對粒子濾波程序中炉擅，狀態(tài)x(k)的采樣的計(jì)算很有幫助辉懒，要采樣x(k)，直接采樣一個過程噪聲谍失，再疊加上 f(x(k-1)) 這個常數(shù)就行了眶俩。
更新：由[圖片上傳失敗...(image-e21bd1-1526518178194)]
得到后驗(yàn)概率[圖片上傳失敗...(image-e1ab07-1526518178194)]
。這個后驗(yàn)概率才是真正有用的東西快鱼，上一步還只是預(yù)測颠印，這里又多了k時刻的測量，對上面的預(yù)測再進(jìn)行修正抹竹，就是濾波了线罕。這里的后驗(yàn)概率也將是代入到下次的預(yù)測，形成遞推窃判。
推導(dǎo)：
[圖片上傳失敗...(image-1e1e4a-1526518178194)]
[圖片上傳失敗...(image-8ff6fb-1526518178194)]
其中歸一化常數(shù)：
[圖片上傳失敗...(image-cd9593-1526518178194)]
等式第一行到第二行是因?yàn)闇y量方程知道, 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)痢艺，很難得到后驗(yàn)概率的解析解仓洼。為了解決這個問題，就得引進(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)分布的樣本舌缤，這個問題先放一放）箕戳，那么就能利用這些樣本去估計(jì)這個分布的某些函數(shù)的期望值。譬如：
[圖片上傳失敗...(image-97b4be-1526518178194)]
[圖片上傳失敗...(image-7c1d6d-1526518178194)]
上面的式子其實(shí)都是計(jì)算期望的問題国撵，只是被積分的函數(shù)不同陵吸。
蒙特卡洛采樣的思想就是用平均值來代替積分，求期望：
[圖片上傳失敗...(image-2983c9-1526518178194)]
這可以從大數(shù)定理的角度去理解它介牙。我們用這種思想去指定不同的f(x)以便達(dá)到估計(jì)不同東西的目的壮虫。比如：要估計(jì)一批同齡人的體重，不分男女，在大樣本中男的有100個囚似，女的有20個剩拢，為了少做事，我們按比例抽取10個男的饶唤，2個女的徐伐，測算這12個人的體重求平均就完事了。注意這里的按比例抽取募狂，就可以看成從概率分布p(x)中進(jìn)行抽樣办素。
下面再看一個稍微學(xué)術(shù)一點(diǎn)的例子：
假設(shè)有一粒質(zhì)地均勻的骰子。規(guī)定在一次游戲中熬尺，連續(xù)四次拋擲骰子摸屠，至少出現(xiàn)一次6個點(diǎn)朝上就算贏。現(xiàn)在來估計(jì)贏的概率粱哼。我們用[圖片上傳失敗...(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)逃顶。
為了估計(jì)取勝的概率，在第n次游戲中定義一個指示函數(shù):
[圖片上傳失敗...(image-4d4ba3-1526518178194)]
其中充甚，指示函數(shù)I{x }是指以政，若x的條件滿足，則結(jié)果為1伴找，不滿足結(jié)果為0盈蛮。回到這個問題技矮，這里函數(shù) f()的意義就是單次游戲中抖誉，若四次投擲中只要有一個6朝上，f()的結(jié)果就會是1衰倦。由此袒炉，就可以估計(jì)在這樣的游戲中取勝的期望，也就是取勝的概率：
[圖片上傳失敗...(image-cf2a1e-1526518178194)]
當(dāng)抽樣次數(shù)N足夠大的時候耿币，上式就逼近真實(shí)取勝概率了梳杏，看上面這種估計(jì)概率的方法，是通過蒙特卡洛方法的角度去求期望達(dá)到估計(jì)概率的目的淹接。是不是就跟我們拋硬幣的例子一樣十性，拋的次數(shù)足夠多就可以用來估計(jì)正面朝上或反面朝上的概率了。
當(dāng)然可能有人會問塑悼，這樣估計(jì)的誤差有多大劲适，對于這個問題，有興趣的請去查看我最下面列出的參考文獻(xiàn)2厢蒜。（啰嗦一句：管的太多太寬霞势，很容易讓我們忽略主要問題。博主就是在看文獻(xiàn)過程中斑鸦，這個是啥那個是啥愕贡，都去查資料，到頭來粒子濾波是干嘛完全不知道了巷屿，又重新看資料固以。個人感覺有問題還是先放一放，主要思路理順了再關(guān)注細(xì)節(jié)嘱巾。）
接下來憨琳，回到我們的主線上，在濾波中蒙特卡洛又是怎么用的呢旬昭？
由上面我們知道篙螟，它可以用來估計(jì)概率，而在上一節(jié)中问拘，貝葉斯后驗(yàn)概率的計(jì)算里要用到積分遍略，為了解決這個積分難的問題，可以用蒙特卡洛采樣來代替計(jì)算后驗(yàn)概率骤坐。
假設(shè)可以從后驗(yàn)概率中采樣到N個樣本绪杏，那么后驗(yàn)概率的計(jì)算可表示為：
[圖片上傳失敗...(image-1c11f1-1526518178194)]
其中，在這個蒙特卡洛方法中或油，我們定義[圖片上傳失敗...(image-37f665-1526518178194)]
,是狄拉克函數(shù)(dirac delta function)寞忿，跟上面的指示函數(shù)意思差不多。
看到這里顶岸，既然用蒙特卡洛方法能夠用來直接估計(jì)后驗(yàn)概率腔彰，現(xiàn)在估計(jì)出了后驗(yàn)概率，那到底怎么用來做圖像跟蹤或者濾波呢辖佣？要做圖像跟蹤或者濾波霹抛，其實(shí)就是想知道當(dāng)前狀態(tài)的期望值：
[圖片上傳失敗...(image-d13559-1526518178194)]
[圖片上傳失敗...(image-5bf8b3-1526518178194)]
[圖片上傳失敗...(image-b9ef2-1526518178194)]
(1)
也就是用這些采樣的粒子的狀態(tài)值直接平均就得到了期望值，也就是濾波后的值卷谈，這里的 f(x) 就是每個粒子的狀態(tài)函數(shù)杯拐。這就是粒子濾波了，只要從后驗(yàn)概率中采樣很多粒子，用它們的狀態(tài)求平均就得到了濾波結(jié)果端逼。
思路看似簡單朗兵，但是要命的是，后驗(yàn)概率不知道啊顶滩，怎么從后驗(yàn)概率分布中采樣余掖！所以這樣直接去應(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)式
上面的期望計(jì)算都可以通過蒙特卡洛方法來解決它冗美，也就是說，通過采樣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)解決了不能從后驗(yàn)概率直接采樣的問題柱衔，但是上面這種每個粒子的權(quán)重都直接計(jì)算的方法樊破，效率低，因?yàn)槊吭黾右粋€采樣唆铐，p( x(k) |y(1:k))都得重新計(jì)算哲戚，并且還不好計(jì)算這個式子。所以求權(quán)重時能否避開計(jì)算p(x(k)|y(1:k))艾岂？而最佳的形式是能夠以遞推的方式去計(jì)算權(quán)重顺少，這就是所謂的序貫重要性采樣（SIS），粒子濾波的原型王浴。
下面開始權(quán)重w遞推形式的推導(dǎo)：
假設(shè)重要性概率密度函數(shù)
脆炎，這里x的下標(biāo)是0:k，也就是說粒子濾波是估計(jì)過去所有時刻的狀態(tài)的后驗(yàn)氓辣。假設(shè)它可以分解為：
后驗(yàn)概率密度函數(shù)的遞歸形式可以表示為：
[圖片上傳失敗...(image-7c4ee9-1526518178194)]
其中秒裕，為了表示方便，將 y(1:k) 用 Y(k) 來表示钞啸，注意 Y 與 y 的區(qū)別几蜻。同時喇潘，上面這個式子和上一節(jié)貝葉斯濾波中后驗(yàn)概率的推導(dǎo)是一樣的，只是之前的x(k)變成了這里的x(0:k)梭稚，就是這個不同颖低，導(dǎo)致貝葉斯估計(jì)里需要積分，而這里后驗(yàn)概率的分解形式卻不用積分哨毁。
粒子權(quán)值的遞歸形式可以表示為:
( 5)式
注意枫甲，這種權(quán)重遞推形式的推導(dǎo)是在前面（2）式的形式下進(jìn)行推導(dǎo)的源武，也就是沒有歸一化扼褪。而在進(jìn)行狀態(tài)估計(jì)的公式為這個公式中的的權(quán)重是歸一化以后的，所以在實(shí)際應(yīng)用中粱栖，遞推計(jì)算出w(k)后,要進(jìn)行歸一化话浇，才能夠代入(4)式中去計(jì)算期望。同時闹究，上面(5)式中的分子是不是很熟悉幔崖，在上一節(jié)貝葉斯濾波中我們都已經(jīng)做了介紹，p( y|x ),p( x(k)|x(k-1) )的形狀實(shí)際上和狀態(tài)方程中噪聲的概率分布形狀是一樣的渣淤，只是均值不同了赏寇。因此這個遞推的(5)式和前面的非遞推形式相比，公式里的概率都是已知的价认，權(quán)重的計(jì)算可以說沒有編程方面的難度了嗅定。權(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)]
遞推計(jì)算各個粒子的權(quán)重梅誓；
End For
粒子權(quán)值歸一化。粒子有了佛南，粒子的權(quán)重有了梗掰，就可以由(4)式,對每個粒子的狀態(tài)進(jìn)行加權(quán)去估計(jì)目標(biāo)的狀態(tài)了。
-----------------------end -----------------------------------------------
這個算法就是粒子濾波的前身了共虑。只是在實(shí)際應(yīng)用中愧怜，又發(fā)現(xiàn)了很多問題，如粒子權(quán)重退化的問題妈拌，因此就有了重采樣( resample )拥坛，就有了基本的粒子濾波算法蓬蝶。還有就是重要性概率密度q()的選擇問題，等等猜惋。都留到[下一章](http://blog.csdn.net/heyijia0327/article/details/41122125) 去解決丸氛。
在這一章中，我們是用的重要性采樣這種方法去解決的后驗(yàn)概率無法采樣的問題著摔。實(shí)際上缓窜，關(guān)于如何從后驗(yàn)概率采樣，也就是如何生成特定概率密度的樣本谍咆，有很多經(jīng)典的方法（如拒絕采樣禾锤，Markov Chain Monte Carlo，Metropolis-Hastings算法摹察，Gibbs采樣）恩掷，這里面可以單獨(dú)作為一個課題去學(xué)習(xí)了，有興趣的可以去看看《[統(tǒng)計(jì)之都 的一篇博文](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)計(jì)之都 <LDA-math-MCMC 和 Gibbs Sampling>
五克滴、重采樣
在應(yīng)用SIS 濾波的過程中逼争，存在一個退化的問題。就是經(jīng)過幾次迭代以后劝赔，很多粒子的權(quán)重都變得很小誓焦，可以忽略了，只有少數(shù)粒子的權(quán)重比較大望忆。并且粒子權(quán)值的方差隨著時間增大罩阵，狀態(tài)空間中的有效粒子數(shù)較少。隨著無效采樣粒子數(shù)目的增加启摄，使得大量的計(jì)算浪費(fèi)在對估計(jì)后驗(yàn)濾波概率分布幾乎不起作用的粒子上稿壁，使得估計(jì)性能下降，如圖所示歉备。
通常采用有效粒子數(shù)來衡量粒子權(quán)值的退化程度傅是，即
這個式子的含義是，有效粒子數(shù)越小蕾羊，即權(quán)重的方差越大喧笔，也就是說權(quán)重大的和權(quán)重小的之間差距大，表明權(quán)值退化越嚴(yán)重龟再。在實(shí)際計(jì)算中书闸，有效粒子數(shù)可以近似為：
在進(jìn)行序貫重要性采樣時，若上式小于事先設(shè)定的某一閾值利凑，則應(yīng)當(dāng)采取一些措施加以控制浆劲∠邮酰克服序貫重要性采樣算法權(quán)值退化現(xiàn)象最直接的方法是增加粒子數(shù)，而這會造成計(jì)算量的相應(yīng)增加牌借，影響計(jì)算的實(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, 然后計(jì)算他們的概率累計(jì)和(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)重的計(jì)算公式時：
[圖片上傳失敗...(image-9e4d35-1526518178188)]
[圖片上傳失敗...(image-5d5471-1526518178188)]
會有疑問，權(quán)重大的就多復(fù)制幾次骂际，這一定是正確的嗎疗琉？權(quán)重大，如果是分子大歉铝，即后驗(yàn)概率大盈简，那說明確實(shí)應(yīng)該在后驗(yàn)概率大的地方多放幾個粒子。但權(quán)重大也有可能是分母小造成的太示，這時候的分子也可能小柠贤，也就是實(shí)際的后驗(yàn)概率也可能小，這時候的大權(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àn)概率理茎，在第一章貝葉斯濾波預(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í)值為均值眨八，噪聲方差為方差的高斯分布里去計(jì)算就行了：
[](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è)計(jì)概率密度函數(shù)廉侧，所以在很多程序中都是用的這種方法页响。
下面以偽代碼的形式給出SIR濾波器：
----------------------SIR Particle Filter pseudo code-----------------------------------
[圖片上傳失敗...(image-ab9a96-1526518178188)]
-
FOR i = 1:N
(1)采樣粒子：[圖片上傳失敗...(image-52f8cc-1526518178193)] (2)計(jì)算粒子的權(quán)重：[圖片上傳失敗...(image-89759a-1526518178193)] END FOR
計(jì)算粒子權(quán)重和篓足，t=sum(w)
對每個粒子，用上面的權(quán)重和進(jìn)行歸一化,w = w/t
-
粒子有了闰蚕，每個粒子權(quán)重有了栈拖，進(jìn)行狀態(tài)估計(jì) image
重采樣
-----------------------end -----------------------------------------------
在上面算法中，每進(jìn)行一次没陡，都必須重采樣一次涩哟，這是由于權(quán)重的計(jì)算方式?jīng)Q定的。
分析上面算法中的采樣诗鸭，發(fā)現(xiàn)它并沒有加入測量y(k)染簇。只是憑先驗(yàn)知識p( x(k)|x(k-1) )進(jìn)行的采樣，而不是用的修正了的后驗(yàn)概率强岸。所以這種算法存在效率不高和對奇異點(diǎn)(outliers)敏感的問題锻弓。但不管怎樣，SIR確實(shí)簡單易用蝌箍。
七青灼、粒子濾波的應(yīng)用
在這里主要以第一章的狀態(tài)方程作為例子進(jìn)行演示。
[圖片上傳失敗...(image-84090-1526518178188)]
[圖片上傳失敗...(image-f3de29-1526518178188)]
在這個存在過程噪聲和量測噪聲的系統(tǒng)中妓盲，估計(jì)狀態(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ù)弹沽，越大效果越好，計(jì)算量也越大
%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
%從先驗(yà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ì)算采樣粒子的值筋粗，為后面根據(jù)似然去計(jì)算權(quán)重做鋪墊
z_update(i) = x_P_update(i)^2/20;
%對每個粒子計(jì)算其權(quán)重策橘，這里假設(shè)量測噪聲是高斯分布。所以 w = p(y|x)對應(yīng)下面的計(jì)算公式
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)估計(jì)丽已，重采樣以后，每個粒子的權(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)于沒損失)。請?jiān)彶┲鞯倪@一點(diǎn)點(diǎn)自私。兩個程序得效果如下：
粒子濾波從推導(dǎo)到應(yīng)用這個系列到這里就結(jié)束了枯冈。結(jié)合前面幾章的問題起來看毅贮，基本的粒子濾波里可改進(jìn)的地方很多，正由于此才誕生了很多優(yōu)化了的算法尘奏，而這篇博客只理順了基本算法的思路滩褥，希望有幫到大家。
另外炫加，個人感覺粒子濾波和遺傳算法真是像極了瑰煎。同時，如果你覺得這種用很多粒子來計(jì)算的方式效率低俗孝，在工程應(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
