胡高歌，劉逸涵，高社生，楊 一

（西北工業(yè)大學(xué) 自動(dòng)化學(xué)院，西安 710072）

改進(jìn)的強(qiáng)跟蹤UKF算法及其在INS/GPS組合導(dǎo)航中的應(yīng)用

胡高歌，劉逸涵，高社生，楊 一

（西北工業(yè)大學(xué) 自動(dòng)化學(xué)院，西安 710072）

針對(duì)標(biāo)準(zhǔn) UKF缺乏對(duì)系統(tǒng)狀態(tài)異常的自適應(yīng)調(diào)整能力，導(dǎo)致濾波精度降低的問題，提出一種改進(jìn)的強(qiáng)跟蹤UKF算法。該算法采用假設(shè)檢驗(yàn)的方法對(duì)異常狀態(tài)進(jìn)行檢測(cè)，當(dāng)系統(tǒng)狀態(tài)發(fā)生異常時(shí)，對(duì)預(yù)測(cè)協(xié)方差陣引入次優(yōu)漸消因子自適應(yīng)的調(diào)整濾波增益，實(shí)現(xiàn)對(duì)系統(tǒng)真實(shí)狀態(tài)的強(qiáng)跟蹤。該算法中次優(yōu)漸消因子的確定無需計(jì)算系統(tǒng)模型的雅克比矩陣，提高了傳統(tǒng)強(qiáng)跟蹤 UKF的實(shí)用性。將提出的算法應(yīng)用于INS/GPS組合導(dǎo)航系統(tǒng)進(jìn)行仿真驗(yàn)證，并與標(biāo)準(zhǔn)UKF進(jìn)行比較，結(jié)果表明，在系統(tǒng)狀態(tài)存在異常時(shí)，提出的帶單重次優(yōu)漸消因子的強(qiáng)跟蹤UKF得到的東向、北向位置誤差在[-13.7 m, 14.9 m]以內(nèi)，帶多重次優(yōu)漸消因子的強(qiáng)跟蹤UKF得到的東向、北向位置誤差在[-10.0 m, 12.1 m]以內(nèi)，濾波性能明顯優(yōu)于標(biāo)準(zhǔn)UKF，提高了組合導(dǎo)航系統(tǒng)的解算精度。

強(qiáng)跟蹤濾波；無跡卡爾曼濾波；次優(yōu)漸消因子；INS/GPS組合導(dǎo)航

慣性導(dǎo)航系統(tǒng)(INS)與全球定位系統(tǒng)(GPS)在誤差傳播性能上具有互補(bǔ)特征，前者長(zhǎng)期穩(wěn)定性差、短期穩(wěn)定性好，而后者剛好相反，因此采用組合導(dǎo)航技術(shù)將二者有機(jī)結(jié)合起來，構(gòu)成INS/GPS組合導(dǎo)航系統(tǒng)，可以提高組合導(dǎo)航系統(tǒng)的整體性能[1-2]。

當(dāng)前，擴(kuò)展卡爾曼濾波(Extended Kalman filter, EKF)被廣泛應(yīng)用于組合導(dǎo)航系統(tǒng)設(shè)計(jì)[3]。EKF的基本思想是圍繞狀態(tài)估值將非線性系統(tǒng)模型進(jìn)行一階Taylor展開，然后采用卡爾曼濾波算法進(jìn)行計(jì)算。然而，EKF存在理論上的局限性：系統(tǒng)模型的線性化近似對(duì)系統(tǒng)狀態(tài)的后驗(yàn)均值與方差引入較大的截?cái)嗾`差，當(dāng)系統(tǒng)模型非線性較強(qiáng)時(shí)，濾波精度將嚴(yán)重降低甚至發(fā)散；Jacobian矩陣的求取復(fù)雜、計(jì)算量大，個(gè)別情形下Jacobian矩陣可能不存在；模型失配、存在未知干擾或狀態(tài)突變等情況下，EKF魯棒性差。

為了克服EKF的缺陷，學(xué)者們提出了Unscented卡爾曼濾波(Unscented Kalman filter, UKF)算法[4]。UKF的核心思想是采用Unscented變換對(duì)非線性系統(tǒng)狀態(tài)的后驗(yàn)概率密度函數(shù)進(jìn)行近似。與 EKF相比，UKF以二階以上精度近似高斯非線性系統(tǒng)狀態(tài)的后驗(yàn)均值與方差，且無需計(jì)算Jacobian矩陣，具有實(shí)現(xiàn)簡(jiǎn)單、濾波精度高以及收斂性好等優(yōu)點(diǎn)[5-6]。然而，同EKF類似，UKF算法在模型不準(zhǔn)確或狀態(tài)發(fā)生突變的情況下魯棒性不佳、跟蹤能力差。

文獻(xiàn)[7]基于新息向量的正交性原理提出了強(qiáng)跟蹤濾波器(strong tracking filter，STF)的概念，STF與通常的濾波器相比具有以下優(yōu)良特性：較強(qiáng)的關(guān)于模型參數(shù)失配的魯棒性以及較低的關(guān)于噪聲與初值統(tǒng)計(jì)特性的敏感性；極強(qiáng)的關(guān)于突變狀態(tài)的跟蹤能力,并在濾波器達(dá)到穩(wěn)態(tài)時(shí)仍保持這種能力；適中的計(jì)算復(fù)雜性。因此，可以將STF的思想與UKF結(jié)合起來以彌補(bǔ)后者魯棒性差的不足。文獻(xiàn)[8]將STF與UKF相結(jié)合，設(shè)計(jì)了一種強(qiáng)跟蹤UKF，并用于航天器自主導(dǎo)航，提高了系統(tǒng)的可靠性。文獻(xiàn)[9]將強(qiáng)跟蹤UKF應(yīng)用于視覺跟蹤，改善了非線性視覺跟蹤的魯棒性與跟蹤精度。然而，以上強(qiáng)跟蹤UKF都是通過引入單重次優(yōu)漸消因子對(duì)預(yù)測(cè)誤差協(xié)方差陣進(jìn)行調(diào)整，對(duì)于復(fù)雜的多變量系統(tǒng)，上述算法無法保證對(duì)每個(gè)變量都具有很好的跟蹤能力。文獻(xiàn)[10]提出一種帶多重次優(yōu)漸消因子的強(qiáng)跟蹤UKF，克服了帶單重次優(yōu)漸消因子的強(qiáng)跟蹤UKF對(duì)多變量跟蹤能力差的缺陷。然而，同文獻(xiàn)[8-9]設(shè)計(jì)的兩種算法一樣，該算法中多重次優(yōu)漸消因子的求取需要計(jì)算系統(tǒng)模型的Jacobian矩陣，給實(shí)際應(yīng)用帶來諸多不便。

針對(duì)標(biāo)準(zhǔn) UKF缺乏對(duì)系統(tǒng)狀態(tài)異常的自適應(yīng)調(diào)整能力，導(dǎo)致濾波精度降低的問題，本文從強(qiáng)跟蹤濾波的基本理論著手，結(jié)合UKF算法框架，推導(dǎo)了強(qiáng)跟蹤UKF次優(yōu)漸消因子的具體形式，并在此基礎(chǔ)上提出一種改進(jìn)的強(qiáng)跟蹤UKF(improved strong tracking UKF, ISTUKF)濾波算法。與傳統(tǒng)的強(qiáng)跟蹤UKF不同，提出的ISTUKF中次優(yōu)漸消因子的確定無需計(jì)算系統(tǒng)模型的Jacobian矩陣，便于實(shí)際應(yīng)用。此外，該算法采用假設(shè)檢驗(yàn)的方法對(duì)系統(tǒng)異常狀態(tài)進(jìn)行檢測(cè)，以判斷是否引入次優(yōu)漸消因子對(duì)預(yù)測(cè)誤差協(xié)方差陣進(jìn)行調(diào)節(jié)。提出的ISTUKF有兩種形式：帶單重次優(yōu)漸消因子與帶多重次優(yōu)漸消因子。將提出的 ISTUKF應(yīng)用于INS/GPS組合導(dǎo)航系統(tǒng)進(jìn)行仿真驗(yàn)證，結(jié)果表明：當(dāng)系統(tǒng)狀態(tài)存在異常時(shí)，ISTUKF濾波性能明顯優(yōu)于標(biāo)準(zhǔn)UKF；相比于帶單重次優(yōu)漸消因子的ISTUKF，帶多重次優(yōu)漸消因子的ISTUKF對(duì)不同的狀態(tài)分量以不同的速率調(diào)節(jié)，因而精度更高。

1 標(biāo)準(zhǔn)UKF

考慮非線性離散系統(tǒng)

式中， Qk為非負(fù)定矩陣， Rk為正定矩陣， δkj為Kronecker-δ函數(shù)。

對(duì)于非線性系統(tǒng)(1)，標(biāo)準(zhǔn)UKF濾波算法如下：① 初始化

② Sigma點(diǎn)計(jì)算

③ 時(shí)間更新

其中權(quán)值 ωi選取為：

④ 量測(cè)更新

2 改進(jìn)的強(qiáng)跟蹤UKF

2.1 STF基本原理

式中，第一個(gè)等式為濾波器實(shí)現(xiàn)最優(yōu)估計(jì)的性能指標(biāo)；第二個(gè)等式稱作正交性原理，要求不同時(shí)刻的新息向量保持正交關(guān)系,且具有類似白噪聲的性質(zhì),表明已將新息序列中的一切有效信息提取出來。強(qiáng)跟蹤濾波器將新息序列的不相關(guān)性作為衡量濾波性能是否優(yōu)良的標(biāo)志。

2.2 ISTUKF次優(yōu)漸消因子的確定

強(qiáng)跟蹤UKF將次優(yōu)漸消因子 λk引入預(yù)測(cè)協(xié)方差陣 Pk/k-1以提高系統(tǒng)的魯棒性與自適應(yīng)能力，具體為：

記UKF的估計(jì)誤差與預(yù)測(cè)誤差分別為：

由式(1)(8)(18)可得新息序列：

因而：

將式(9)-(11)代入式(22)有：

即：

將修改后的預(yù)測(cè)協(xié)方差陣(15)代入式(24)可以得到：

式(25)中 V0為濾波器實(shí)際輸出的新息協(xié)方差陣，可根據(jù)下式計(jì)算：

2.2.1 單重次優(yōu)漸消因子

若次優(yōu)漸消因子 λk為標(biāo)量，對(duì)(25)求跡可得：

2.2.2 多重次優(yōu)漸消因子

令

將(29)代入(25)并求解該矩陣方程，得其解：

2.3 系統(tǒng)狀態(tài)異常檢測(cè)

ISTUKF中，修改后的預(yù)測(cè)協(xié)方差陣(15)僅在系統(tǒng)狀態(tài)出現(xiàn)異常時(shí)采用，其他情形下仍使用式(7)進(jìn)行計(jì)算。本節(jié)采用假設(shè)檢驗(yàn)的方法，利用新息向量的統(tǒng)計(jì)信息，對(duì)系統(tǒng)異常進(jìn)行檢測(cè)。

定義統(tǒng)計(jì)量

及以下假設(shè)：

零假設(shè) γ0：系統(tǒng)正常工作；

備選假設(shè) γ1：系統(tǒng)存在異常。

統(tǒng)計(jì)量 αk服從自由度為m的 χ2分布。對(duì)于取定的顯著性水平α(0 ＜ α≤1)，由

2.4 ISTUKF算法實(shí)現(xiàn)

對(duì)于非線性系統(tǒng)(1)，提出的ISTUKF算法主要步驟如下：

① 采用式(3)對(duì)濾波算法初始化；

② 利用式(4)進(jìn)行Sigma點(diǎn)計(jì)算；

③ 采用式(5)(6)計(jì)算狀態(tài)一步預(yù)測(cè)；

④ 運(yùn)用2.3節(jié)提出的方法對(duì)系統(tǒng)狀態(tài)進(jìn)行檢測(cè)，若存在異常，采用式(15)計(jì)算預(yù)測(cè)協(xié)防矩陣，否則采用式(7)進(jìn)行計(jì)算；

⑤ 采用式(8)-(13)完成時(shí)間更新的其余計(jì)算與量測(cè)更新；

⑥ 返回步驟②，進(jìn)行下一時(shí)刻濾波解算。

3 仿真實(shí)驗(yàn)及分析

將提出的ISTUKF算法應(yīng)用于INS/GPS組合導(dǎo)航系統(tǒng)進(jìn)行仿真驗(yàn)證，并與標(biāo)準(zhǔn)UKF進(jìn)行比較。仿真采用文獻(xiàn)[2]中設(shè)計(jì)的INS/GPS組合導(dǎo)航系統(tǒng)模型。

圖1 飛機(jī)理想飛行軌跡Fig.1 Ideal flight trajectory of the aircraft

圖2 采用標(biāo)準(zhǔn)UKF時(shí) αk變化曲線Fig.2 Behavior of αk as the standard UKF is used

圖3 采用單重次優(yōu)漸消因子的ISTUKF時(shí) αk變化曲線Fig.3 Behavior of αk as the ISTUKF with single suboptimal fading factor is used

圖4 標(biāo)準(zhǔn)UKF定位誤差Fig.4 Position error of standard UKF

圖5 帶單重次優(yōu)漸消因子的ISTUKF定位誤差Fig.5 Position error of ISTUKF with single suboptimal fading factor

圖6 帶多重次優(yōu)漸消因子的ISTUKF定位誤差Fig.6 Position error of ISTUKF with multiple suboptimal fading factor

標(biāo)準(zhǔn)UKF與提出的ISTUKF得到的東向、北向位置誤差如圖4~6所示。可以看到，在系統(tǒng)正常狀態(tài)下，標(biāo)準(zhǔn)UKF與提出的ISTUKF均可實(shí)現(xiàn)飛機(jī)的準(zhǔn)確定位。當(dāng)在500~600 s時(shí)間段引入狀態(tài)突變量后，標(biāo)準(zhǔn)UKF定位誤差急劇增大，在該時(shí)間段內(nèi)，標(biāo)準(zhǔn) UKF得到的東向、北向位置誤差在[9.3 m, 23.5 m]內(nèi)。提出的ISTUKF由于能夠快速檢測(cè)出狀態(tài)異常并實(shí)時(shí)調(diào)整濾波增益矩陣，得到的定位精度相比標(biāo)準(zhǔn)UKF明顯提高，在 500~600 s時(shí)間段，帶單重次優(yōu)漸消因子的ISTUKF得到的東向、北向位置誤差在[-13.7 m, 14.9 m]內(nèi)；帶多重次優(yōu)漸消因子的ISTUKF得到的東向、北向位置誤差在[-10.0 m, 12.1 m]內(nèi)。與帶單重次優(yōu)漸消因子的ISTUKF相比，帶多重次優(yōu)漸消因子的ISTUKF由于對(duì)不同的狀態(tài)分量以不同的速率調(diào)節(jié)，因而得到的定位精度更高。

表1給出了標(biāo)準(zhǔn)UKF與ISTUKF在500~600 s時(shí)間段內(nèi)東向、北向位置誤差的平均誤差(mean error)與均方根誤差(root mean square error, RMSE)。由表1可以看出，提出的ISTUKF的平均誤差與均方根誤差均明顯小于標(biāo)準(zhǔn)UKF，表明當(dāng)系統(tǒng)狀態(tài)存在異常時(shí)，提出的ISTUKF濾波算法可以有效地提高濾波精度，減小INS/GPS組合導(dǎo)航系統(tǒng)的定位誤差。

表1 INS/GPS組合導(dǎo)航系統(tǒng)誤差比較Tab.1 Error comparison for the INS/GPS integration

4 結(jié) 論

本文提出一種改進(jìn)的強(qiáng)跟蹤UKF濾波算法以克服標(biāo)準(zhǔn)UKF在系統(tǒng)狀態(tài)異常時(shí)濾波精度降低的缺陷。該算法采用假設(shè)檢驗(yàn)的方法對(duì)異常狀態(tài)進(jìn)行檢測(cè)，當(dāng)系統(tǒng)狀態(tài)出現(xiàn)異常時(shí)，通過對(duì)預(yù)測(cè)協(xié)方差陣引入次優(yōu)漸消因子來自適應(yīng)地調(diào)整濾波增益，進(jìn)而實(shí)現(xiàn)濾波算法對(duì)系統(tǒng)真實(shí)狀態(tài)的強(qiáng)跟蹤。該算法中次優(yōu)漸消因子的確定無需計(jì)算系統(tǒng)模型的雅克比矩陣，提高了傳統(tǒng)強(qiáng)跟蹤UKF的實(shí)用性。提出的強(qiáng)跟蹤UKF具有兩種形式：帶單重次優(yōu)漸消因子與帶多重次優(yōu)漸消因子。將提出的算法應(yīng)用于INS/GPS組合導(dǎo)航系統(tǒng)進(jìn)行仿真驗(yàn)證，并與標(biāo)準(zhǔn)UKF進(jìn)行比較，結(jié)果表明，在系統(tǒng)狀態(tài)存在異常時(shí)，提出的強(qiáng)跟蹤UKF可以有效地檢測(cè)出異常狀態(tài)并自適應(yīng)調(diào)節(jié)濾波增益，濾波性能明顯優(yōu)于標(biāo)準(zhǔn)UKF，提高了INS/GPS組合導(dǎo)航系統(tǒng)的定位精度。

（

）：

[1] Groves P D. Principles of GNSS, inertial, and multisensor integrated navigation systems[M]. London, Boston: Artech House, 2008.

[2] 楊波，秦永元，柴艷. UKF 在 INS/GPS 直接法卡爾曼濾波中的應(yīng)用[J]. 傳感技術(shù)學(xué)報(bào)，2007，20(4)：842-846.

YANG Bo, QIN Yong-yuan, CHAI Yan. Application of UKF in direct method of Kalman filter for INS/GPS[J]. Chinese Journal of Sensors and Actuators, 2007, 20(4): 842-846.

[3] Ali J, Mirza M R U B. Performance comparison among some nonlinear filters for a low cost SINS/GPS integrated solution[J]. Nonlinear Dynamics, 2010, 61(3): 491-502.

[4] Xiong K, Zhang H Y, Chan C W. Performance evaluation of UKF-based nonlinear filtering[J]. Automatica, 2006, 42(2): 261-270.

[5] 薛麗，高社生，胡高歌. 自適應(yīng) Sage-Husa 粒子濾波及其在組合導(dǎo)航中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報(bào)，2013，21(1)：84-88.

XUE Li, GAO She-sheng, HU Gao-ge. Adaptive Sage-Husa particle filtering and its application in integrated navigation[J]. Journal of Chinese Inertial Technology, 2013, 21(1): 84-88.

[6] Gao S S, Hu G G, Zhong Y M. Windowing and random weighting-based adaptive unscented Kalman filter[J]. International Journal of Adaptive Control and Signal Processing, 2014 (in press).

[7] 周東華，席裕庚，張鐘俊. 一種帶多重次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波器[J]. 自動(dòng)化學(xué)報(bào)，1991，17(6)： 689-695.

ZHOU Dong-hua, XI Yu-geng, ZHANG Zhong-jun. A suboptimal multiple fading extended Kalman filter[J]. Acta Automatica Sinica, 1991, 17(6): 689-695.

[8] 楊文博，李少遠(yuǎn). 基于強(qiáng)跟蹤UKF的航天器自主導(dǎo)航間接量測(cè)濾波算法[J]. 系統(tǒng)工程與電子技術(shù)，2011，33(11)：2485-2491.

YANG Wen-bo, LI Shao-yuan. Autonomous navigation filtering algorithm for spacecraft based on strong tracking UKF[J]. Systems Engineering and Electronics, 2011, 33(11): 2485-2491.

[9] Zhang Z T, Zhang J S. A strong tracking nonlinear robust filter for eye tracking[J]. Journal of Control Theory and Applications, 2010, 8(4): 503-508.

[10] 錢華明，黃蔚，孫龍，等. 基于多重次漸消因子的強(qiáng)跟蹤 UKF姿態(tài)估計(jì)[J]. 系統(tǒng)工程與電子技術(shù)，2013，35(3)：580-585.

QIAN Hua-ming, HUANG Wei, SUN Long, et al. Attitude estimation of strong tracking UKF based on multiple fading factors[J]. Systems Engineering and Electronics, 2013, 35(3): 580-585.

Improved strong tracking UKF and its application in INS/GPS integrated navigation

HU Gao-ge, LIU Yi-han, GAO She-sheng, YANG Yi
(School of Automatics, Northwestern Polytechnical University, Xi’an 710072, China)

The performance of the standard UKF would be degraded when the system states are abnormal due to the absence of capability to adapt itself to the changing conditions. This paper presents an improved strong tracking UKF (ISTUKF) to overcome the limitation of the standard UKF. The hypothesis testing method is employed in the ISTUKF to identify the abnormal system states, and in case they occur, the suboptimal fading factors are introduced into the prediction covariance to adaptively adjust the Kalman gain matrix to realize the strong tracking of the real state. Compared with the traditional strong tracking UKF, the proposed ISTUKF avoids the cumbersome evaluation of Jacobian matrices involved in the calculation of the suboptimal fading factors, making it more applicable. The proposed ISTUKF is applied to the INS/GPS integrated system for simulation in comparison with the standard UKF. The simulation results demonstrate that the position errors in east and north obtained by the ISTUKF with single suboptimal fading factor are within [-13.7 m, 14.9 m], and the corresponding errors obtained by the ISTUKF with multiple suboptimal fading factors are within [-10.0 m, 12.1 m]. The performance of the proposed ISTUKF is significantly superior to that of the standard UKF, leading to improved position precision.

strong tracking filter; unscented Kalman filter; suboptimal fading factor; INS/GPS integrated navigation

聯(lián) 系 人：高社生（1956—），男，教授，博士生導(dǎo)師，研究方向?yàn)閷?dǎo)航、制導(dǎo)與控制。E-mail：gshshnpu@163.com

1005-6734(2014)05-0634-05

10.13695/j.cnki.12-1222/o3.2014.05.015

U666.1

A

2014-05-20；

2014-09-04

國家自然科學(xué)基金（基金號(hào)61174193）；西北工業(yè)大學(xué)博士論文創(chuàng)新基金（項(xiàng)目號(hào)CX201409）

胡高歌（1988—），男，博士研究生，從事組合導(dǎo)航與非線性濾波算法研究。E-mail：hugaoge1111@126.com