基于SSUKF的粒子濾波算法研究

楊 萌，郝燕玲，高 偉

(哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，黑龍江 哈爾濱 150001)

粒子濾波是當(dāng)前得到廣泛應(yīng)用的一種非線性濾波，其主要問(wèn)題是粒子退化、樣本貧化以及計(jì)算量大，前兩者會(huì)對(duì)濾波精度產(chǎn)生影響，后者則會(huì)影響算法的快速性.粒子濾波的原理決定了濾波精度與采樣點(diǎn)數(shù)量關(guān)系密切，即要達(dá)到較為理想的濾波精度，就需要大量的采樣點(diǎn)，算法的計(jì)算量也隨之增大.此外，各種改進(jìn)步驟以及對(duì)算法的優(yōu)化也會(huì)增加整個(gè)算法的復(fù)雜性，進(jìn)而影響濾波的實(shí)時(shí)性.由此可以得到粒子濾波的上述3個(gè)問(wèn)題并不是孤立的，而是彼此關(guān)聯(lián)的.也就是說(shuō)，如果想抑制粒子退化和樣本貧化，取得較好的濾波精度，就需要增加采樣點(diǎn)數(shù)量或增大算法的復(fù)雜性，導(dǎo)致濾波的實(shí)時(shí)性降低，反之亦然.因此一種理想的粒子濾波算法應(yīng)該在濾波精度和實(shí)時(shí)性上都具有較好的表現(xiàn)，或者針對(duì)所應(yīng)用的具體場(chǎng)合在這兩方面達(dá)到某種程度的平衡.

1 無(wú)跡粒子濾波(UPF)

1.1 粒子濾波

粒子濾波以蒙特卡羅方法和遞推貝葉斯估計(jì)為基礎(chǔ)，用帶有權(quán)值的隨機(jī)采樣點(diǎn)來(lái)表示需要的后驗(yàn)概率密度［4］，根據(jù)測(cè)量調(diào)整粒子的權(quán)重和位置，獲得狀態(tài)變量的最優(yōu)估計(jì)值.當(dāng)采樣點(diǎn)的數(shù)量足夠多時(shí)，可以認(rèn)為采樣值的統(tǒng)計(jì)特性近似于后驗(yàn)概率密度的統(tǒng)計(jì)特性.

具體計(jì)算步驟如下:

1)初始化粒子，設(shè)置其初始均值和方差.

2)產(chǎn)生粒子:xik～π(xk|xik-1，yk)，選取重要性密度為先驗(yàn)密度.

3)重要性權(quán)值遞推:

然后對(duì)權(quán)值進(jìn)行歸一化處理.

4)再采樣:定義 Neff來(lái)衡量有效粒子數(shù)量，給定某一門限值 Nthr，當(dāng) Neff＜Nthr時(shí)，執(zhí)行再采樣.產(chǎn)生新的粒子集，重新設(shè)定粒子的權(quán)值為:wik=1/N.

5)輸出一組粒子{(xik，wik)，i=1，…，N}，得到當(dāng)前時(shí)間步的后驗(yàn)均值估計(jì)或加權(quán)均值估計(jì).

1.2 UPF 算法

UPF算法即在PF的基礎(chǔ)上利用UKF獲得重要性概率密度.每一次采樣后的粒子都由UKF算法進(jìn)行更新，所得的均值和方差用于下次采樣新的粒子［5］.在重要性采樣環(huán)節(jié)引入最新的觀測(cè)量，可以在一定程度上抑制粒子退化，但UKF也增加了算法的計(jì)算量.UKF采用UT變換，非線性函數(shù)無(wú)需線性化，將被估計(jì)狀態(tài)的后驗(yàn)概率密度和方差精確到三階，使分布朝高似然度區(qū)域移動(dòng)，獲得的重要性分布函數(shù)更接近狀態(tài)的后驗(yàn)概率密度，因此濾波精度相對(duì)PF有較大提高.

2 超球面分布采樣UKF(SSUKF)

UKF算法的計(jì)算效率很大程度上取決于UT變換中采樣點(diǎn)的個(gè)數(shù).對(duì)于n維隨機(jī)向量，標(biāo)準(zhǔn)的UT變換采用2n+1個(gè) sigma點(diǎn)對(duì)稱采樣［1］，計(jì)算量隨向量維數(shù)的增大而增長(zhǎng).超球面分布采樣變換是通過(guò)n+1個(gè)分布在以隨機(jī)狀態(tài)的均值為原點(diǎn)的超球面上的等權(quán)值sigma點(diǎn)來(lái)近似狀態(tài)的概率分布，由n+1個(gè)超球面分布采樣點(diǎn)和狀態(tài)的均值點(diǎn)構(gòu)成了n+2個(gè)Unscented變換采樣點(diǎn).

均值為0，均方差陣為單位陣的n維隨機(jī)變量y的超球面分布采樣點(diǎn)算法如下［5］:

1)選擇權(quán)值 w0，滿足:0≤w0≤1.

2)確定權(quán)值wi:

3)初始化向量序列:

4)擴(kuò)展向量序列(j=2，…，n):

式中:eji表示j維隨機(jī)變量的第i個(gè)采樣點(diǎn);0j表示j維零向量.

通過(guò)上述算法得到y(tǒng)的n+2個(gè)采樣點(diǎn)eni，i=0，1，…，n+1.對(duì)于均值為 x^，均方差為 Pxx的 n 維隨機(jī)變量x的超球面分布采樣點(diǎn)可以由下式得到:

SSUKF用n+2個(gè)超球面分布的采樣點(diǎn)代替了2n+1個(gè)對(duì)稱分布的采樣點(diǎn)，對(duì)于多維系統(tǒng)而言，減少了采樣點(diǎn)個(gè)數(shù)，減少了計(jì)算量.

3 粒子群優(yōu)化算法

PSO初始化為一群隨機(jī)粒子，然后計(jì)算適配值對(duì)每個(gè)粒子進(jìn)行評(píng)價(jià)，最后通過(guò)迭代找到最優(yōu)解［6］.在每次迭代的過(guò)程中，粒子通過(guò)跟蹤2個(gè)極值來(lái)更新自己的速度和位置.第i個(gè)粒子的位置表示為 Xi=(xi1，xi2，…，xin)，速度 Vi=(vi1，vi2，…，vin).個(gè)體極值 pbestPi=(pi1，pi2，…，pin);全局極值gbestGi=(gi1，gi2，…，gin).在找到上面 2 個(gè)最優(yōu)解之后，每個(gè)粒子根據(jù)下面兩式來(lái)更新其速度和位置:

式中:r1和r2是介于(0，1)區(qū)間的隨機(jī)數(shù);c1和c2稱為學(xué)習(xí)因子，本文仿真中取c1=c2=2;w稱為慣性因數(shù)，w較大則算法具有較強(qiáng)的全局搜索能力，w較小則算法傾向于局部搜索，本文仿真中取［0.9 0.4］隨時(shí)間步線性遞減.粒子適應(yīng)度函數(shù)定義如下:

式中:Rk是觀測(cè)噪聲方差，yNew是最新的量測(cè)值，ypred是預(yù)測(cè)量測(cè)值.如果粒子集都分布在真實(shí)狀態(tài)附近，那么粒子群中每個(gè)粒子的適應(yīng)度都很高.反之，如果粒子群中每個(gè)粒子的個(gè)體最優(yōu)值以及粒子群的全局最優(yōu)值都很低，則說(shuō)明粒子沒(méi)有分布在真實(shí)狀態(tài)附近.此時(shí)粒子集根據(jù)最優(yōu)值并利用迭代來(lái)更新每個(gè)粒子的速度與位置，使得粒子不斷地向真實(shí)狀態(tài)靠近.

4 基于SSUKF的粒子濾波算法

基于 SSUKF的粒子濾波算法(SSUPF)即在UPF的UT變換環(huán)節(jié)采用超球面分布的SSUT變換，進(jìn)而獲得重要性概率密度.在理論上，SSUKF的濾波性能與采用對(duì)稱分布采樣的標(biāo)準(zhǔn)UKF相當(dāng)［7］.因此，SSUPF算法既具有UPF充分考慮當(dāng)前時(shí)刻觀測(cè)值的特點(diǎn)，又可以有效提高計(jì)算效率.

PSO-SSUPF算法設(shè)計(jì)如下:

1)初始時(shí)刻，由初始分布p(x0)中得到一組初始粒子，并設(shè)置其初始的均值和方差.設(shè)置粒子初速度，設(shè)置慣性因數(shù)w，學(xué)習(xí)因子c1和c2，求解初始時(shí)刻的全局最優(yōu)解G(0).

3)產(chǎn)生粒子:引入比例參數(shù) c(0＜c＜1)，從SSUKF結(jié)果N(，Pik)中產(chǎn)生c×N個(gè)粒子，從先驗(yàn)概率分布中產(chǎn)生(1-c)×N個(gè)粒子.

4)權(quán)值更新，之后對(duì)其進(jìn)行歸一化處理.

5)再采樣，并重新設(shè)定粒子權(quán)值.在本文仿真中不設(shè)定有效粒子閾值，直接進(jìn)行再采樣.

7)求解當(dāng)前時(shí)刻的全局最優(yōu)解G(t).

5 仿真及結(jié)果分析

5.1 單變量非靜態(tài)增長(zhǎng)模型(UNGM)

利用UNGM模型對(duì)PSO-SSUPF算法的性能和其他粒子濾波的性能進(jìn)行比較，其狀態(tài)方程和觀測(cè)方程如下:

其中觀測(cè)噪聲vt～N(0，1)，系統(tǒng)噪聲為如下的高斯和分布:

為驗(yàn)證PSO-SSUPF濾波方法使用較少的粒子，就可以獲得相對(duì)較高的濾波精度，仿真分別使用50、100個(gè)粒子，建議分布比例參數(shù)c取為0.6.通過(guò)50次獨(dú)立實(shí)驗(yàn)對(duì)目標(biāo)的狀態(tài)進(jìn)行估計(jì).

仿真數(shù)據(jù)在表1中顯示，實(shí)驗(yàn)表明，當(dāng)粒子數(shù)為50時(shí)，使用PSO優(yōu)化的SSUPF性能有了明顯的改善，誤差明顯減小，這是因?yàn)榧尤隤SO后，粒子的多樣性得到增強(qiáng)，精度隨之提高.當(dāng)粒子數(shù)為100時(shí)，本文算法的性能也明顯優(yōu)于其他粒子濾波算法.

表1 不同粒子濾波的RMSE比較Table 1 RMSE of different particle filters

5.2 強(qiáng)非線性非高斯模型

UKF算法中，sigma點(diǎn)數(shù)量的多少?zèng)Q定計(jì)算量的大?。?］.無(wú)論UT變換采用對(duì)稱分布，還是超球面分布，計(jì)算量都會(huì)隨系統(tǒng)狀態(tài)維數(shù)的加大而增長(zhǎng)，即目標(biāo)狀態(tài)維數(shù)越大，sigma點(diǎn)的數(shù)量越多，系統(tǒng)的計(jì)算量也就越大，本文提出的SSUPF在計(jì)算效率上的優(yōu)勢(shì)也就越明顯.

綜合上述條件，利用下面的模型對(duì)算法的性能進(jìn)行驗(yàn)證［8］，該模型是比較各種粒子濾波的算法性能的標(biāo)準(zhǔn)驗(yàn)證程序之一［9］，其狀態(tài)方程和觀測(cè)方程如下:

其中過(guò)程噪聲vt服從Gamma(3，2)分布，量測(cè)噪聲nk服從高斯分布N(0，10-5).系統(tǒng)狀態(tài)估計(jì)為，一次獨(dú)立實(shí)驗(yàn)的均方誤差為:Var=

在實(shí)驗(yàn)中，分別采用標(biāo)準(zhǔn)PF、EKPF以及UPF與本文提出的PSO-SSUPF算法進(jìn)行比較，仿真的粒子數(shù)N分別為100和200個(gè)，量測(cè)時(shí)間為T=60，時(shí)間間隔為1 s，目標(biāo)初始狀態(tài)設(shè)為進(jìn)行x0=1，50次獨(dú)立的實(shí)驗(yàn)，仿真在Windows XP平臺(tái)通過(guò)Matlab編程實(shí)現(xiàn).

圖1給出了在不同c的取值下各運(yùn)行10次，PSO-SSUPF算法與其他粒子濾波算法均方根誤差的對(duì)比曲線.可以得到，當(dāng) c取0時(shí)，算法即相當(dāng)于PSOPF;當(dāng)c取0～0.2之間時(shí)，本文算法的誤差大于UPF和SSUPF，運(yùn)算時(shí)間較少;當(dāng) c取0.3時(shí)，算法均方根誤差與UPF非常接近，運(yùn)算時(shí)間為UPF的29%;當(dāng)c取0.4時(shí)，算法誤差比UPF低17%，運(yùn)算時(shí)間是 UPF的33%;當(dāng) c取0.5時(shí)，算法誤差比UPF低23%，運(yùn)算時(shí)間是UPF的38%;當(dāng)c取0.5以上時(shí)，均方根誤差變化不明顯，運(yùn)算時(shí)間逐漸增大，直到比SSUPF略大.總體上，隨著c值的增加，本文算法在運(yùn)行時(shí)間上也隨之增加，均方根誤差則呈現(xiàn)遞減的趨勢(shì).在實(shí)際應(yīng)用中，通過(guò)調(diào)整c的取值以平衡精度和運(yùn)行效率，c值的合理取值范圍應(yīng)該在0.4與0.7之間，這樣才能取得比較理想的優(yōu)化效果，下面的仿真將c取為0.6.

圖2給出了粒子數(shù)為200條件下標(biāo)準(zhǔn)PF、EKPF與本文算法在一次獨(dú)立實(shí)驗(yàn)中的狀態(tài)估計(jì)結(jié)果.圖3為3種算法的估計(jì)誤差.表2和表3分別給出了經(jīng)過(guò)50次運(yùn)行4種粒子濾波算法的均方根誤差(RMSE)、有效粒子數(shù)以及平均運(yùn)算時(shí)間.

圖2 不同粒子濾波的估計(jì)(N=200)Fig.2 Estimation performance of different particle filters(N=200)

圖3 不同粒子濾波的估計(jì)誤差(N=200)Fig.3 Estimation error of different particle filters(N=200)

在理論上，標(biāo)準(zhǔn)UPF算法與SSUPF算法的復(fù)雜性相同，SSUT的優(yōu)勢(shì)是對(duì)于高維系統(tǒng)sigma點(diǎn)的數(shù)量是標(biāo)準(zhǔn)UT的一半略多.本文算法在使用混合建議分布基礎(chǔ)上，加入了PSO，PSO對(duì)于粒子只是增加了2次加法運(yùn)算，因此對(duì)于算法復(fù)雜性的影響很小.通過(guò)性能指標(biāo)的比較可以得到，在4種濾波算法中，EKPF、UPF和PSO-SSUPF對(duì)于非線性問(wèn)題的適應(yīng)性要明顯好于標(biāo)準(zhǔn)PF，本文算法的精度與UKF相近，較為明顯的好于EKPF.但由于算法的改進(jìn)和優(yōu)化，增大了計(jì)算量，使上述3種算法的運(yùn)算時(shí)間相對(duì)傳統(tǒng)PF算法明顯增大.本文算法的運(yùn)算效率好于UPF，平均運(yùn)算時(shí)間大幅少于UPF.在有效粒子數(shù)方面，UPF和本文算法的有效樣本多于EKPF和標(biāo)準(zhǔn)PF，尤其是本文算法具有較高的粒子利用率，這表明PSO在抑制粒子退化改善粒子多樣性上作用較為明顯.此外，通過(guò)對(duì)表2和表3數(shù)據(jù)的縱向比較可以得到，各種濾波算法的精度會(huì)隨著粒子數(shù)的增大而提高，同時(shí)運(yùn)算時(shí)間也會(huì)隨之增加.這說(shuō)明在對(duì)于快速性要求不高的場(chǎng)合，使用較大數(shù)量的粒子，可以得到較高的濾波精度.

表2 不同粒子濾波的性能比較(N=100)Table 2 Performance comparison of different particle filters(N=100)

表3 不同粒子濾波的性能比較(N=200)Table 3 Performance comparison of different particle filters(N=200)

6 結(jié)束語(yǔ)

本文從兼顧算法計(jì)算效率和精度的角度出發(fā)，對(duì)標(biāo)準(zhǔn)UPF算法加以改進(jìn)，采用SSUKF設(shè)計(jì)重要性概率密度函數(shù)和先驗(yàn)分布相結(jié)合的混合建議分布，并引入PSO方法，提出了一種基于SSUKF的粒子濾波算法，即PSO-SSUPF.通過(guò)仿真對(duì)比本文算法與標(biāo)準(zhǔn)PF、EKPF、UPF的性能，仿真結(jié)果以及統(tǒng)計(jì)數(shù)據(jù)表明:1)這種算法保持了與標(biāo)準(zhǔn)UPF算法相近的濾波精度，優(yōu)于標(biāo)準(zhǔn)PF和EKPF，運(yùn)算時(shí)間明顯少于UPF，而與EKPF相當(dāng).2)由UT變換和SSUT變換的原理可以得到，系統(tǒng)維數(shù)越大，SSUKF在計(jì)算效率上的優(yōu)勢(shì)就將越發(fā)明顯.

本文只是運(yùn)用簡(jiǎn)單的非線性模型對(duì)PSO-SSUPF算法的理論性能進(jìn)行了驗(yàn)證，而這種算法在解決復(fù)雜非線性問(wèn)題時(shí)的性能，以及如何與再采樣環(huán)節(jié)相結(jié)合發(fā)揮出理想的濾波效果還有很大的研究空間.

［1］JULIER S J，UHLMANN J K，DURRANT-WHYTTE H F.A new method for the nonlinear transformation of means and covariance in filter and estimators［J］.IEEE Transactions on Automatic Control，2000，45(3):477-482.

［2］JULIER S J.The spherical simplex unscented transformation［C］//Proceedings of the American Control Conference.Denver，USA，2003.

［3］宮軼松，歸慶明，孫付平，等.智能優(yōu)化方法與粒子濾波技術(shù)融合的分析與展望［J］.海洋測(cè)繪，2009，29(2):74-77.GONG Yisong，GUI Qingming，SUN Fuping，et al.Survey and prospect of the fusion of intelligent compupational approaches and particle filtering technique［J］.Hydrographic Surveying and Charting，2009，29(2):74-77.

［4］HAUG A J.A tutorial on Bayesian estimation and tracking techniques applicable to nonlinear and non-Gaussian processes［R］.［s.l.］MITRE Technical Report，2005.

［5］丁楊斌，申功勛.Unscented粒子濾波在靜基座捷聯(lián)慣導(dǎo)系統(tǒng)大方位失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用研究［J］.航空學(xué)報(bào)，2007，28(2):397-401.DING Yangbin，SHEN Gongxun.Study on unscented particle filter applied in initial alignment of large azimuth misalignment on static base of SINS［J］.Acta Aeronautica et Astronautica Sinica，2007，28(2):397-401.

［6］TOMOAKI K，KEITA N.Real time object tracking on video image sequence using particle swarm optimization［J］.Control，Aotomation and Systems，2007，10:1773-1778.

［7］潘泉，楊峰，葉亮，等.一類非線性濾波器——UKF綜述［J］.控制與決策，2005，20(5):481-489.PAN Quan，YANG Feng，YE Liang，et al.Survey of a kind of nonlinear filters—UKF［J］.Control and Decision，2005，20(5):481-489.

［8］KOTECHA J H，DJURIC P M.Gaussian partical filter［J］.IEEE Transactions on Signal Processing，2003，51(2):2592-2601.

［9］方正，佟國(guó)峰，徐心和.粒子群優(yōu)化粒子濾波方法［J］.控制與決策，2007，22(3):273-277.FANG Zheng，TONG Guofeng，XU Xinhe.Particle swarm optimized particle filter［J］.Control and Decision，2007，22(3):273-277.