粒子濾波學(xué)習(xí)筆記

本文參考博客地址

粒子濾波

粒子濾波的狀態(tài)轉(zhuǎn)移方程和觀測(cè)方程如下：
$x(t) = f(x(t - 1), u(t), w(t))$
$y(t) = f(x(t), e(t))$
其中的x(t)為t時(shí)刻狀態(tài)吹泡，u(t)為控制量匾效，w(t)和e(t)分別為狀態(tài)噪音和觀測(cè)噪音，粒子濾波從觀測(cè)y(t)和上個(gè)時(shí)刻狀態(tài)x(t - 1)株汉，u(t)，w(t)中過濾出t時(shí)刻的狀態(tài)x(t)的過程如下：
1歌殃、初始狀態(tài)：用大量粒子模擬X(t)乔妈，粒子在空間均勻分布。
2挺份、預(yù)測(cè)階段：要依據(jù)上個(gè)時(shí)間點(diǎn)系統(tǒng)的狀態(tài)的概率分布來采樣褒翰，產(chǎn)生的樣本稱為粒子，它們的分布就是上個(gè)時(shí)間點(diǎn)系統(tǒng)狀態(tài)的概率分布，根據(jù)狀態(tài)轉(zhuǎn)移方程废酷，每個(gè)粒子得到一個(gè)預(yù)測(cè)粒子搁吓；
3、校正階段：根據(jù)條件概率 $p(y|x_i)$ 對(duì)預(yù)測(cè)粒子進(jìn)行評(píng)價(jià)揣非，該條件概率值即為粒子的權(quán)重，越接近于真實(shí)狀態(tài)的粒子躲因，其權(quán)重越大早敬。
4、重采樣：根據(jù)粒子權(quán)重對(duì)粒子進(jìn)行篩選大脉，即要大量保留權(quán)重大的粒子搞监，又要有一小部分權(quán)重小的粒子。
5镰矿、將重采樣后的粒子帶入步驟2狀態(tài)轉(zhuǎn)移方程得到新的預(yù)測(cè)粒子

粒子濾波理論

推導(dǎo)過程參考
貝葉斯濾波
貝葉斯濾波實(shí)際上就是根據(jù)上一時(shí)刻系統(tǒng)狀態(tài)的概率分布來計(jì)算當(dāng)前時(shí)刻系統(tǒng)狀態(tài)的概率分布
從貝葉斯理論的角度看琐驴，狀態(tài)估計(jì)問題(目標(biāo)跟蹤、信號(hào)濾波)就是根據(jù)之前一系列的已有數(shù)據(jù) $y_{k - 1}$ (后驗(yàn)知識(shí))遞推地計(jì)算出當(dāng)前狀態(tài) $x_k$ 的可信度，即 $p(x_k|y_{1 : k})$ 绝淡，它需要通過預(yù)測(cè)和更新連個(gè)步驟來遞推計(jì)算宙刘，方程如下：
$x_k = f_k(x_{k - 1}, v_{k- 1}) （1）$
$y_k = h_k(x_k, n_k)) （2）$

預(yù)測(cè)過程：預(yù)測(cè)過程通過狀態(tài)轉(zhuǎn)移方程（公式1）來完成，即通過一些先驗(yàn)知識(shí)來對(duì)系統(tǒng)的未來狀態(tài)進(jìn)行猜測(cè)牢酵，猜的是可行度用條件概率 $p(x_k | x_{k - 1})$ 來表示悬包，更新過程中，利用新到的觀測(cè)值 $y_k$ 來修正先驗(yàn)概率( $p(x_k | x_{k - 1})$ )得到后驗(yàn)概率馍乙。
在處理類似問題時(shí)布近，一般先假設(shè)系統(tǒng)的狀態(tài)轉(zhuǎn)移服從隱馬爾科夫模型，即當(dāng)前時(shí)刻的狀態(tài)僅僅和上一個(gè)時(shí)刻的狀態(tài)有關(guān)系潘拨。同時(shí)吊输，假設(shè)k時(shí)刻的測(cè)量數(shù)據(jù)僅僅和k時(shí)刻的狀態(tài) $x_k$ 有關(guān)
公式如下：
$p(x_k|y_{1 : k - 1}) = \int p(x_{k},x_{k - 1}|y_{1 : k - 1})dx_{k - 1}\\ = \int p(x_{k}|x_{k - 1}, y_{1 : k - 1})p(x_{k - 1}|y_{1 : k})dx_{k - 1}\\ = \int p(x_{k}|x_{k - 1})p(x_{k - 1}|y_{1 : k})dx_{k - 1}\\$
更新過程：由 $p(x_k | y_{1 : k - 1})$ 得到后驗(yàn)概率 $p(x_k | y_{1 :k })$ ，這是通過新到的觀測(cè)值 $y_k$ 修正 $p(x_k | y_{1 : k - 1})$ 的過程铁追，公式如下：
$p(x_k|y_{1:k}) = \frac{p(y_k|x_k,y_{1:k - 1})p(x_k|y_{1:k - 1})}{p(y_k|y_{1:k - 1})}\\ = \frac{p(y_k|x_k)p(x_k|y_{1:k - 1})}{p(y_k|y_{1:k - 1})}$

關(guān)于貝葉斯濾波的簡(jiǎn)介就到這里季蚂，我們可以注意到，貝葉斯后驗(yàn)概率的計(jì)算過程中需要用到積分琅束，但是對(duì)于一般的非線性扭屁，非高斯系統(tǒng)，很難得到后驗(yàn)概率的解析解涩禀，所以就得引進(jìn)蒙特卡洛采樣了料滥。

蒙特卡洛采樣

蒙特卡洛采樣通過與總體分布一致的采樣樣本(粒子)來估計(jì)總體分布某些函數(shù)(往往可以表示某一事件的發(fā)生)期望值的一種方法，其思想是用平均值來代替積分艾船。
因?yàn)樨惾~斯后驗(yàn)概率的計(jì)算過程中用到積分葵腹，為了解決積分難的問題，可以用蒙特卡洛采樣來代替計(jì)算后驗(yàn)概率屿岂，假設(shè)可以從后驗(yàn)概率中采樣到N個(gè)樣本践宴，那么后驗(yàn)概率的計(jì)算可以表示為：
$\hat{P}\left ( x_{n} | y_{1 : k}\right ) = \frac{1}{N}\sum_{i = 1}^{N}\delta \left ( x_n - x_n^{(i)} \right )\approx P\left ( x_{n} | y_{1 : k}\right )$
其中 $f(x) = \delta \left ( x_n - x_n^{(i)} \right )$ 為狄拉克函數(shù)(dirac delta function)，即為指示函數(shù)爷怀，用來表示某事件是否發(fā)生阻肩。
既然用蒙特卡洛方法能夠用來直接估計(jì)后驗(yàn)概率，現(xiàn)在估計(jì)出了后驗(yàn)概率运授，那到底怎么用來做圖像跟蹤或者濾波呢烤惊？ $\color{red} {要做圖像跟蹤或?yàn)V波，其實(shí)就是想知道當(dāng)前狀態(tài)的期望值}$ ：
$E\left [ f(x_n) \right ]\approx \int f(x_n) \hat {P}\left ( x_{n} | y_{1 : k}\right ) dx_n \\ = \frac{1}{N}\sum_{i = 1}^{N}\int f(x_n)\delta \left ( x_n - x_n^{(i)} \right )dx_n\\ =\frac{1}{N}\sum_{i = 1}^{N}f(x_n^{(i)})$
從上面可知吁朦，粒子濾波實(shí)際上就是從后驗(yàn)概率（通過貝葉斯濾波或蒙特卡洛采樣得到）中采樣很多粒子柒室，用它們的狀態(tài)(以粒子為參數(shù)的狄拉克函數(shù)，表示某事件是否發(fā)生)求平均就得到了濾波結(jié)果逗宜。
蒙特卡洛采樣的假設(shè)是可以從后驗(yàn)概率分布中采樣N個(gè)樣本雄右，但是此時(shí)后驗(yàn)概率未知剥啤，所以，直接用是不行的不脯，我們需要祭出重要性采樣這個(gè)大神了

重要性采樣

重要性采樣的核心思想就是從任意一個(gè)已知的分布(可以對(duì)應(yīng)系統(tǒng)任意狀態(tài)？)中采樣刻诊，而在用均值估計(jì)期望時(shí)與蒙特卡洛采樣不同的是它采用例子的加權(quán)平均值估計(jì)系統(tǒng)當(dāng)前的期望防楷，下面推導(dǎo)權(quán)重的計(jì)算。
設(shè)當(dāng)前系統(tǒng)狀態(tài)分布為 $q(x|y)$ 即 $q(x_k|y_{1 : k})$ 是可采樣的则涯，這樣以上求期望的式子可寫成（2）式

其中

由于

(2)式可進(jìn)一步寫成：

上面的期望計(jì)算都可以采用蒙特卡洛方法來解決复局，也就是說，通過采樣N個(gè)樣本粟判，再用樣本的均值來估計(jì)當(dāng)前系統(tǒng)狀態(tài)期望亿昏，所以(3)式可近似為：

其中

以上的權(quán)重是歸一化后的權(quán)重，而(2)式中的權(quán)重是沒有歸一化的档礁。
到這里已經(jīng)解決了不能從后驗(yàn)概率直接采樣的問題角钩，但是上面這種每個(gè)粒子的權(quán)重都直接計(jì)算的方法，效率低呻澜，因?yàn)槊吭黾右粋€(gè)采樣 $p(x_k|y_{1 :k})$ 都需要重新計(jì)算递礼，所以我們需要尋找一種能避免每次增加一個(gè)采樣都計(jì)算 $p(x_k|y_{1 :k})$ 的方法，對(duì)的羹幸，我們要引入 $p(x_k|y_{1 :k})$ 的遞推公式脊髓，推導(dǎo)如下：
首先假設(shè)重要性概率密度函數(shù) $q(x_{0 : k}|y_{1 : k})$ ，這里x的下標(biāo)是0:k栅受，也就是說粒子濾波是估計(jì)過去所有時(shí)刻的狀態(tài)的后驗(yàn)将硝。假設(shè)它可以分解為：

則后驗(yàn)概率密度函數(shù)的遞推形式可以表示未：

其中，為了表示方便屏镊，將用來表示依疼，注意 Y 與 y 的區(qū)別。同時(shí)闸衫，上面這個(gè)式子和上一節(jié)貝葉斯濾波中后驗(yàn)概率的推導(dǎo)是一樣的涛贯，只是之前的x(k)變成了這里的x(0:k)，就是這個(gè)不同蔚出，導(dǎo)致貝葉斯估計(jì)里需要積分弟翘，而這里后驗(yàn)概率的分解形式卻不用積分。
粒子權(quán)值的遞歸形式可以表示為:

注意骄酗，這種權(quán)重遞推形式的推導(dǎo)是在前面（2）式的形式下進(jìn)行推導(dǎo)的稀余，也就是沒有歸一化。而在進(jìn)行狀態(tài)估計(jì)的公式為：

這個(gè)公式中的的權(quán)重是歸一化以后的趋翻，所以在實(shí)際應(yīng)用中睛琳，遞推計(jì)算出w(k)后,要進(jìn)行歸一化，才能夠代入(4)式中去計(jì)算期望。
上面(5)式中的分子是不是很熟悉师骗，在上一節(jié)貝葉斯濾波中我們都已經(jīng)做了介紹历等，,的形狀實(shí)際上和狀態(tài)方程中噪聲的概率分布形狀是一樣的，只是均值不同了辟癌。因此這個(gè)遞推的(5)式和前面的非遞推形式相比寒屯，公式里的概率都是已知的，權(quán)重的計(jì)算可以說沒有編程方面的難度了黍少。權(quán)重也有了以后寡夹，只要進(jìn)行稍微的總結(jié)就可以得到SIS Filter。
重要性采樣通過引入重要性概率密度函數(shù)來解決蒙特卡洛采樣中后驗(yàn)概率無法采樣的問題厂置，如今已經(jīng)有很多經(jīng)典的算法來解決關(guān)于如何從后驗(yàn)概率采樣（也就是如何生成特定概率密度的樣本）的問題（如拒絕采樣菩掏，Markov Chain Monte Carlo，Metropolis-Hastings算法昵济，Gibbs采樣）智绸，可以參考博文

序貫重要性采樣濾波(Sequential Importance Sampling(SIS) Filter)

在實(shí)際應(yīng)用中我們假設(shè)重要性分布函數(shù) $q()$ 滿足：
$q(x_k|x_{0:k - 1}) = q(x_k|x_{k - 1},y_k)$
這個(gè)假設(shè)說明重要性分布只和前一時(shí)刻的狀態(tài) $x_{k - 1}$ 以及測(cè)量 $y_k$ 有關(guān)了，那么(5)式就可以轉(zhuǎn)化為：

下面是序貫重要性濾波算法：
SIS采樣形式化如下

具體偽代碼表示如下
For i = 1 : N

采樣：
根據(jù)

遞推計(jì)算各個(gè)例子的權(quán)重
End For

這個(gè)算法（SIS）即為粒子濾波的前身砸紊，因?yàn)樗膶?shí)踐體驗(yàn)中有很多問題（如：粒子權(quán)重退化）因此需要通過重采樣（resample）去解決传于，采用重采樣后，就產(chǎn)生了基本的粒子濾波算法醉顽。
這個(gè)算法還有一個(gè)問題是重要性概率密度函數(shù)的選擇問題沼溜。

重采樣

在SIS采樣濾波中，有一個(gè)粒子退化的問題游添，主要表現(xiàn)在1）經(jīng)過迭代后有些粒子的權(quán)重變得很小系草，可以忽略了2）粒子權(quán)值的方差越來越大，這樣就會(huì)造成無效采樣唆涝，使得大量的計(jì)算浪費(fèi)在對(duì)估計(jì)后驗(yàn)概率分布幾乎不起作用的粒子上找都，使算法性能下降。
通常采用有效厘子湖來衡量粒子退化程度即：

這個(gè)式子的含義是廊酣，有效粒子數(shù)越小能耻，即權(quán)重的方差越大，也就是說權(quán)重大的和權(quán)重小的之間差距大亡驰，表明權(quán)值退化越嚴(yán)重晓猛。在實(shí)際計(jì)算中，有效粒子數(shù)可以近似為：

在進(jìn)行序貫重要性采時(shí)凡辱，若上式小于一定的閾值戒职，則應(yīng)該采取一些措施加以制止。
克服序貫重要性采樣算法權(quán)值退化現(xiàn)象最直接的方法是增加粒子數(shù)透乾，而這會(huì)造成計(jì)算量的相應(yīng)增加洪燥，影響計(jì)算的實(shí)時(shí)性磕秤。因此，一般采用以下兩種途徑：(1)選擇合適的重要性概率密度函數(shù)捧韵；(2)在序貫重要性采樣之后市咆，采用重采樣方法。
這里重要討論第二種再来，即重采樣
重采樣的思路很簡(jiǎn)單床绪，它會(huì)舍棄權(quán)重很小的例子，同時(shí)通過重復(fù)權(quán)重較大的粒子來保持粒子數(shù)目不變其弊，復(fù)制數(shù)量是根據(jù)粒子權(quán)重大小占比分配的
前面提到，求某種期望問題變成了加權(quán)和的形式：

通過重采樣之后膀斋，期望的表示形式時(shí)下面這樣的：

對(duì)比1梭伐、2我們就可以發(fā)現(xiàn)，重采樣之每個(gè)例子的權(quán)重都是1/N(1式中的表示重采樣前的粒子仰担，2式中的表示重采樣之后的粒子)糊识，2式中的表示重采樣后粒子重復(fù)次數(shù)，它可能為0
有了思路之后摔蓝，具體操作方法請(qǐng)參考遺傳算法中的輪盤賭
將重采樣的方法放入之前的SIS算法中赂苗，便可以得到基本的粒子濾波算法。

至此贮尉，粒子濾波的基本算法已經(jīng)給出來了拌滋，下面介紹SIR粒子濾波

Sampling Importance Resampling Filter(SIR)

最后編輯于：2019.04.14 16:48:03

?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請(qǐng)聯(lián)系作者

人面猴
序言：七十年代末，一起剝皮案震驚了整個(gè)濱河市猜谚，隨后出現(xiàn)的幾起案子败砂，更是在濱河造成了極大的恐慌，老刑警劉巖魏铅，帶你破解...
沈念sama閱讀 211,561評(píng)論 6贊 492
死咒
序言：濱河連續(xù)發(fā)生了三起死亡事件昌犹，死亡現(xiàn)場(chǎng)離奇詭異，居然都是意外死亡览芳，警方通過查閱死者的電腦和手機(jī)斜姥，發(fā)現(xiàn)死者居然都...
沈念sama閱讀 90,218評(píng)論 3贊 385
救了他兩次的神仙讓他今天三更去死
文/潘曉璐我一進(jìn)店門，熙熙樓的掌柜王于貴愁眉苦臉地迎上來沧竟，“玉大人铸敏，你說我怎么就攤上這事⊥驼蹋” “怎么了搞坝？”我有些...
開封第一講書人閱讀 157,162評(píng)論 0贊 348
道士緝兇錄：失蹤的賣姜人
文/不壞的土叔我叫張陵，是天一觀的道長(zhǎng)魁袜。經(jīng)常有香客問我桩撮，道長(zhǎng)敦第，這世上最難降的妖魔是什么？我笑而不...
開封第一講書人閱讀 56,470評(píng)論 1贊 283
?港島之戀（遺憾婚禮）
正文為了忘掉前任店量，我火速辦了婚禮芜果，結(jié)果婚禮上，老公的妹妹穿的比我還像新娘融师。我一直安慰自己右钾，他們只是感情好，可當(dāng)我...
茶點(diǎn)故事閱讀 65,550評(píng)論 6贊 385
惡毒庶女頂嫁案：這布局不是一般人想出來的
文/花漫我一把揭開白布旱爆。她就那樣靜靜地躺著舀射，像睡著了一般。火紅的嫁衣襯著肌膚如雪怀伦。梳的紋絲不亂的頭發(fā)上脆烟，一...
開封第一講書人閱讀 49,806評(píng)論 1贊 290
城市分裂傳說
那天，我揣著相機(jī)與錄音房待，去河邊找鬼邢羔。笑死，一個(gè)胖子當(dāng)著我的面吹牛桑孩，可吹牛的內(nèi)容都是我干的拜鹤。我是一名探鬼主播，決...
沈念sama閱讀 38,951評(píng)論 3贊 407
雙鴛鴦連環(huán)套：你想象不到人心有多黑
文/蒼蘭香墨我猛地睜開眼流椒，長(zhǎng)吁一口氣：“原來是場(chǎng)噩夢(mèng)啊……” “哼敏簿！你這毒婦竟也來了？” 一聲冷哼從身側(cè)響起宣虾，我...
開封第一講書人閱讀 37,712評(píng)論 0贊 266
萬榮殺人案實(shí)錄
序言：老撾萬榮一對(duì)情侶失蹤极谊，失蹤者是張志新（化名）和其女友劉穎，沒想到半個(gè)月后安岂，有當(dāng)?shù)厝嗽跇淞掷锇l(fā)現(xiàn)了一具尸體轻猖，經(jīng)...
沈念sama閱讀 44,166評(píng)論 1贊 303
?護(hù)林員之死
正文獨(dú)居荒郊野嶺守林人離奇死亡，尸身上長(zhǎng)有42處帶血的膿包…… 初始之章·張勛以下內(nèi)容為張勛視角年9月15日...
茶點(diǎn)故事閱讀 36,510評(píng)論 2贊 327
?白月光啟示錄
正文我和宋清朗相戀三年域那，在試婚紗的時(shí)候發(fā)現(xiàn)自己被綠了咙边。大學(xué)時(shí)的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片。...
茶點(diǎn)故事閱讀 38,643評(píng)論 1贊 340
活死人
序言：一個(gè)原本活蹦亂跳的男人離奇死亡次员，死狀恐怖败许，靈堂內(nèi)的尸體忽然破棺而出，到底是詐尸還是另有隱情淑蔚，我是刑警寧澤市殷，帶...
沈念sama閱讀 34,306評(píng)論 4贊 330
?日本核電站爆炸內(nèi)幕
正文年R本政府宣布，位于F島的核電站刹衫，受9級(jí)特大地震影響醋寝，放射性物質(zhì)發(fā)生泄漏搞挣。R本人自食惡果不足惜，卻給世界環(huán)境...
茶點(diǎn)故事閱讀 39,930評(píng)論 3贊 313
男人毒藥：我在死后第九天來索命
文/蒙蒙一音羞、第九天我趴在偏房一處隱蔽的房頂上張望囱桨。院中可真熱鬧，春花似錦嗅绰、人聲如沸舍肠。這莊子的主人今日做“春日...
開封第一講書人閱讀 30,745評(píng)論 0贊 21
一樁弒父案窘面，背后竟有這般陰謀
文/蒼蘭香墨我抬頭看了看天上的太陽翠语。三九已至，卻和暖如春财边，著一層夾襖步出監(jiān)牢的瞬間啡专，已是汗流浹背。一陣腳步聲響...
開封第一講書人閱讀 31,983評(píng)論 1贊 266
情欲美人皮
我被黑心中介騙來泰國(guó)打工制圈，沒想到剛下飛機(jī)就差點(diǎn)兒被人妖公主榨干…… 1. 我叫王不留，地道東北人畔况。一個(gè)月前我還...
沈念sama閱讀 46,351評(píng)論 2贊 360
代替公主和親
正文我出身青樓鲸鹦，卻偏偏與公主長(zhǎng)得像，于是被迫代替她去往敵國(guó)和親跷跪。傳聞我的和親對(duì)象是個(gè)殘疾皇子馋嗜，可洞房花燭夜當(dāng)晚...
茶點(diǎn)故事閱讀 43,509評(píng)論 2贊 348