粒子濾波原理和仿真

1、粒子濾波算法原理和仿真1 引言粒子濾波(Particle Filter, PF是一種基于蒙特卡洛(Monte Carlo, MC方法的遞推貝葉斯濾波算法。其核心思想是通過從狀態(tài)空間尋找的一系列隨機(jī)樣本來近似系統(tǒng)變量的概率密度函數(shù)，以樣本均值代替積分運(yùn)算，從而獲得狀態(tài)的最小方差估計(jì)。其中從狀態(tài)空間中抽取的樣本稱為“粒子”。一般地，隨著粒子數(shù)目的增加，粒子的概率密度函數(shù)就逐漸逼近狀態(tài)的概率密度函數(shù)，從而達(dá)到最優(yōu)貝葉斯估計(jì)的效果。2 粒子濾波原理2.1 系統(tǒng)的動態(tài)空間對于被觀測對象的狀態(tài)，可以通過以下非線性離散系統(tǒng)來描述： (1 (2以上為系統(tǒng)的狀態(tài)方程和觀測方程。其中，f(為狀態(tài)函數(shù)，h(為觀測函

2、數(shù)，xt是系統(tǒng)在時(shí)間t的狀態(tài)變量，wt為對應(yīng)的過程噪聲，zt是系統(tǒng)在時(shí)間t的觀測值，vt為對應(yīng)的觀測噪聲。從貝葉斯估計(jì)角度來看，狀態(tài)估計(jì)問題就是根據(jù)觀測信息z0:t構(gòu)造狀態(tài)的概率密度函數(shù)p(x0:t|z0:t，從而估計(jì)在系統(tǒng)在任何狀態(tài)下的濾波值。設(shè)系統(tǒng)狀態(tài)序列函數(shù)為gt，則有： (3根據(jù)蒙特卡洛方法，后驗(yàn)概率分布可以用有限的離散樣本來近似，由大數(shù)定律，當(dāng)系統(tǒng)粒子數(shù)N時(shí)，期望Egt(x0:t可近似為： (4式中: i=1,2,.N為狀態(tài)空間中按p(x0:t|z0:t得到的采樣點(diǎn)。2.2 重要性采樣在粒子采集過程中，p(x0:t|z0:t往往是未知且多變的，因此可先從一個(gè)已知且容易采樣的參考分布q

3、(x0:t|z0:t中抽樣，再通過對抽樣粒子集進(jìn)行加權(quán)求和來估計(jì)系統(tǒng)的狀態(tài)值，即： (5令t(x0:t = p(z0:t|x0:tp(x0:t q(x0:t|z0:t，則式(5可表示為： (6按照式(4，式(6可近似為： (7其中為的歸一化權(quán)值，是由q( x0:t z0:t 采樣獲得的粒子。2.3 序貫重要性抽樣貝葉斯估計(jì)是一個(gè)序列估計(jì)問題，通過序貫重要性抽樣(Sequential Importance Sampling, SIS建立采樣粒子的序列關(guān)系。在t+1時(shí)刻采樣時(shí)，不改變狀態(tài)序列過去的樣本集，而采用遞歸的形式計(jì)算重要性權(quán)值。因此參考分布可表示為： (8同時(shí)，假設(shè)系統(tǒng)狀態(tài)是一階馬爾可夫過

4、程，即xt只與xt-1相關(guān)，zt只與xt相關(guān)，因此： (9 (10再將式(8 (10代入t，可推知： (11一般q( xt|x0:t-1 , z0:t =p(xt|xt-1, zt為參考分布的最優(yōu)分布，其概率密度僅依賴于xt-1和zt。但這種選擇在實(shí)際中往往難以實(shí)現(xiàn)，因此多選用q( xt|x0:t-1 , z0:t = p(xt|xt-1來近似。此時(shí)系統(tǒng)僅記錄當(dāng)前狀態(tài)粒子和其權(quán)重，則由式(11可知： (122.4 粒子濾波重采樣粒子退化是序貫重要性采樣算法的主要問題，即經(jīng)過若干次遞推后，權(quán)值方差會逐漸增大，這樣大量權(quán)值都集中在少數(shù)粒子上，而多數(shù)粒子則對系統(tǒng)狀態(tài)的估計(jì)影響甚微，由此嚴(yán)重影響了濾波

5、器的性能。重采樣是解決粒子退化的重要方法，其核心思想是減少或剔除權(quán)值較小的粒子，而復(fù)制權(quán)值較大的粒子，最后將所有粒子的權(quán)值設(shè)為1/N。粒子濾波重采樣方法的如下圖所示：圖1 粒子濾波重采樣示意圖中為采樣得到的粒子，對應(yīng)的權(quán)值為；為重采樣所得的粒子，對應(yīng)權(quán)值為1/N。目前常用的重采樣算法包括隨機(jī)重采樣(random resampling、多項(xiàng)式重采樣(multinomial resampling、系統(tǒng)重采樣(systematic resampling和殘差重采樣(residual resampling，幾種算法的計(jì)算步驟分別如下：1. 隨機(jī)重采樣設(shè)uU(0,1，ui : i=1, 2, ., N

6、獨(dú)立同分布。定義函數(shù)D(，若： (13 則D(ui = m，且。2. 多項(xiàng)式重采樣設(shè)U(0,1， : i=1, 2, ., N 獨(dú)立同分布，定義： (14對函數(shù)D(，由式(13可知：D(ui = m，且。3. 系統(tǒng)重采樣4. 殘差重采樣綜上所述，粒子濾波算法的一般流程為：（1）粒子集初始化，由先驗(yàn)概率密度p(x0產(chǎn)生粒子集，并使所有粒子權(quán)值為1/N；（2）按照對t時(shí)刻的系統(tǒng)進(jìn)行重要性采樣，并通過當(dāng)前觀測值zt根據(jù)式(12計(jì)算粒子權(quán)值；（3）計(jì)算歸一化權(quán)值，并按式(15計(jì)算有效粒子數(shù)Neff； (15（4）若Neff小于設(shè)定閾值Nth，則對粒子進(jìn)行重采樣，得到新的粒子集，其中=1/N，否則；（5

7、）由式(7推知系統(tǒng)的狀態(tài)估計(jì)值為： (16再轉(zhuǎn)到步驟（2），對t+1時(shí)刻的系統(tǒng)進(jìn)行重要性采樣。3 粒子濾波仿真首先構(gòu)造系統(tǒng)的狀態(tài)模型和觀測模型： (17 (18式中，過程噪聲wk N(0, 5，觀測噪聲vk N(0,5。設(shè)系統(tǒng)的狀態(tài)初始值x0 = 0.1，初始分布p(x0 N(0,2。觀測時(shí)長T = 200，每個(gè)時(shí)刻的采樣粒子數(shù)N = 500。觀測時(shí)間內(nèi)，系統(tǒng)狀態(tài)值和觀測值如圖2所示：圖2 系統(tǒng)狀態(tài)值和觀測值圖中的理論狀態(tài)值是系統(tǒng)在無過程噪聲(wt = 0的情況下，通過嚴(yán)格時(shí)序遞推得到的理想值，可以看出，它與系統(tǒng)的真實(shí)狀態(tài)之間存在差異；理論觀測值反映系統(tǒng)真實(shí)狀態(tài)的理論觀測情況(vt = 0，而真實(shí)觀測值是系統(tǒng)在觀測噪聲的干擾下，監(jiān)測所得的實(shí)際數(shù)值。粒子濾波將通過觀的真實(shí)值估計(jì)系統(tǒng)的狀態(tài)，并期望濾波結(jié)果接近于真實(shí)值。對仿真系統(tǒng)的濾波結(jié)果如圖3所示：圖3 系統(tǒng)的濾波效果由圖3知，粒子濾波很好地實(shí)現(xiàn)了對系統(tǒng)狀態(tài)的估計(jì)。其中，為了進(jìn)一步評估濾波的效果，將狀態(tài)的估計(jì)值代入式(18，并令vt = 0，得到濾波后的觀測值。通過式(19可以計(jì)算當(dāng)前觀測信號Z的信噪比SNR： (19式中為觀測信號的理論值，代表Z中真實(shí)有用的成分。表1給出幾組不同濾波器參數(shù)下的觀測信號的信噪比：表1 不同參數(shù)濾波效果比較觀測信號z采樣方差12510SNR(dB-1.2393.9924.0524.747