一種新的自適應UKF算法及其在組合導航中的應用

胡高歌，高社生，趙 巖

（西北工業大學 自動化學院，西安 710072）

一種新的自適應UKF算法及其在組合導航中的應用

胡高歌，高社生，趙 巖

（西北工業大學 自動化學院，西安 710072）

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

SINS/BDS組合導航；Kalman濾波；自適應UKF；協方差匹配

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

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

為了提高組合導航系統的濾波精度，本文提出一種新的自適應UKF濾波算法。該算法根據協方差匹配原理，利用UKF濾波算法的殘差序列與新息序列，在線估計、調整系統過程噪聲和量測噪聲的統計特性，提高UKF的自適應能力，克服了標準UKF在系統噪聲統計未知或不準確情況下濾波精度下降甚至發散的問題。將提出的算法用于SINS/BDS組合導航系統進行仿真驗證，結果表明，當系統噪聲統計不準確時，提出的自適應UKF濾波性能明顯優于標準UKF和抗差UKF，能顯著提高組合導航的濾波精度。

1 一種新的自適應UKF濾波算法

考慮非線性離散系統：

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

1.1 標準UKF

UKF濾波算法通過UT變換近似計算系統狀態向量的后驗均值與方差，能夠以較高的精度處理非線性系統的狀態估計問題。對于非線性系統(1)，標準UKF濾波算法如下：

① 初始化

② Sigma點計算與時間更新

式中，a∈R為調節參數，通常設定為一個小的正值；為矩陣nPk-1均方根的第i列；權值選取為

③ 量測更新

1.2 提出的自適應UKF

針對標準 UKF在系統噪聲統計未知或不準確情況下濾波精度下降甚至發散的問題，基于協方差匹配技術，利用 UKF濾波算法的殘差序列與新息序列，設計一種帶有系統噪聲統計估計器的自適應UKF濾波算法。

1.2.1 系統噪聲統計估計

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

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

定義：

容易得知：

由于量測誤差與狀態估計誤差具有正交性，因而新息協方差矩陣可計算如下：

由式(21)有：

利用新息向量有限的樣本來計算其協方差矩陣：

其中N為估計窗口寬度。

將(23)代入(22)，得到系統量測噪聲協方差矩陣的估計值：

由式(14)(15)可得：

對式(25)右乘其轉置，有：

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

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

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

1.2.2 自適應UKF算法的實現

對于非線性系統(1)，提出的自適應 UKF算法可以在非線性系統濾波解算的同時實現對系統噪聲統計的估計，具體步驟如下：

① 初始化

② 時間更新

預測協方差為：

③ 量測更新

量測協方差矩陣為：

④ 更新系統噪聲統計特性

2 SINS/BDS組合導航系統模型

在SINS/BDS組合導航系統的直接法濾波中，以捷聯慣導力學編排方程和姿態誤差方程作為組合導航系統的狀態方程，以北斗接收機輸出的速度和位置信息作為組合導航系統的量測。

2.1 組合導航系統狀態方程

取導航坐標系為東北天(E-N-U)地理坐標系，SINS的力學編排方程和姿態誤差方程分別為

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

SINS/BDS組合導航系統的狀態向量X(t)選取為。結合式(37)-(39)，可得SINS/BDS組合導航系統狀態方程：

式中，f(·)為非線性函數，為系統噪聲。對式(40)進行離散化處理，可得離散化的系統狀態方程

2.2 系統量測方程

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

根據之前選取的系統狀態向量X與量測Z，建立SINS/BDS組合導航系統的量測方程：

3 仿真實驗及分析

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

為了比較標準UKF、抗差UKF和提出的自適應UKF在系統噪聲統計不準確時的濾波性能，三種濾波器中選取的噪聲統計初值均與實際值存在較大偏差，具體為

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

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

圖1 標準UKF定位誤差Fig.1 Position error of standard UKF

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

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

表1 SINS/BDS組合導航系統誤差比較Tab.1 Error comparison for SINS/BDS integration

4 結 論

為了提高組合導航系統的濾波精度，針對標準UKF濾波性能依賴于系統先驗噪聲統計的缺陷，提出一種新的自適應UKF濾波算法。該算法能夠在線地估計和調整系統過程噪聲與量測噪聲的統計特性，克服了標準UKF在系統噪聲統計未知或不準確情況下濾波精度下降甚至發散的問題，提高了UKF的自適應能力。

將該算法應用于 SINS/BDS組合導航系統進行仿真驗證，結果表明，當系統噪聲統計不準確時，提出的自適應UKF濾波性能明顯優于標準UKF和抗差UKF，有效地提高的SINS/BDS組合導航系統的定位精度。

（References）：

[1]高社生，宋飛彪，姜微微.抗差自適應模型預測濾波及其在組合導航中的應用[J].中國慣性技術學報，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 組合系統直接法卡爾曼濾波[J].系統工程與電子技術，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 直接法卡爾曼濾波中的應用[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.

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

[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

國家自然科學基金（61174193）；航天科技創新基金（CASC201102）

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

聯 系 人：高社生（1956—），男，教授，博士生導師，研究方向為導航、制導與控制。E-mail：gshshnpu@163.com