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

崔　丹，吳　楠，孟凡坤

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

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

崔丹，吳楠，孟凡坤

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

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

噪聲統計特性；無跡卡爾曼濾波；噪聲在線估計；新息；協方差

0　引 言

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

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

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

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

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

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

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

其中：

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

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

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

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

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

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

利用式（4）可得系統噪聲協方差Q和觀測噪聲協方差R：

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

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

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

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

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

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

每個sigma點通過系統模型實現傳播：

于是，可計算狀態估計及其誤差協方差的一步預測：

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

計算觀測新息的協方差：

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

3　仿真實驗及分析

3.1仿真條件

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

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

3.2仿真結果及分析

在INS/GPS松耦合組合導航系統中，采用改進的無跡卡爾曼濾波器和傳統卡爾曼濾波器進行仿真對比，分別得到相應的位置、速度及姿態誤差曲線。圖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依次為改進濾波算法與卡爾曼濾波算法對目標位置誤差、速度誤差、姿態誤差的估計曲線。

圖2　目標的運動軌跡

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

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

圖5　改進UKF與Kalman北向、東向、天向姿態誤差比較

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

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

4　結 語

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

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

［2］ 高青偉，趙國榮，吳芳等.衰減記憶自適應濾波在慣導系統傳遞對準中的應用［J］.系統工程與電子技術，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組合導航系統算法研究［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］.哈爾濱工業大學學報，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］ 張一，馬永奎，張中兆.現代衛星導航信號的發展及特性分析［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］.西安:西北工業大學出版社，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