胡高歌，高社生，趙 巖

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

一種新的自適應(yīng)UKF算法及其在組合導(dǎo)航中的應(yīng)用

胡高歌，高社生，趙 巖

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

為了提高組合導(dǎo)航系統(tǒng)的濾波精度，提出一種帶噪聲統(tǒng)計(jì)估計(jì)器的自適應(yīng)UKF濾波算法。該算法根據(jù)協(xié)方差匹配原理，利用UKF濾波算法的殘差序列與新息序列，在線估計(jì)、調(diào)整系統(tǒng)過程噪聲和量測噪聲的統(tǒng)計(jì)特性，提高UKF的自適應(yīng)能力，克服了標(biāo)準(zhǔn)UKF在系統(tǒng)噪聲統(tǒng)計(jì)未知或不準(zhǔn)確情況下濾波精度下降甚至發(fā)散的問題。將提出的算法應(yīng)用于SINS/BDS組合導(dǎo)航系統(tǒng)進(jìn)行仿真驗(yàn)證，并與標(biāo)準(zhǔn)UKF和抗差UKF進(jìn)行比較，結(jié)果表明，提出的自適應(yīng)UKF得到的水平位置誤差和天向誤差分別在以內(nèi)，濾波性能明顯優(yōu)于標(biāo)準(zhǔn)UKF與抗差UKF，提高了組合導(dǎo)航系統(tǒng)的解算精度。

SINS/BDS組合導(dǎo)航；Kalman濾波；自適應(yīng)UKF；協(xié)方差匹配

在組合系統(tǒng)導(dǎo)航定位中，高精度的濾波算法對(duì)導(dǎo)航定位的解算精度有重要影響[1-2]?？柭鼮V波是常用的導(dǎo)航濾波算法，它要求系統(tǒng)的狀態(tài)方程與量測方程均為線性，而在捷聯(lián)慣導(dǎo)(SINS)/北斗(BDS)組合導(dǎo)航系統(tǒng)的直接法濾波中，由于系統(tǒng)狀態(tài)方程具有非線性形式，因此傳統(tǒng)的卡爾曼濾波算法已不再適用[3-5]。為了解決非線性系統(tǒng)的濾波解算問題，人們提出了擴(kuò)展卡爾曼濾波(Extended Kalman filter，EKF)，其基本思想是將非線性系統(tǒng)模型進(jìn)行Taylor展開，略去二階以上的高階項(xiàng)，得到非線性系統(tǒng)的線性化模型，然后對(duì)線性化后的系統(tǒng)模型采用卡爾曼濾波算法進(jìn)行計(jì)算，因此EKF是一種次優(yōu)濾波。EKF雖然在非線性濾波中得到了廣泛應(yīng)用，但該算法仍然存在一些局限性。當(dāng)動(dòng)力學(xué)系統(tǒng)的非線性特性較強(qiáng)時(shí)，略去Taylor展開式的高階項(xiàng)，將會(huì)引起較大的線性化誤差，從而導(dǎo)致EKF的濾波誤差增大甚至發(fā)散。此外，在EKF的濾波計(jì)算中，雅可比矩陣的求取復(fù)雜、計(jì)算量大，在實(shí)際應(yīng)用中很難實(shí)施。

為了克服EKF的局限性，一些學(xué)者提出了Unscented卡爾曼濾波(Unscented Kalman filter，UKF)算法。UKF的核心思想是采用 Unscented變換(Unscented Transformation，UT)對(duì)非線性系統(tǒng)狀態(tài)的后驗(yàn)概率密度函數(shù)進(jìn)行近似[6]。與EKF相比，UKF以二階以上精度近似高斯非線性系統(tǒng)狀態(tài)的后驗(yàn)均值與方差，具有實(shí)現(xiàn)簡單、濾波精度高以及收斂性好等優(yōu)點(diǎn)。然而，UKF要求精確已知系統(tǒng)噪聲的先驗(yàn)統(tǒng)計(jì)信息，未知或不準(zhǔn)確的系統(tǒng)噪聲統(tǒng)計(jì)會(huì)導(dǎo)致濾波精度下降甚至發(fā)散[7-8]。文獻(xiàn)[9]在UKF的基礎(chǔ)上提出了SPRHKF (Sigma point based receding horizon Kalman filter)，該算法對(duì)模型不確定性及瞬時(shí)干擾誤差具有魯棒性。然而，與 UKF比較SPRHKF收斂性較差。文獻(xiàn)[10]通過最小化UKF殘差協(xié)方差之和的方法，來求取噪聲的統(tǒng)計(jì)特性，但是該算法計(jì)算量大，實(shí)時(shí)性較差。文獻(xiàn)[6]通過引入比例因子調(diào)節(jié)濾波增益的方法，提出一種抗差UKF，該算法可以抑制量測異常對(duì)濾波精度的影響，然而該算法中比例因子的選取具有一定的經(jīng)驗(yàn)性，不能從根本上克服UKF的上述缺陷。

為了提高組合導(dǎo)航系統(tǒng)的濾波精度，本文提出一種新的自適應(yīng)UKF濾波算法。該算法根據(jù)協(xié)方差匹配原理，利用UKF濾波算法的殘差序列與新息序列，在線估計(jì)、調(diào)整系統(tǒng)過程噪聲和量測噪聲的統(tǒng)計(jì)特性，提高UKF的自適應(yīng)能力，克服了標(biāo)準(zhǔn)UKF在系統(tǒng)噪聲統(tǒng)計(jì)未知或不準(zhǔn)確情況下濾波精度下降甚至發(fā)散的問題。將提出的算法用于SINS/BDS組合導(dǎo)航系統(tǒng)進(jìn)行仿真驗(yàn)證，結(jié)果表明，當(dāng)系統(tǒng)噪聲統(tǒng)計(jì)不準(zhǔn)確時(shí)，提出的自適應(yīng)UKF濾波性能明顯優(yōu)于標(biāo)準(zhǔn)UKF和抗差UKF，能顯著提高組合導(dǎo)航的濾波精度。

1 一種新的自適應(yīng)UKF濾波算法

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

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

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

UKF濾波算法通過UT變換近似計(jì)算系統(tǒng)狀態(tài)向量的后驗(yàn)均值與方差，能夠以較高的精度處理非線性系統(tǒng)的狀態(tài)估計(jì)問題。對(duì)于非線性系統(tǒng)(1)，標(biāo)準(zhǔn)UKF濾波算法如下：

① 初始化

② Sigma點(diǎn)計(jì)算與時(shí)間更新

式中，a∈R為調(diào)節(jié)參數(shù)，通常設(shè)定為一個(gè)小的正值；為矩陣nPk-1均方根的第i列；權(quán)值選取為

③ 量測更新

1.2 提出的自適應(yīng)UKF

針對(duì)標(biāo)準(zhǔn) UKF在系統(tǒng)噪聲統(tǒng)計(jì)未知或不準(zhǔn)確情況下濾波精度下降甚至發(fā)散的問題，基于協(xié)方差匹配技術(shù)，利用 UKF濾波算法的殘差序列與新息序列，設(shè)計(jì)一種帶有系統(tǒng)噪聲統(tǒng)計(jì)估計(jì)器的自適應(yīng)UKF濾波算法。

1.2.1 系統(tǒng)噪聲統(tǒng)計(jì)估計(jì)

定義新息與殘差序列分別為：

將式(1)中的量測方程代入(14)得：

定義：

容易得知：

由于量測誤差與狀態(tài)估計(jì)誤差具有正交性，因而新息協(xié)方差矩陣可計(jì)算如下：

由式(21)有：

利用新息向量有限的樣本來計(jì)算其協(xié)方差矩陣：

其中N為估計(jì)窗口寬度。

將(23)代入(22)，得到系統(tǒng)量測噪聲協(xié)方差矩陣的估計(jì)值：

由式(14)(15)可得：

對(duì)式(25)右乘其轉(zhuǎn)置，有：

由新息序列與殘差序列的不相關(guān)性，對(duì)(26)求期望可以得到：

與(23)類似，殘差向量的協(xié)方差可近似為：

將式(7)(23)和(28)代入式(27)，可以得到如下矩陣方程：

1.2.2 自適應(yīng)UKF算法的實(shí)現(xiàn)

對(duì)于非線性系統(tǒng)(1)，提出的自適應(yīng) UKF算法可以在非線性系統(tǒng)濾波解算的同時(shí)實(shí)現(xiàn)對(duì)系統(tǒng)噪聲統(tǒng)計(jì)的估計(jì)，具體步驟如下：

① 初始化

② 時(shí)間更新

預(yù)測協(xié)方差為：

③ 量測更新

量測協(xié)方差矩陣為：

④ 更新系統(tǒng)噪聲統(tǒng)計(jì)特性

2 SINS/BDS組合導(dǎo)航系統(tǒng)模型

在SINS/BDS組合導(dǎo)航系統(tǒng)的直接法濾波中，以捷聯(lián)慣導(dǎo)力學(xué)編排方程和姿態(tài)誤差方程作為組合導(dǎo)航系統(tǒng)的狀態(tài)方程，以北斗接收機(jī)輸出的速度和位置信息作為組合導(dǎo)航系統(tǒng)的量測。

2.1 組合導(dǎo)航系統(tǒng)狀態(tài)方程

取導(dǎo)航坐標(biāo)系為東北天(E-N-U)地理坐標(biāo)系，SINS的力學(xué)編排方程和姿態(tài)誤差方程分別為

式中，fE、fN、fU為東北天向的比力量測值；δVE、δVN、δL、δh在實(shí)際計(jì)算中可由SINS輸出的速度、位置減去BDS對(duì)應(yīng)的輸出來近似獲得；陀螺常值漂移εbE、εbN、εbU通常用隨機(jī)常數(shù)來描述，即：

SINS/BDS組合導(dǎo)航系統(tǒng)的狀態(tài)向量X(t)選取為。結(jié)合式(37)-(39)，可得SINS/BDS組合導(dǎo)航系統(tǒng)狀態(tài)方程：

式中，f(·)為非線性函數(shù)，為系統(tǒng)噪聲。對(duì)式(40)進(jìn)行離散化處理，可得離散化的系統(tǒng)狀態(tài)方程

2.2 系統(tǒng)量測方程

取BDS輸出的載體速度信息vEBD、vNBD、vUBD和位置信息LBD、λBD、hBD作為量測Z，即

根據(jù)之前選取的系統(tǒng)狀態(tài)向量X與量測Z，建立SINS/BDS組合導(dǎo)航系統(tǒng)的量測方程：

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

將提出的自適應(yīng)UKF算法應(yīng)用于SINS/BDS組合導(dǎo)航系統(tǒng)中，并與標(biāo)準(zhǔn)UKF和抗差UKF進(jìn)行仿真比較。設(shè)運(yùn)載器的初始位置為東經(jīng)109°，北緯34°，高度1200 m；初始速度300 m/s，方向正北。陀螺常值漂移為0.01(°)/h，隨機(jī)游走；加速度機(jī)常值誤差10-4g，隨機(jī)游走。BDS的水平位置誤差均方根為5m，高度誤差均方根8 m，速度誤差均方根0.05 m/s。SINS初始位置誤差為10 m，初始速度誤差0.2 m/s，初始姿態(tài)誤差1′。SINS采樣周期為0.01 s，BDS采樣周期1 s，仿真時(shí)間為3600 s。

為了比較標(biāo)準(zhǔn)UKF、抗差UKF和提出的自適應(yīng)UKF在系統(tǒng)噪聲統(tǒng)計(jì)不準(zhǔn)確時(shí)的濾波性能，三種濾波器中選取的噪聲統(tǒng)計(jì)初值均與實(shí)際值存在較大偏差，具體為

標(biāo)準(zhǔn)UKF、抗差UKF和提出的自適應(yīng)UKF計(jì)算得到的定位誤差如圖1～3所示。由仿真結(jié)果可以看到，在200～3600 s時(shí)間段內(nèi)，采用標(biāo)準(zhǔn)UKF計(jì)算得到的水平位置誤差在[-14.3 m，+13.7 m]內(nèi)，天向誤差在[-15.5 m，+16.2 m]內(nèi)?？共頤KF能夠在一定程度上抑制異常量測對(duì)濾波精度的影響，得到的定位精度相比標(biāo)準(zhǔn)UKF有所提高，分別為：水平位置誤差[-9.8 m，+10.6 m]，天向誤差[-12.7 m，+14.0 m]。提出的自適應(yīng)UKF可以自適應(yīng)地更新系統(tǒng)過程噪聲與量測噪聲的統(tǒng)計(jì)特性，因而得到的定位精度最高，其水平位置誤差在[-6.2 m，+6.4 m]內(nèi)，天向誤差在[-9.8 m，+8.6 m]內(nèi)。

表1給出了三種濾波算法200～3600 s時(shí)間段內(nèi)定位誤差的平均誤差(mean error)與均方根誤差(root mean square error，RMSE)。由表1可以看出，提出的自適應(yīng) UKF的平均誤差與均方根誤差均明顯小于標(biāo)準(zhǔn)UKF與抗差UKF，表明當(dāng)系統(tǒng)噪聲統(tǒng)計(jì)不準(zhǔn)確時(shí)，提出的自適應(yīng) UKF濾波算法可以有效的提高濾波精度，減小SINS/BDS組合導(dǎo)航直接法濾波的定位誤差。

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

圖2 抗差UKF定位誤差Fig.2 Position error of robust UKF

圖3 提出的自適應(yīng)UKF定位誤差Fig.3 Position error of the proposed adaptive UKF

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

4 結(jié) 論

為了提高組合導(dǎo)航系統(tǒng)的濾波精度，針對(duì)標(biāo)準(zhǔn)UKF濾波性能依賴于系統(tǒng)先驗(yàn)噪聲統(tǒng)計(jì)的缺陷，提出一種新的自適應(yīng)UKF濾波算法。該算法能夠在線地估計(jì)和調(diào)整系統(tǒng)過程噪聲與量測噪聲的統(tǒng)計(jì)特性，克服了標(biāo)準(zhǔn)UKF在系統(tǒng)噪聲統(tǒng)計(jì)未知或不準(zhǔn)確情況下濾波精度下降甚至發(fā)散的問題，提高了UKF的自適應(yīng)能力。

將該算法應(yīng)用于 SINS/BDS組合導(dǎo)航系統(tǒng)進(jìn)行仿真驗(yàn)證，結(jié)果表明，當(dāng)系統(tǒng)噪聲統(tǒng)計(jì)不準(zhǔn)確時(shí)，提出的自適應(yīng)UKF濾波性能明顯優(yōu)于標(biāo)準(zhǔn)UKF和抗差UKF，有效地提高的SINS/BDS組合導(dǎo)航系統(tǒng)的定位精度。

（References）：

[1]高社生，宋飛彪，姜微微.抗差自適應(yīng)模型預(yù)測濾波及其在組合導(dǎo)航中的應(yīng)用[J].中國慣性技術(shù)學(xué)報(bào)，2011，19(6)：701-705.Gao Shesheng，Song Feibiao，Jiang Weiwei.Robust adaptive model predictive filtering algorithm and application to integrated navigation[J].Journal of Chinese Inertial Technology，2011，19(6):701-705.

[2]Gao S S，Zhong Y M，Li W.Robust adaptive filtering method for SINS/SAR integrated navigation system[J].Aerospace Science and Technology，2011，15(6):425-430.

[3] 汪秋婷，胡修林.基于 UKF 的新型北斗/SINS 組合系統(tǒng)直接法卡爾曼濾波[J].系統(tǒng)工程與電子技術(shù)，2010，32(2)：376-379.WANG Qiu-ting,HU Xiu-lin.Improved Kalman filtering algorithm for passive 2BD/ SINS integrated navigation system based on UKF[J].Systems Engineering and Electronics,2010,32(2):376-379.

[4]楊波，秦永元，柴艷.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.

[5]Wang X L.Fast alignment and calibration algorithms for inertial navigation system[J].Aerospace Science and Technology,2009,13(4-5):204-209.

[6]Soken H E,Hajiyev C.Pico satellite attitude estimation via robust unscented Kalman filter in the presence of measurement faults[J].ISA Transactions,2010,49(3):249-256.

[7]Hu G G,Gao S S,Xue L.A novel adaptive unscented Kalman filter[C]// 2012 Third International Conference on Intelligent Control and Information Processing.IEEE,2012:497-502.

[8]薛麗，高社生，胡高歌.自適應(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.

[9]Cho S Y,Choi W S,Robust positioning technique in low-cost DR/GPS for land navigation[J].IEEE Transactions on Instrumentation and Measurement,2006,55(4):1132-1142.

[10]Song Q,Han J D.An adaptive UKF algorithm for the state and parameter estimation of a mobile robot[J].Acta Automatica Sinica,2008,34(1):72-79.

Novel adaptive UKF and its application in integrated navigation

HU Gao-ge，GAO She-sheng，ZHAO Yan
(School of Automatics，Northwestern Polytechnical University，Xi’an 710072，China)

This paper presents a novel adaptive UKF with noise statistic estimator for the purpose of improving the filtering accuracy of integrated navigation systems.The covariance matching technique is employed in the proposed algorithm，and the innovation and residual sequences are used to estimate and adjust the covariance matrices of the process and measurement noises online.The proposed algorithm enhances the adaptive capability of the UKF and overcomes the limitation of the standard UKF，otherwise the filtering solution will be deteriorated or even divergent as the system noise statistics are unknown or inaccurate.The proposed algorithm is applied to the SINS/BDS integrated system for simulation in comparison with the standard UKF and robust UKF.The simulation results demonstrate that the horizontal position error and vertical error obtained by the proposed adaptive UKF are within [-6.2 m，+6.4 m]and[-9.8 m，+8.6 m]，respectively.The performance of the proposed algorithm is significantly superior to that of the standard UKF and robust UKF，leading to improved calculation precision of the integrated navigation system.

SINS/BDS integrated navigation;Kalman filtering;adaptive UKF;covariance matching

U666.1

A

1005-6734(2014)03-0357-05

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

2013-12-18；

2014-04-08

國家自然科學(xué)基金（61174193）；航天科技創(chuàng)新基金（CASC201102）

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

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