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

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

0 引 言

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

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

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

1 STEPF算法

1.1 STEKF算法

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

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

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

則STEKF算法的實現步驟如下

其中

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

1.2 STEPF算法

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

步驟2 已知k＋1時刻的量測值yk＋1，利用STEKF算法對每個粒子進行更新，得到粒子的重要性密度函數，并根據重要性密度函數抽樣， 獲得相應粒子｛xik＋1i＝1，2，…，N｝及其概率密度值

步驟3 更新權值并歸一化

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

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

因此，尋找一種魯棒性較強、估計精度更高、可以模塊化實現的濾波算法，替代STEKF產生重要性密度函數，成為提高STEPF估計精度、實現方便的關鍵。而STUKF恰恰滿足這些要求，將STUKF與PF結合的STUPF算法，可得到比STEPF估計精度更高，更容易實現的濾波算法。

2 STUPF算法

2.1 STUKF算法

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

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

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

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

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

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

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

2.2 STUPF算法

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

步驟1 初始化，設置粒子數目N ，并對N（＾x0，P0）抽樣，得到初始粒子集｛i＝1，2，…，N｝，并設粒子的權值皆為1／N。

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

步驟3 將粒子權值進行更新，并進行歸一化處理

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

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

3 數值仿真

本節(jié)分別采用兩個非線性模型對STUPF進行仿真，其中例1是粒子濾波算法的標準檢驗模型，來驗證STUPF算法對狀態(tài)的估計能力，例2 是分時恒定值問題，來驗證STUPF對狀態(tài)突變的強跟蹤能力。

例1：標準模型的狀態(tài)方程和量測方程為

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

圖1 標準模型的狀態(tài)估計

圖2 標準模型的均方誤差

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

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

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

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

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

4 結束語

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

［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，Femandez－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 無味粒子濾波的目標跟蹤 算 法 ［J］.系 統(tǒng) 工 程 與 電 子 技 術，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）.［胡昌華，張琪，喬玉坤.強跟蹤粒子濾波算法及其在故障預報中的應用 ［J］.自動化學報，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變換的強跟蹤濾波器 ［J］.控制與決策，2010，25 （7）：1063－1068.］