席志紅，付存利

（哈爾濱工程大學(xué) 信息與通信工程學(xué)院，黑龍江 哈爾濱150001）

0 引 言

非線性、非高斯系統(tǒng)的狀態(tài)估計(jì)問題一直是高精度機(jī)動(dòng)目標(biāo)跟蹤領(lǐng)域研究的難點(diǎn)［1］。近年來出現(xiàn)了很多用于目標(biāo)跟蹤的非線性濾波方法，EKF［2］克服了標(biāo)準(zhǔn)卡爾曼濾波不能應(yīng)用于非線性系統(tǒng)的問題，但EKF存在非線性系統(tǒng)不能夠進(jìn)行雅克比矩陣的求解、泰勒級數(shù)線性化只具有一階的精度、要求噪聲服從高斯分布等問題［3］。Julier等人根據(jù)確定性采樣的基本思路，基于Unscented變換（UT）提出了無跡卡爾曼濾波（UKF）［4］。文獻(xiàn) ［5，6］已經(jīng)驗(yàn)證采用UT 變換的方法可以獲得非線性函數(shù)的二階或二階以上精度，濾波性能較EKF有所提高。為克服粒子濾波 （particle filter，PF）存在的粒子退化、粒子貧化、濾波精度低等缺點(diǎn)，本文提出了一種基于MCMC的改進(jìn)粒子濾波算法即權(quán)值自適應(yīng)馬爾可夫蒙特卡洛無跡粒子濾波 （weight adaptive unscented particle filter－Markov chain Monte Carlo，WAUPF－MCMC）。

1 PF及其實(shí)現(xiàn)

PF算法是一種將遞推貝葉斯統(tǒng)計(jì)思想與蒙特卡羅思想相結(jié)合的一種現(xiàn)代信號處理領(lǐng)域中的濾波方法。當(dāng)粒子數(shù)目足夠多時(shí)，修正后的經(jīng)驗(yàn)條件分布將收斂于系統(tǒng)狀態(tài)向量真實(shí)的條件分布，而狀態(tài)向量的估計(jì)值可以通過粒子的均值近似得到。因?yàn)椴捎昧祟A(yù)測和更新來自于系統(tǒng)概率密度函數(shù)的采樣集近似非線性系統(tǒng)的貝葉斯估計(jì)，所以PF得到了工程上的廣泛應(yīng)用。

假設(shè)動(dòng)態(tài)系統(tǒng)狀態(tài)空間模型如下所示

式中：fk－1（·）——系統(tǒng)狀態(tài)函數(shù)，hk（·）——系統(tǒng)量測函數(shù)，xk——系統(tǒng)狀態(tài)向量，zk——系統(tǒng)量測向量，wk和vk為相互獨(dú)立的噪聲，統(tǒng)計(jì)特性如式 （2）所示

式中：δkj——Kronecker－δ函數(shù)，初始狀態(tài)x0與系統(tǒng)狀態(tài)噪聲wk和量測噪聲vk相互獨(dú)立。

標(biāo)準(zhǔn)粒子濾波的實(shí)現(xiàn)步驟如下：

步驟1 初始化粒子和權(quán)值

步驟2 序貫重要性采樣

步驟3 計(jì)算粒子權(quán)值，并歸一化

步驟4 重采樣，得到新的粒子集合［8］。

步驟5 均值和方差的估計(jì)

PF的一個(gè)缺點(diǎn)是粒子退化問題，解決粒子退化的問題主要有2 種方法：一是優(yōu)選重要性密度函數(shù)，例如利用EKF、UKF產(chǎn)生粒子濾波的建議分布函數(shù)；二是在粒子權(quán)值更新后引入重采樣步驟。圖1為重采樣原理，上面的大小不規(guī)則的圓表示系統(tǒng)經(jīng)過重要性采樣后的粒子，圓的直徑越大，代表著權(quán)值越大；下面的圓表示經(jīng)過重采樣后權(quán)值重新分配的粒子，重采樣后所有圓的直徑相同，意味著所有粒子的權(quán)值都變?yōu)?／N 。

圖1 重采樣原理

2 MCMC在PF中的應(yīng)用

針對粒子貧化問題，有效地解決方法是在重采樣過程后對每個(gè)粒子引入可逆跳躍馬爾可夫移動(dòng)步驟，構(gòu)成跳躍馬爾可夫蒙特卡羅 （Markov chain Monte Carlo，MCMC）粒子濾波，即在不影響估計(jì)的后驗(yàn)概率分布的前提下增加重采樣后粒子的多樣性。典型的MCMC方法是通過搜索技術(shù)來獲得函數(shù)的最大后驗(yàn) （maximum a posteriori，MAP）估計(jì)，或者是在同一框架下對傳統(tǒng)的粒子濾波算法進(jìn)行多樣化采樣來實(shí)現(xiàn)的。由構(gòu)造馬爾科夫鏈方法的不同將MCMC 算法分為 MH （metropolis Hastins）和GS （Gibbs sampling）算法［9］。本文采用MH 算法，其具體的實(shí)現(xiàn)步驟如下：

步驟1 令v～U［0，1］。

步驟2 初始化采樣分布。依照狀態(tài)轉(zhuǎn)移概率p（xk｜）得到粒子。

在移動(dòng)判決步驟中，如果采樣粒子的分布與真實(shí)后驗(yàn)分布相同，MCMC移動(dòng)不但不會(huì)改變既定的粒子分布，還能減少粒子之間的相關(guān)性；如果采樣粒子的分布脫離了后驗(yàn)分布，MCMC移動(dòng)可能會(huì)將這些粒子移動(dòng)到真實(shí)的分布區(qū)域。因此將PF算法與MCMC 方法相結(jié)合，可以有效提高粒子多樣性，降低粒子的貧化問題。

3 WAUPF－MCMC及其實(shí)現(xiàn)

標(biāo)準(zhǔn)粒子濾波算法存在粒子退化現(xiàn)象，雖然重采樣可以緩解這個(gè)問題，但不能從本質(zhì)上解決這一缺陷，而且重采樣后可能出現(xiàn)粒子貧化問題，使濾波的性能受到很大影響。本文提出了用UKF 算法產(chǎn)生粒子濾波的建議分布函數(shù)，可以將實(shí)時(shí)觀測信息融入到系統(tǒng)中，不丟失最新觀測的數(shù)據(jù)；然后改進(jìn)標(biāo)準(zhǔn)粒子濾波中的權(quán)值選取和重采樣這2個(gè)步驟，分別使用似然分布自適應(yīng)權(quán)值調(diào)整策略和系統(tǒng)重采樣技術(shù)；最后融入典型的MCMC 抽樣算法，形成了WAUPF－MCMC算法。算法的流程如圖2所示。

步驟2 UKF采樣并進(jìn)行重要性密度函數(shù)采樣。使用基于比例對稱方法產(chǎn)生Sigma點(diǎn)的UKF算法更新采樣粒子進(jìn)行狀態(tài)預(yù)測，遞推步驟如下所示：

（1）UT 變換。計(jì)算2n＋1個(gè)樣本點(diǎn)以及傳統(tǒng)計(jì)算權(quán)值的方法如下

式中：px——x 的協(xié)方差，n——系統(tǒng)狀態(tài)維數(shù)，本文采樣一種基于比例對稱策略產(chǎn)生Sigma點(diǎn)的方法，在權(quán)值選取方面做了改進(jìn)，對應(yīng)的一階、二階權(quán)值為

圖2 WAUPF－MCMC算法流程

式中：λ＝α2（n＋κ）－n，κ——比例參數(shù)。α——比例縮放因子，可以改變?chǔ)恋拇笮碚{(diào)節(jié)Sigma點(diǎn)到中心點(diǎn)的距離。β——狀態(tài)向量的先驗(yàn)分布信息，通過合并協(xié)方差中高階項(xiàng)的動(dòng)差，提高協(xié)方差的近似精度。

（2）根據(jù)系統(tǒng)狀態(tài)方程求樣本點(diǎn)傳遞值

（3）求系統(tǒng)狀態(tài)均值和方差的一步預(yù)測

（4）根據(jù)系統(tǒng)量測方程求取狀態(tài)一步預(yù)測的傳遞值

（5）獲得預(yù)測量的均值和協(xié)方差

式中：pzz——量測方差矩陣，pxz——狀態(tài)向量與量測向量的協(xié)方差矩陣。

（6）計(jì)算濾波增益并輸出系統(tǒng)狀態(tài)向量估計(jì)值和方差

（7）重要性采樣。利用UKF更新后的數(shù)據(jù)使

步驟3 自適應(yīng)權(quán)值調(diào)整并歸一化：

步驟5 MCMC 步驟。根據(jù)系統(tǒng)狀態(tài)方程，采用MH算法進(jìn)行重要性采樣，得到k時(shí)刻的一組預(yù)測粒子。

步驟6 令k＝k＋1，返回步驟2。

4 算法仿真與分析

對于本文提出的新算法采用典型的非線性非高斯模型來驗(yàn)證其有效性，該模型是非線性、非高斯系統(tǒng)目標(biāo)跟蹤中常見的模型，是研究比較各種非線性濾波的經(jīng)典模型，狀態(tài)方程和觀測方程分別為

其中，φ1 ＝0.5，φ2 ＝0.2，φ3 ＝－2，狀態(tài)方程噪聲wk～Γ（3，2），觀測方程噪聲vk～N（0，10－5）。

文獻(xiàn) ［10］已經(jīng)說明了在相同條件下PF－MCMC 和EPF－MCMC的濾波精度要高于EKF、UKF、PF。因此本實(shí)驗(yàn)采用PF－MCMC、EPF－MCMC、WAUPF－MCMC 這3種算法做比較。設(shè)粒子數(shù)N＝100，仿真時(shí)間T＝50s，采樣間隔為1s，α＝1，β＝0，κ＝2，η＝0.01，初始狀態(tài)x0＝1，初始方差P0＝0.75，利用Matlab2011a對3種算法進(jìn)行仿真，結(jié)果如圖3～圖7所示。

圖3 3種算法跟蹤

圖4 3種濾波器的跟蹤誤差曲線

圖5 PF－MCMC后驗(yàn)概率分布

圖6 EPF－MCMC后驗(yàn)概率分布

圖7 WAUPF－MCMC后驗(yàn)概率分布

圖3給出真實(shí)運(yùn)動(dòng)目標(biāo)運(yùn)動(dòng)軌跡與PF－MCMC、EPFMCMC、WAUPF－MCMC的跟蹤曲線。圖4顯示了3種跟蹤算法與真實(shí)軌跡的誤差曲線。可見，PF－MCMC 的誤差值最大，EPF－MCMC 的誤差值次之，WAUPF－MCMC 的誤差最小。圖5～圖7給出了3種濾波算法的后驗(yàn)概率分布情況?？梢钥闯?，EPF－MCMC 利用EKF 生成建議分布函數(shù)代替?zhèn)鹘y(tǒng)PF采用狀態(tài)轉(zhuǎn)移先驗(yàn)概率作為建議分布，改善了濾波效果，WAUPF－MCMC 利用UKF 生成建議分布函數(shù)，在此基礎(chǔ)上融入 MCMC 步驟，濾波效果得到更好改進(jìn)。

為了比較PF－MCMC、EPF－MCMC、WAUPF－MCMC這3種算法的性能，經(jīng)50次獨(dú)立仿真實(shí)驗(yàn)，對比3種算法的均方根誤差 （RMSE），計(jì)算方法為

表1 3種算法性能比較

從表1可以看出，當(dāng)粒子數(shù)N＝100時(shí)3種算法濾波精度相對于N＝50時(shí)均得以提高，但相應(yīng)計(jì)算耗時(shí)增加。當(dāng)粒子數(shù)相同時(shí)，3 種算法中PF－MCMC 計(jì)算耗時(shí)最少，但RMSE最大，不能滿足高精度濾波的要求；EPF－MCMC 在濾波精度和耗時(shí)兩方面都沒有明顯的優(yōu)勢；WAUPF－MCMC的RMSE最小，但耗時(shí)較多。隨著計(jì)算機(jī)硬件水平的不斷提高，以及可以根據(jù)具體的濾波精度要求相應(yīng)的減少粒子數(shù)目和優(yōu)化程序來解決WAUPF－MCMC 的實(shí)時(shí)性問題。因此，WAUPF－MCMC估計(jì)狀態(tài)能夠更好的吻合真實(shí)軌跡，在非線性、非高斯場合的估計(jì)性能較優(yōu)。

5 結(jié)束語

近年來，PF由于自身理論上的一些優(yōu)勢，在解決非線性、非高斯目標(biāo)跟蹤問題得到廣泛應(yīng)用，但是粒子退化仍是尚待解決的難題。雖然重采樣可以緩解這個(gè)問題，但不能從本質(zhì)上解決這一缺陷，而且重采樣后可能出現(xiàn)粒子貧化問題，使濾波的性能受到很大影響。本文從解決粒子退化、粒子多樣性喪失、提高算法濾波精度等角度出發(fā)，提出了一種新型的改進(jìn)粒子濾波算法即WAUPF－MCMC 算法。仿真實(shí)驗(yàn)結(jié)果表明，WAUPF－MCMC 的濾波精度明顯高于PF－MCMC和EPF－MCMC及傳統(tǒng)的非線性濾波器。

［1］YU Xi，ZHANG Tianqi，BAI Juan，et al.Improved algorithm of particle filtering ［J］.Computer Engineering and Design，2012，33 （10）：4010－4014 （in Chinese）.［余熙，張?zhí)祢U，白娟，等.改進(jìn)的粒子濾波算法 ［J］.計(jì)算機(jī)工程與設(shè)計(jì)，2012，33 （10）：4010－4014.］

［2］XIA Nan，QIU Tianshuang，LI Jingchun，et al.A nonlinear filtering algorithm combining the Kalman filter and the particle filter［J］.Acta Electronica Sinica，2013，41 （1）：148－152（in Chinese）.［夏楠，邱天爽，李景春，等.一種卡爾曼濾波與粒子濾波相結(jié)合的非線性濾波算法 ［J］.電子學(xué)報(bào)，2013，41 （1）：148－152.］

［3］Arasaratnam I，Haykin S.Cubature Kalman filters ［J］.IEEE Trans Automatic Control，2008，54 （6）：1254－1269.

［4］REN Fujun，ZHANG Xiuhua，JIANG Yongcheng.Applica－tion of an improved UKF algorithm in mobile－robot localization system ［J］.Journal of Harbin Engineering University，2012，33 （10）：1289－1294 （in Chinese）. ［任福軍，張秀華，姜永成.改進(jìn)UKF算法在移動(dòng)機(jī)器人定位系統(tǒng)中的應(yīng)用 ［J］.哈爾濱工程大學(xué)學(xué)報(bào)，2012，33 （10）：1289－1294.］

［5］Kim J，Vaddi S S，Menon P K，et al.Comparison between nonlinear filtering techniques for spiraling ballistic missilestate estimation ［J］.IEEE Transactions on Aerospace and Electronic Systems，2012，48 （1）：313－328.

［6］SUN Jingle，TANG Linbo，ZHAO Baojun，et al.A new particle filter tracking algorithm based on rayleigh distribution［J］.Journal of Electronics ＆Information Technology，2013，35 （4）：763－769 （in Chinese）. ［孫景樂，唐林波，趙保軍，等.基于瑞利分布的粒子濾波跟蹤算法 ［J］.電子與信息學(xué)報(bào)，2013，35 （4）：763－769.］

［7］Zhan R，Xin Q，Wan J.Modified unscented particle filter for nonlinear Bayesian tracking ［J］.Journal of Systems Engineering and Electronics，2008，19 （1）：7－14.

［8］Scharcanski J，Oliveira A B，Cavalcanti P G，et al.A partclefiltering approach for vehicular tracking adaptive to occlusions［J］.IEEE Transactions on Vehicular Technology，2011，60（2）：381－389.

［9］Assheton A，Hunter A.A shape－based voting algorithm for pedestrian detection and tracking ［J］.Pattern Recognition，2010，44 （5）：1106－1120.

［10］LU Chuanguo，F(xiàn)ENG Xinxi，ZHANG Di，et al.Monte Carlo Markov chain cubature particle filter ［J］.Journal of University of Electronic Science and Technology of China，2012，41 （6）：859－864 （in Chinese）. ［鹿傳國，馮新喜，張迪，等.馬爾可夫鏈蒙特卡羅容積粒子濾波器 ［J］.電子科技大學(xué)學(xué)報(bào)，2012，41 （6）：859－864.］