改進的強跟蹤UKF算法及其在INS/GPS組合導航中的應用

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

（西北工業(yè)大學 自動化學院，西安 710072）

改進的強跟蹤UKF算法及其在INS/GPS組合導航中的應用

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

（西北工業(yè)大學 自動化學院，西安 710072）

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

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

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

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

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

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

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

1 標準UKF

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

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

對于非線性系統(tǒng)(1)，標準UKF濾波算法如下：① 初始化

② Sigma點計算

③ 時間更新

其中權值 ωi選取為：

④ 量測更新

2 改進的強跟蹤UKF

2.1 STF基本原理

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

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

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

記UKF的估計誤差與預測誤差分別為：

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

因而：

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

即：

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

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

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

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

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

令

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

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

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

定義統(tǒng)計量

及以下假設：

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

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

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

2.4 ISTUKF算法實現(xiàn)

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

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

② 利用式(4)進行Sigma點計算；

③ 采用式(5)(6)計算狀態(tài)一步預測；

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

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

⑥ 返回步驟②，進行下一時刻濾波解算。

3 仿真實驗及分析

將提出的ISTUKF算法應用于INS/GPS組合導航系統(tǒng)進行仿真驗證，并與標準UKF進行比較。仿真采用文獻[2]中設計的INS/GPS組合導航系統(tǒng)模型。

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

圖2 采用標準UKF時 αk變化曲線Fig.2 Behavior of αk as the standard UKF is used

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

圖4 標準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

標準UKF與提出的ISTUKF得到的東向、北向位置誤差如圖4~6所示?？梢钥吹?，在系統(tǒng)正常狀態(tài)下，標準UKF與提出的ISTUKF均可實現(xiàn)飛機的準確定位。當在500~600 s時間段引入狀態(tài)突變量后，標準UKF定位誤差急劇增大，在該時間段內(nèi)，標準 UKF得到的東向、北向位置誤差在[9.3 m, 23.5 m]內(nèi)。提出的ISTUKF由于能夠快速檢測出狀態(tài)異常并實時調(diào)整濾波增益矩陣，得到的定位精度相比標準UKF明顯提高，在 500~600 s時間段，帶單重次優(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由于對不同的狀態(tài)分量以不同的速率調(diào)節(jié)，因而得到的定位精度更高。

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

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

4 結(jié) 論

本文提出一種改進的強跟蹤UKF濾波算法以克服標準UKF在系統(tǒng)狀態(tài)異常時濾波精度降低的缺陷。該算法采用假設檢驗的方法對異常狀態(tài)進行檢測，當系統(tǒng)狀態(tài)出現(xiàn)異常時，通過對預測協(xié)方差陣引入次優(yōu)漸消因子來自適應地調(diào)整濾波增益，進而實現(xiàn)濾波算法對系統(tǒng)真實狀態(tài)的強跟蹤。該算法中次優(yōu)漸消因子的確定無需計算系統(tǒng)模型的雅克比矩陣，提高了傳統(tǒng)強跟蹤UKF的實用性。提出的強跟蹤UKF具有兩種形式：帶單重次優(yōu)漸消因子與帶多重次優(yōu)漸消因子。將提出的算法應用于INS/GPS組合導航系統(tǒng)進行仿真驗證，并與標準UKF進行比較，結(jié)果表明，在系統(tǒng)狀態(tài)存在異常時，提出的強跟蹤UKF可以有效地檢測出異常狀態(tài)并自適應調(diào)節(jié)濾波增益，濾波性能明顯優(yōu)于標準UKF，提高了INS/GPS組合導航系統(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 直接法卡爾曼濾波中的應用[J]. 傳感技術學報，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] 薛麗，高社生，胡高歌. 自適應 Sage-Husa 粒子濾波及其在組合導航中的應用[J]. 中國慣性技術學報，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)漸消因子的擴展卡爾曼濾波器[J]. 自動化學報，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] 楊文博，李少遠. 基于強跟蹤UKF的航天器自主導航間接量測濾波算法[J]. 系統(tǒng)工程與電子技術，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] 錢華明，黃蔚，孫龍，等. 基于多重次漸消因子的強跟蹤 UKF姿態(tài)估計[J]. 系統(tǒng)工程與電子技術，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—），男，教授，博士生導師，研究方向為導航、制導與控制。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

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

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