強(qiáng)跟蹤UKF粒子濾波算法

楊麗華，葛 磊，李保林＋，黃海波

（1.湖北汽車工業(yè)學(xué)院 經(jīng)濟(jì)管理學(xué)院，湖北 十堰442002；2.中國(guó)航天科工集團(tuán)第二研究院706所，北京100854；3.湖北汽車工業(yè)學(xué)院電氣與信息工程學(xué)院，湖北 十堰442002）

0 引 言

就粒 子 濾 波 （particle filtering，PF）［1－7］本 身 而 言，其存在著缺點(diǎn)，主要為粒子退化問題，盡管通過重采樣可以在一定程度上緩解粒子退化，但也由此產(chǎn)生了樣本貧化。因此，眾多學(xué)者通過研究選取重要性密度函數(shù)來緩解粒子退化問題，其中較為有效的是基于擴(kuò)展卡爾曼濾波 （extended kalman filter，EKF）的 擴(kuò) 展 粒 子 濾 波 （extended particle filter，EPF）和基于無(wú)跡卡爾曼濾波 （unscented Kalman filter，UKF）的無(wú)跡粒子濾波 （unscented particle filter，UPF）［8］。然而EPF和UPF雖然在一定程度上可以緩解粒子退化問題，但是由于EKF 和UKF 本身的局限，使兩種算法對(duì)系統(tǒng)模型誤差和狀態(tài)突變的魯棒性不強(qiáng)，當(dāng)系統(tǒng)存在模型誤差或狀態(tài)突變時(shí)，濾波估計(jì)效果不佳，甚至發(fā)散。

為了提高PF對(duì)系統(tǒng)模型誤差和狀態(tài)突變的魯棒性，提高濾波器對(duì)狀態(tài)估計(jì)的跟蹤能力，胡昌華等提出了強(qiáng)跟蹤EPF （strong tracking EPF，STEPF）算法［9］，該算法將強(qiáng)跟蹤EKF （strong tracking EKF，STEKF）與PF結(jié)合，利用STEKF對(duì)粒子進(jìn)行估計(jì)，產(chǎn)生重要性密度函數(shù)，并更新粒子。由于STEKF對(duì)狀態(tài)具有較強(qiáng)的跟蹤能力，魯棒性較強(qiáng)，故STEPF既避免了粒子退化和樣本貧化，又具有高于STEKF的估計(jì)精度，同時(shí)對(duì)系統(tǒng)狀態(tài)突變具有較強(qiáng)魯棒性。然而STEKF是基于一階線性化截?cái)嗟姆椒?，其估?jì)精度不高，從而影響了STEPF 的估計(jì)精度，并且由于STEKF需要計(jì)算非線性函數(shù)的雅可比矩陣，計(jì)算繁瑣，容易出錯(cuò)，難以模塊化實(shí)現(xiàn)，從而影響了STEPF的數(shù)值穩(wěn)定性，也增加了在實(shí)際應(yīng)用中的實(shí)現(xiàn)難度。

本文基于STEPF引入STEKF產(chǎn)生重要性密度函數(shù)的思 想，提 出 了 將 強(qiáng) 跟 蹤 UKF （strong tracking UKF，STUKF）與PF結(jié)合的強(qiáng)跟蹤UPF （strong tracking UPF，STUPF）算法，該算法利用STUKF產(chǎn)生重要性密度函數(shù)，并更新粒子。由于在非線性高斯系統(tǒng)下，UKF估計(jì)精度可達(dá)非線性函數(shù)的三階泰勒展開，遠(yuǎn)遠(yuǎn)高于EKF的一階，所以STUPF的估計(jì)精度高于STEPF，同時(shí)STUKF 不必求取計(jì)算繁瑣的雅可比矩陣，故STUPF具有較高的數(shù)值穩(wěn)定性，也較容易實(shí)現(xiàn)。

1 STEPF算法

1.1 STEKF算法

考慮如下所示的非線性離散系統(tǒng)

其中：xk∈Rn、yk∈Rm分別為系統(tǒng)狀態(tài)向量和量測(cè)向量；fk（·）：Rn→Rn、hk（·）：Rn→Rm分別為系統(tǒng)非線性函數(shù)和量測(cè)函數(shù)；wk、vk為互不相關(guān)的高斯白噪聲，具有如下的統(tǒng)計(jì)特性

其中：Qk、Rk都為正定矩陣；δk，j為Kronecker－δ函數(shù)。

則STEKF算法的實(shí)現(xiàn)步驟如下

其中

STEKF對(duì)模型誤差和狀態(tài)突變具有良好的魯棒性，能以較快的速度跟蹤狀態(tài)變化，具有較強(qiáng)的實(shí)用性。

1.2 STEPF算法

STEKF雖然具有良好的魯棒性，但是由于STEKF 算法本身的局限，其估計(jì)精度不高，只能達(dá)到泰勒展開的一階精度，在某些情況下難以滿足對(duì)狀態(tài)估計(jì)的高精度要求。而將STEKF 與PF 結(jié)合的STEPF，則可以在保證STEKF的魯棒性的同時(shí)，提高濾波精度。STEPF 算法的實(shí)現(xiàn)步驟如下：

步驟2 已知k＋1時(shí)刻的量測(cè)值yk＋1，利用STEKF算法對(duì)每個(gè)粒子進(jìn)行更新，得到粒子的重要性密度函數(shù)，并根據(jù)重要性密度函數(shù)抽樣， 獲得相應(yīng)粒子｛xik＋1i＝1，2，…，N｝及其概率密度值

步驟3 更新權(quán)值并歸一化

步驟4 對(duì)粒子樣本｛xik＋1i＝1，2，…，N｝重采樣得到新粒子樣本｛xik＋1i＝1，2，…，N｝，并求取狀態(tài)估計(jì)

相比于STEKF，STEPF 具有較高的估計(jì)精度，同時(shí)也克服了粒子退化和樣本貧化的問題，并對(duì)系統(tǒng)模型誤差和狀態(tài)突變具有較強(qiáng)的魯棒性，對(duì)狀態(tài)具有較強(qiáng)的跟蹤能力。但是由于STEKF的估計(jì)精度較低，而STEPF 的粒子更新過程是根據(jù)STEKF產(chǎn)生的重要性密度函數(shù)，所以也直接影響了STEPF的估計(jì)精度，同時(shí)由于STEKF需要計(jì)算雅可比矩陣，計(jì)算較為繁瑣，且容易出錯(cuò)，這也影響了STEPF的數(shù)值穩(wěn)定性。同時(shí)由于雅可比矩陣的計(jì)算難以模塊化實(shí)現(xiàn)，導(dǎo)致STEPF也存在難以模塊化的問題。

因此，尋找一種魯棒性較強(qiáng)、估計(jì)精度更高、可以模塊化實(shí)現(xiàn)的濾波算法，替代STEKF產(chǎn)生重要性密度函數(shù)，成為提高STEPF估計(jì)精度、實(shí)現(xiàn)方便的關(guān)鍵。而STUKF恰恰滿足這些要求，將STUKF與PF結(jié)合的STUPF算法，可得到比STEPF估計(jì)精度更高，更容易實(shí)現(xiàn)的濾波算法。

2 STUPF算法

2.1 STUKF算法

基于非線性系統(tǒng)式 （1）和式 （2）的STUKF 算法可表示如下［10］：

步驟1 已知k 時(shí)刻的狀態(tài)xk的統(tǒng)計(jì)特性（＾xk，Pk），根據(jù)Sigma采樣策略，確定Sigma點(diǎn)ξi，k（i＝0，1，…，L）及其相應(yīng)權(quán)值。

步驟2 計(jì)算ξi，k 經(jīng)非線性函數(shù)（1）傳播后的點(diǎn)γi，k＋1／k，從而得到一步狀態(tài)預(yù)測(cè)及誤差協(xié)方差陣P（l）k＋1／k

步驟5 求得λk＋1后，利用＾xk＋1／k和Pk＋1／k求 取Sigma點(diǎn)，再 通 過 非 線 性 測(cè) 量 函 數(shù) （2）傳 播 后 的 點(diǎn)ηi，k＋1／k，由可得引入 漸 消 因 子 后 的 自 協(xié) 方 差Pyk＋1和互協(xié)方差Pxk＋1yk＋1

步驟6 在獲得新的量測(cè)后yk＋1，即可求得濾波增益Kk＋1、狀態(tài)估計(jì)值和協(xié)方差Pk＋1

由于STUKF算法同樣采用了U 變換，故當(dāng)系統(tǒng)正常時(shí)，STUKF與UKF具有相同的估計(jì)精度，但是當(dāng)系統(tǒng)具有模型誤差或狀態(tài)突變時(shí)，UKF的濾波增益不會(huì)自適應(yīng)調(diào)整，導(dǎo)致狀態(tài)估計(jì)不準(zhǔn)確，嚴(yán)重時(shí)濾波會(huì)發(fā)散。而STUKF是基于正交性原理推導(dǎo)出來，通過以強(qiáng)迫輸出殘差序列正交的方法，自適應(yīng)的調(diào)整濾波增益，所以STUKF 對(duì)系統(tǒng)模型誤差和狀態(tài)突變具有良好的魯棒性，即使當(dāng)濾波趨于穩(wěn)定時(shí)，STUKF也會(huì)通過因狀態(tài)異常而導(dǎo)致的量測(cè)異常，自適應(yīng)地調(diào)整一步預(yù)測(cè)方差陣，從而達(dá)到調(diào)整濾波增益、跟蹤真實(shí)狀態(tài)的目的。

同時(shí)由于STUKF采用逼近精度更高的U 變換來近似狀態(tài)的后驗(yàn)均值和協(xié)方差，所以與STEKF 相比，STUKF克服了STEKF通過一階線性化截?cái)鄬?dǎo)致精度偏低的缺點(diǎn)，又避免了計(jì)算雅可比矩陣，能提高濾波的數(shù)值穩(wěn)定性，且有利于算法的模塊化設(shè)計(jì)。

2.2 STUPF算法

STUPF算法的設(shè)計(jì)思想是，利用最新的觀測(cè)數(shù)據(jù)，通過STUKF計(jì)算粒子重要性密度函數(shù)的均值和協(xié)方差，并根據(jù)重要性密度函數(shù)統(tǒng)計(jì)特性采樣得到新的粒子及相應(yīng)權(quán)值，再通過粒子權(quán)值的歸一化和對(duì)粒子的重采樣，再加權(quán)求和，完成對(duì)狀態(tài)的估計(jì)。其具體實(shí)現(xiàn)步驟如下：

步驟1 初始化，設(shè)置粒子數(shù)目N ，并對(duì)N（＾x0，P0）抽樣，得到初始粒子集｛i＝1，2，…，N｝，并設(shè)粒子的權(quán)值皆為1／N。

步驟2 在獲得k＋1 時(shí)刻的量測(cè)值yk＋1后，利用STUKF對(duì)每個(gè)粒子進(jìn)行狀態(tài)估計(jì)，得到相應(yīng)估計(jì)值和 協(xié)方差Pik＋1／k，并以此作為 粒 子 的 重 要 性 密 度 函 數(shù)進(jìn)行抽樣，從而得到新粒子及其概率密度值

步驟3 將粒子權(quán)值進(jìn)行更新，并進(jìn)行歸一化處理

由于STUPF采用STUKF產(chǎn)生重要性密度函數(shù)，所以在一定程度上克服了粒子退化和樣本貧化的問題，當(dāng)系統(tǒng)正常時(shí)，由于UKF 與STUKF 具有相同的估計(jì)精度，故STUPF與UPF也具有相同的估計(jì)精度，所以STUPF算法兼具UPF的估計(jì)精度高和STUKF對(duì)狀態(tài)突變的強(qiáng)跟蹤能力，具有較強(qiáng)的適應(yīng)性，即使當(dāng)系統(tǒng)模型失配或發(fā)生狀態(tài)突變時(shí)，也可以迅速跟蹤狀態(tài)真實(shí)值，完成對(duì)狀態(tài)的估計(jì)。同時(shí)由于STUKF滿足正交性原理，其估計(jì)結(jié)果是無(wú)偏的，而STUPF算法不影響其正交性，故STUPF可以保證估計(jì)值的無(wú)偏和次優(yōu)。

與STEPF相比，由于STUKF 估計(jì)精度高于STEKF，所以由STUKF對(duì)粒子的更新更加集中于狀態(tài)概率密度函數(shù)的高似然區(qū)域，故STUPF 的估計(jì)值更加精確。同時(shí)由于STUKF避免了求取雅可比矩陣，降低了計(jì)算復(fù)雜度，故增加了STUPF算法的可實(shí)現(xiàn)性和數(shù)值穩(wěn)定性，且容易模塊化實(shí)現(xiàn)。

3 數(shù)值仿真

本節(jié)分別采用兩個(gè)非線性模型對(duì)STUPF進(jìn)行仿真，其中例1是粒子濾波算法的標(biāo)準(zhǔn)檢驗(yàn)?zāi)Ｐ?，來?yàn)證STUPF算法對(duì)狀態(tài)的估計(jì)能力，例2 是分時(shí)恒定值問題，來驗(yàn)證STUPF對(duì)狀態(tài)突變的強(qiáng)跟蹤能力。

例1：標(biāo)準(zhǔn)模型的狀態(tài)方程和量測(cè)方程為

其中，wk和vk分別為均值為0，方差分別為Qk＝10，Rk＝1的高斯白噪聲。設(shè)x0＝0.1，P0＝10，ρ＝0.95，取粒子數(shù)為100，進(jìn)行100步的迭代估計(jì)，分別用UPF、STEPF 和STUPF算法對(duì)該模型進(jìn)行濾波，所得的狀態(tài)估計(jì)曲線和均方誤差曲線分別如圖1和圖2所示。

圖1 標(biāo)準(zhǔn)模型的狀態(tài)估計(jì)

圖2 標(biāo)準(zhǔn)模型的均方誤差

從圖1和圖2 可以看到，UPF、STEPF 和STUPF 都能對(duì)該模型進(jìn)行估計(jì)，其中UPF和STUPF估計(jì)效果較好，且精度相當(dāng)，而STEPF 的均方誤差曲線大于UPF 和STUPF，估計(jì)精度稍差。UPF 和STUPF 估計(jì)精度相當(dāng)?shù)脑蚴莾煞N算法的重要性密度函數(shù)UKF 和STUKF，當(dāng)系統(tǒng)沒有模型誤差或狀態(tài)突變時(shí)，具有相同的估計(jì)精度。而STEPF估計(jì)精度稍差的原因是采用估計(jì)精度低于UKF 和STUKF的STEKF作為重要性密度函數(shù)。這也驗(yàn)證了當(dāng)系統(tǒng)正常時(shí)，STUPF和UPF 估計(jì)精度相同，且高于STEPF的理論分析。

例2：分時(shí)恒定值的狀態(tài)方程和量測(cè)方程為

設(shè)T ＝120，Q ＝1，P0＝5，ρ＝0.95，粒子濾波的樣本個(gè)數(shù)為100，進(jìn)行120 步的濾波計(jì)算。分別用UPF、STEPF和STUPF算法對(duì)該系統(tǒng)進(jìn)行濾波，所得的濾波曲線如圖3所示。

圖3 分時(shí)恒定值的狀態(tài)估計(jì)

從圖3可以看到，UPF對(duì)狀態(tài)突變完全失去了跟蹤能力，其估計(jì)值難以跟蹤狀態(tài)真實(shí)值，產(chǎn)生較大誤差，魯棒性較差。而STEPF和STUPF對(duì)狀態(tài)突變都具有較強(qiáng)的跟蹤能力，都可以在狀態(tài)突變后迅速跟蹤到真實(shí)狀態(tài)，并且估計(jì)精度都很高。但是，由于STEPF采用STEKF 作為重要性密度函數(shù)，而STEKF的估計(jì)精度不高，所以導(dǎo)致在對(duì)狀態(tài)突變時(shí)刻進(jìn)行估計(jì)時(shí)，STEPF的估計(jì)值與真實(shí)值的誤差較大，行成一個(gè)尖刺，影響了狀態(tài)估計(jì)的平滑性，而STUPF則由于估計(jì)精度高，避免了這一缺陷，從而驗(yàn)證了STUPF具有較高的估計(jì)精度和對(duì)狀態(tài)突變較強(qiáng)的跟蹤能力。

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

STEPF算法對(duì)系統(tǒng)模型誤差和狀態(tài)突變具有較強(qiáng)的魯棒性，但是由于STEPF 以STEKF 作為重要性密度函數(shù)，導(dǎo)致其估計(jì)精度受到影響，且需要計(jì)算雅可比矩陣，增加了實(shí)現(xiàn)難度，影響了數(shù)值穩(wěn)定性。本文提出的STUPF算法使用估計(jì)精度更高的STUKF 作為重要性密度函數(shù)，兼具了UPF估計(jì)精度高和STUKF 較強(qiáng)魯棒性的優(yōu)點(diǎn)，有效克服了STEPF 所存在的缺點(diǎn)。數(shù)值仿真結(jié)果表明，STUPF在具有較強(qiáng)的魯棒性的同時(shí)，具有高于STEPF 的估計(jì)精度。

［1］YAO Anbang.A compact association of particle filtering and kernel based object tracking ［J］.Pattern Recognition，2012，45 （7）：2584－2597.

［2］YUAN Chen.Particle filtering for blind equalization with unknown noise variance ［J］.Advanced Materials Research，2012，462：10－16.

［3］Orguner U，Gustafsson F.Target tracking with particle filters under signal propagation delays ［J］.IEEE Transactions on Signal Processing，2011，59 （6）：2485－2495.

［4］Closas P，F(xiàn)emandez－Prades C.Bayesian nonlinear filters for direct position estimation ［C］／／IEEE Aerospace Coference，2010：1－12.

［5］ZHANG Miaohui，LIU Xiansheng.Target tracking algorithm based on MCMC unscented particle filter ［J］.Systems Engineering and Electronics，2009，31 （8）：1810－1813 （in Chinese）.［張苗輝，劉先省.基于MCMC 無(wú)味粒子濾波的目標(biāo)跟蹤 算 法 ［J］.系 統(tǒng) 工 程 與 電 子 技 術(shù)，2009，31 （8）：1810－1813.］

［6］SUN F，TANG LJ.Improved particle filter algorithm for INS／GPS integrated navigation system ［C］／／IEEE International Conference on Mechatronics and Automation， 2011：2392－2396.

［7］Hai H Hoang，Bing W Kwan.Suboptimal particle filtering for MIMO flat fading channel estimation ［J］.International Journal of Communication Systems，2013，26 （3）：356－368.

［8］Seok－Han Lee.Real－time camera tracking using aparticle filter combined with unscented kalman filters ［J］.Journal of Electronic Imaging，2014，23 （1）：1－18.

［9］HU Changhua，ZHANG Qi，QIAO Yukun.A strong tracking particle filter with application to fault prediction ［J］.Acta Automatica Sinica，2008，34 （12）：1522－1528 （in Chinese）.［胡昌華，張琪，喬玉坤.強(qiáng)跟蹤粒子濾波算法及其在故障預(yù)報(bào)中的應(yīng)用 ［J］.自動(dòng)化學(xué)報(bào)，2008，34 （12）：1522－1528.］

［10］WANG Xiaoxu，ZHAO Lin，XIA Quanxi，et al.Strong tracking filter based on Unscented transformation ［J］.Control and Decision，2010，25 （7）：1063－1068 （in Chinese）.［王小旭，趙琳，夏全喜，等.基于Unscented變換的強(qiáng)跟蹤濾波器 ［J］.控制與決策，2010，25 （7）：1063－1068.］