組合導航中的噪聲在線估計無跡卡爾曼濾波*

崔　丹，吳　楠，孟凡坤

（信息工程大學 導航與空天目標工程學院，河南 鄭州 450001）

組合導航中的噪聲在線估計無跡卡爾曼濾波*

崔丹，吳楠，孟凡坤

（信息工程大學 導航與空天目標工程學院，河南 鄭州 450001）

為了解決大多數(shù)實際應用中噪聲統(tǒng)計特性未知且時變引起的濾波精度下降甚至發(fā)散的問題，在常規(guī)無跡卡爾曼濾波算法基礎上，提出了一種基于新息的噪聲在線估計無跡卡爾曼濾波算法 。該算法利用觀測新息的協(xié)方差，可同時在線估計系統(tǒng)噪聲協(xié)方差Q和觀測噪聲協(xié)方差R。將改 進的濾波算法應用 到INS/GPS松耦合組合導航系統(tǒng)，仿真結果表明，該算法對時變噪聲統(tǒng)計特性表現(xiàn)出了良好的適應性，濾波器更穩(wěn)定，定位精度更高。

噪聲統(tǒng)計特性；無跡卡爾曼濾波；噪聲在線估計；新息；協(xié)方差

0　引 言

為了解決強非線性條件下的估計問題，有學者提出了無跡卡爾曼濾波（Unscented Kalman Filter，UKF）算法。UKF算 法采用無跡變換（Unscented Transformation，UT）對一組精確選擇的sigma點進行非線性變換，以估計系統(tǒng)狀態(tài)的均值和協(xié)方差。由于不需 要對非線性系統(tǒng)進行線性化，且可以很容易地應用于非線性系統(tǒng)的狀態(tài)估計，因此UKF方法在許多方面得到了廣泛應用［1］。

在使用過程中，常規(guī)的UKF系統(tǒng)噪聲協(xié)方差矩陣Q、觀測噪聲協(xié)方差矩陣R都是準確已知的。但是，在大多 數(shù)實際導航系統(tǒng)中，機動載體的噪聲統(tǒng)計特性往往是未知且時變的。若假設的噪聲統(tǒng)計特性與實際的噪聲統(tǒng)計特性偏差較大，則容易導致濾波器發(fā)散。為了解決此問題，國內外研究人 員進行了一系列相關研究，提出了各種自適應卡爾曼濾波算法，如Sage-Huge自適應濾波算法、衰減記憶卡爾曼濾波算法、模糊自適應卡爾曼濾波算法［2］等。Sage-H uge［3］自適應濾波算法是由經(jīng)典Kalman濾波算法和噪聲統(tǒng)計估值器組成的次優(yōu)無偏極大后驗估計，可同時估計狀態(tài)和噪聲統(tǒng)計特性，但該算法需保證量測噪聲協(xié)方差陣的正定性和系統(tǒng)噪聲協(xié)方差陣的半正定性，否則濾波器容易發(fā)散［4］。Fagin和 Sorenson等人提出了衰減記憶濾波算法，利用衰減因子限制卡爾曼濾波器的記憶長度，增加當前量測值的權重來抑制濾波發(fā)散，但其濾波精度有所降低［5-7］。通常情況下，衰減記憶卡爾曼濾波采用一個大于1的固定常數(shù)作為衰減因子，若系統(tǒng)遇到突發(fā)干擾，單純的衰減因子并不能準確調整所有的狀態(tài)［8］。文獻［9］將經(jīng)典Kalman濾波算法與模糊邏輯方法相結合，通過設計的模糊推理系統(tǒng)在線實時調整量測噪聲方差，但該算法需要預先對機動環(huán)境的噪聲特性有一定的了解。當實際噪聲較強時，噪聲特性很難在比值1附近劃定隸屬度函數(shù)［10-11］。

針對時變噪聲統(tǒng)計特性造成的濾波發(fā)散現(xiàn)象，本文研究了一種改進的無跡卡爾曼濾波算法。通過觀測新息的統(tǒng)計特性，利用最后N個觀測新息的協(xié)方差，同時計算系統(tǒng)噪聲協(xié)方差Q和觀測噪聲協(xié)方差R，對噪聲統(tǒng)計特性進行在線估計，從而達到抑制濾波器發(fā)散的目的。在INS/GPS組合導航系統(tǒng)中，采用改進的UKF算法對飛行目標誤差狀態(tài)進行估計，并對其進行仿真。仿真結果表明，改進的無跡卡爾曼濾波器能有效抑制濾波發(fā)散，且比傳統(tǒng)的卡爾曼濾波器的濾波精度高，對實際工程應用具有一定的參考價值。

1　INS/GPS松耦合組合導航模型

慣性導航系統(tǒng)（Inertial Navigation System，INS）既能提供有效的姿態(tài)、角速率和加速度測量，又能輸出位置和速度，而且其自主性強，不易受環(huán)境、載體機動和無線電干擾，短期精度高且穩(wěn)定性強。但是，慣導測量存在誤差隨時間迅速積累的問題。導航精度隨時間而發(fā)散，不能單獨長時間工作，必須不斷加以校準。全球定位系統(tǒng)（Global Positioning Systems，GPS）可以提供長時間內誤差只有幾米的高精度位置輸出，其導航定位授時服務可達到全球性、全天候，并且精度高、誤差不隨時間積累。但是，GPS信號容易受到遮擋或干擾［12］、導航結果的數(shù)據(jù)更新率低、沒有姿態(tài)信息的輸出，因此不能依賴GPS來提供連續(xù)導航參數(shù)。

可以看出，INS和GPS的優(yōu)缺點是互補的。因此，可將二者組合，結合兩種技術的優(yōu)勢，實現(xiàn)在兩個系統(tǒng)間取長補短的目的，從而提供連續(xù)、高帶寬、長時和短時精度都較高的、完整的導航參數(shù)，提高整個導航系統(tǒng)的魯棒性。常見的組合導航結構有4種：松耦合、緊耦合、深耦合、超緊耦合。其中，松耦合組合導航是基于速度與位置的組合，其結構簡單靈活，易于實現(xiàn)。本文基于松耦合組合導航系統(tǒng)進行研究，選取誤差狀態(tài)進行估計，包含姿態(tài)、位置、速度、加速度計零偏及陀螺儀零偏共15個參數(shù)。在松耦合組合導航中，不用估計GPS狀態(tài)，因此狀態(tài)估計依賴于INS。GPS速度誤差的變化可以推出INS的姿態(tài)誤差和儀表誤差，因此使用GPS的位置和速度作為觀測參數(shù)。系統(tǒng)的觀測值包括位置量測差值和速度量測差值，其維數(shù)為6。

目標實際運動系統(tǒng)模型通常為非線性，INS/GPS組合導航系統(tǒng)濾波的狀態(tài)方程與觀測方程可寫為：

其中：

2　噪聲在線估計無跡卡爾曼濾波算法

2.1基于新息的噪聲在線估計

常規(guī)無跡卡爾曼濾波和標準卡爾曼濾波都屬于線性最小方差估計。算法都基于模型，區(qū)別在于最佳增益矩陣的求取方法不同［13］。計算最佳增益時，UKF根據(jù)觀測量和被估計量的協(xié)方差陣來確定最佳增益陣。在UKF算法中，狀態(tài)向量經(jīng)無跡變換將n維狀態(tài)向量變換到2n維的并行狀態(tài)向量，即sigma點。這些sigma點則根據(jù)系統(tǒng)方程和量測方程來計算狀態(tài)估計和誤差協(xié)方差陣。在導航系統(tǒng)中，UKF系統(tǒng)傳播對姿態(tài)不確定度大的應用場合很有用；UKF的觀測更新對發(fā)射器和接收器距離較近的應用場合比較有用。

常規(guī)的UKF算法中，噪聲統(tǒng)計特性Q和R均為預先設定的高斯白噪聲。然而，目標在實際運動過程中Q和R是實時變化的。為了解決此問題，本文利用觀測新息對系統(tǒng)噪聲協(xié)方差Q和觀測噪聲協(xié)方差R進行在線估計。

觀測新息矢量包括GPS位置、速度與校正后的INS位置、速度的差值：

計算最后N個觀測新息的協(xié)方差，N為自適應窗口，取值可變：

利用式（4）可得系統(tǒng)噪聲協(xié)方差Q和觀測噪聲協(xié)方差R：

給定Q和R的初值后，將式（5）和式（6）分別代入狀態(tài)估計協(xié)方差和觀測新息協(xié)方差求解中，便能將在線估計的噪聲統(tǒng)計特性引入濾波的估計過程中。

下面將改進的UKF算法應用到INS/GPS組合導航系統(tǒng)中。圖1給出了改進無跡卡爾曼濾波算法的計算流程框圖。

圖1　改進無跡卡爾曼濾波算法流程框

2.2基于噪聲在線估計的無跡卡爾曼濾波算法

本文采用INS/GPS松 耦合組合導航系統(tǒng)。改進的自適應UKF使用GPS輸出的位置和速度參數(shù)來估計INS誤差，然后利用估計誤差對INS導航參數(shù)進行校正。經(jīng)過校正后的INS導航參數(shù)，即為組合導航輸出。

基于新息的噪聲在線估計UKF系統(tǒng)，傳播流程的第一步是利用Cholesky求解誤差協(xié)方差矩陣的均方根Sk：

每個sigma點通過系統(tǒng)模型實現(xiàn)傳播：

于是，可計算狀態(tài)估計及其誤差協(xié)方差的一步預測：

將式（12）代入式（8）產(chǎn)生新的sigma點，由新的樣點可得到觀測新息的一步預測。新的sigma點和對應的平均觀測新息為：

計算觀測新息的協(xié)方差：

最后，基于新息的自適應UKF算法增益矩陣、狀態(tài)向量更新和誤差協(xié)方差更新為：

3　仿真實驗及分析

3.1仿真條件

為了驗證本文提出的基于新息的噪聲在線估計無跡卡爾曼濾波算法的性能以及其在INS/GPS松耦合組合導航系統(tǒng)中的應用效果，本文在MATLAB開發(fā)環(huán)境下對改進無跡卡爾曼濾波算法在INS/GPS松耦合組合導航中的應用進行仿真實驗。目標初始位置為：大地緯度50.337 4°，經(jīng)度-3.306 8°，大地高度10 000 m。初始速度為：北向速度-120 m/s，東向速度160 m/s，地向速度0 m/s。初始姿態(tài)為：傾斜角-14.036 3°，仰角0°，航向角126.871 5°。目標飛行時間為290 s，傳播間隔為0.5 s。在仿真過程中，飛行目標的機載過程包括加速、減速、下降、勻速飛行、轉向以及爬升。

在噪聲統(tǒng)計特性不確定的情況下，基于新息的自適應無跡卡爾曼濾波器初始參數(shù)設置為：

3.2仿真結果及分析

在INS/GPS松耦合組合導航系統(tǒng)中，采用改進的無跡卡爾曼濾波器和傳統(tǒng)卡爾曼濾波器進行仿真對比，分別得到相應的位置、速度及姿態(tài)誤差曲線。圖2為飛行目標的運動軌跡，初始北向速度為120 m/s，東向速度為160 m/s，50 s時目標減速到200 m/s，83 s時目標開始做爬升運動，天向速度勻加速至20 m/s，116 s時爬升運動結束，目標保持200 m/s向東勻速運動。圖3、圖4、圖5依次為改進濾波算法與卡爾曼濾波算法對目標位置誤差、速度誤差、姿態(tài)誤差的估計曲線。

圖2　目標的運動軌跡

圖3　改進UKF與Kalman北向、東向、天向位置誤差比較

圖4　改進UKF與Kalman北向、東向、天向速度誤差比較

圖5　改進UKF與Kalman北向、東向、天向姿態(tài)誤差比較

圖3、圖4、圖5中，改進UKF算法對目標的位置誤差、速度誤差、姿態(tài)誤差的狀態(tài)估計結果明顯比Kalman濾波算法的狀態(tài)估計結果穩(wěn)定。采用Kalman濾波算法的仿真曲線中，位置的北向、東向、天向誤差分 別高達22.5 m、17.8 m、11.7 m。而采用本文改進濾波算法得到的北向、東向、天向位置誤差最大值分別為2.3 m、6 m、5 m。可見，改進算法的定位精度更高。圖4中，濾波器輸出穩(wěn)定后，北向和東向速度誤差值有較明顯改善，且天向速度誤差值也有少許減小。目標運動狀態(tài)在50 s、83 s、116 s時均有較明顯變化，因此Kalman濾波器在50 s、83 s和116 s附近的速度誤差有較大波動。而本文提出的改進算法能很好地克服這種由運動軌跡突變造成的影響，速度誤差值接近于0且保持穩(wěn)態(tài)。由圖5可以看出，Kalman濾波器輸出結果中，50 s時，目標傾斜角的突變使得繞北向的姿態(tài)誤差急劇增大；83 s到116 s目標做爬升運動，導致天向姿態(tài)誤差呈發(fā)散趨勢。而本文改進濾波器輸出的姿態(tài)誤差值接近于0且沒有明顯波動，表現(xiàn)出了良好的平穩(wěn)性。此外，改進濾波算法對北向姿態(tài)誤差的改善最明顯，東向姿態(tài)誤差僅稍稍得到改善。對于天向姿態(tài)誤差，Kalman濾波器的輸出是發(fā)散的，而改進UKF濾波器的輸出是持續(xù)穩(wěn)定的。

通過對上述INS/GPS松耦合組合導航系統(tǒng)中飛行目標的北向、東向、天向位置、速度和姿態(tài)誤差的比較，可以看出在系統(tǒng)噪聲協(xié)方差Q和觀測噪聲協(xié)方差R不確定的情況下，本文提出的基于新息的自適應無跡卡爾曼濾波算法比傳統(tǒng)的卡爾曼濾波算法穩(wěn)定性更好、濾波精度更高。

4　結 語

使用常規(guī)卡爾曼濾波器對組合導航系統(tǒng)狀態(tài)進行估計時，需先給定系統(tǒng)噪聲和觀測噪聲的統(tǒng)計特性，但在實際物理過程中，無法確定噪聲統(tǒng)計特性。針對此問題，本文提出了一種利用觀測新息協(xié)方差對噪聲統(tǒng)計特性在線估計的自適應無跡卡爾曼濾波算法。此外，描述INS/GPS松耦合組合導航系統(tǒng)模型，介紹改進算法對該系統(tǒng)狀態(tài)估計的詳細過程，對改進濾波算法、卡爾曼濾波算法進行仿真對比實驗。仿真結果表明，基于新息的自適應無跡卡爾曼濾波算法克服了由噪聲統(tǒng)計特性未知引起的濾波發(fā)散問題，且與常規(guī)卡爾曼濾波算法相比，改進濾波算法表現(xiàn)出了更好的穩(wěn)定性，更高的濾波精度。

［1］ 何友，修建娟，關欣等.雷達數(shù)據(jù)處理及應用［M］.北京:電子工業(yè)出版社，2013. HE You，XIU Jian-juan，GUAN Xin，et al.Radar Data Processing with Applications［M］.Beijing:Electronic Industry Press，2013.

［2］ 高青偉，趙國榮，吳芳等.衰減記憶自適應濾波在慣導系統(tǒng)傳遞對準中的應用［J］.系統(tǒng)工程與電子技術，2010，32（12）:2648-2651. GAO Qing-wei，ZHAO Guo-rong，WU Fang，et al.Application of Adaptive Fading Filter Technique in Transfer Alignment of Inertial Navigation Systems［J］.Systems Engineering and Electro nics，2010，32（12）:2648-2651.

［3］ Sage A P，Husa G W.Adptive Filtering with Unknown Prior Statistics［C］.In Proceedings of Joint Automatic Control Conference，1969:760-769.

［4］ 屈新芬，李世玲，曾超等.漸消記憶自適應濾波在傳遞對準中的應用［J］.探測與控制學報，2014，36（05）:43-46. QU Xin-fen，LI Shi-ling，ZENG Chao，et al.Application of Adaptive Fading Memory Adaptive Filter in Transferring Alignment［J］.Journal of Detection & Control，2014，36（05）:43-46.

［5］ TIAN Chong，WANG Xing-liang，LU Yan-e.An Adaptive Filter Algorithm for Exponentially-Weighted Multiple Attenuated Memory［J］.Journal of Wuhan University of Technology，2011，33（11）:147-152.

［6］ Long Zhao，Jing Liu.An Improved Adaptive Filtering Algorithm with Applications in Integrated Navigation［C］. IEEE International Conference on Digital Manufacturing &Automation，2010:182-185.

［7］ 袁美桂，嚴玉國，龐春雷，等.改進的自適應Kalman濾波在GPS/SINS中的應用［J］.空軍工程大學學報:自然科學版，2015，16（05）:65-69. YUAN Mei-gui，YAN Yu-guo，PANG Chun-lei，et al.The Application of Improved Adaptive Kalman Filter to GPS/ SINS［J］.Journal of Air Force Engineering University（Natural Science Edition），2015，16（05）:65-69.

［8］ 李楊，胡柏青.基于改進衰減記憶濾波的組合導航算法研究［J］.計算機仿真，2012，29（03）:117-157. LI Yang，HU Bai-qing.Research on Integrated Navigation Algorithm based on Improved Fading-memory Filter［J］. Computer Simulation，2012，29（03）:117-157.

［9］ 徐田來，游文虎，崔平遠.基于模糊自適應濾波的INS/GPS組合導航系統(tǒng)算法研究［J］.宇航學報，2005，26（05）:571-575. XU Tian-lai，YOU Wen-hu，CUI Ping-yuan.Research on GPS/INS Integrated Navigation System based on Fuzzy Adaptive Kalman Filtering［J］.Journal of Astronautics，2005，26（05）:571-575.

［10］ 張迎春，李璟璟，吳麗娜等.模糊自適應無跡卡爾曼濾波方法用于天文導航［J］.哈爾濱工業(yè)大學學報，2012，44（01）:12-16. ZHANG Ying-chun，LI Jing-jing，WU Li-na，et al.Application of Fuzzy Adaptive Unscented Kalman Filter in Spacecraft Celestial Navigation［J］.Journal of Harbin Institute of Technology，2012，44（01）:12-16.

［11］ Enbo Shi，An Improved Real-time Adaptive Kalman Filter For Low-Cost Integrated GPS/INS Navigation［C］.IEEE International Conference on Measurement，Information and Control，2012:1093-1098.

［12］ 張一，馬永奎，張中兆.現(xiàn)代衛(wèi)星導航信號的發(fā)展及特性分析［J］.通信技術，2006，39（01）:28-34. ZHANG Yi，MA Yong-kui，ZHANG Zhong-zhao.Development and Analysis of Modernized Satellite Navigation Signal［J］. Communications technology，2006，39（01）:28-34.

［13］ 秦永遠，張洪鉞，汪叔華.卡爾曼濾波與組合導航原理［M］.西安:西北工業(yè)大學出版社，2015. QIN Yong-yuan，ZHANG Hong-yue，WANG Shu-hua. Theory of Kalman Filter and Integrated Navigation［M］. Xi’an:Northwestern Polytechnical University Press，2015.

崔 丹（1991—），女，碩士研究生，主要研究方向為空間目標信息處理與應用；

吳 楠（1984—），男，博士，講師，主要研究方向為空間目標信息處理與應用；

孟凡坤（1963—），男，碩士，教授，主要研究方向為空間目標信息處理與應用。

Unscented Kalman Filter based on Online Noise Estimation in Integrated Navigation

CUI Dan， WU Nan， MENG Fan-kun
（Institute of Navigation and Aerospace Target Engineering， Information Engineering University， Zhengzhou Henan 450001， China）

In order to solve the problem of filtering-accuracy decline or even divergent caused by the unknown statistics and time-correlated variation of noise in the most practical applications， an innovationbased adaptive Unscented Kalman Filter algorithm is proposed. This algorithm could simultaneously estimate the system noise covariance Q and the measurement noise covariance R from the covariance of measurement innovation. For the purpose of simulation， the modified filtering algorithm is applied to the loosely-coupled INS/GPS in tegrated navigation system， and the simulation results indicate that the modified algorithm exhibits fairly good adaptability to the ti me-varied noise， and the filter is more stable， the positioning accuracy is even higher.

noise statistics； Unscented Kalman Filter； online noise estimation； innovation； covariance

TN713

A

1002-0802（2016）-10-1306-06

10.3969/j.issn.1002-0802.2016.10.008

2016-06-14；

2016-09-17

data:2016-06-14；Revised data:2016-09-17