基于降階卡爾曼濾波算法在組合導航上的應用

胡 彬，楊新民，王勝紅

（1.南京理工大學 江蘇 南京 210094；2.淮海工業(yè)集團 山西 長治 046000）

基于降階卡爾曼濾波算法在組合導航上的應用

胡 彬1，楊新民1，王勝紅2

（1.南京理工大學 江蘇 南京 210094；2.淮海工業(yè)集團 山西 長治 046000）

本文在簡單介紹了卡爾曼濾波基本原理基礎上，基于分段線性系統(tǒng)理論以及SOM方法對系統(tǒng)狀態(tài)可觀性和可觀度的分析，采用降階卡爾曼濾波算法，合理剔除不可觀或觀測度較低的狀態(tài)因子，對高階系統(tǒng)進行降階設計。采用Visual Studio 2010語言開發(fā)環(huán)境，對常規(guī)卡爾曼濾波和降階卡爾曼濾波進行了算法仿真，通過對前后誤差波形的分析比較，結果表明降階后的濾波系統(tǒng)繼承了傳統(tǒng)卡爾曼濾波高精度的優(yōu)點，同時，算法階數的降低，顯著減少了導航計算機的計算負擔，實時性得到了顯著增強，更易于數字化實現，具有重要實際意義。

慣性導航；組合導航；卡爾曼濾波；數字仿真

工程中實性是個必須考慮的問題，尤其是在組合導航系統(tǒng)中，要求系統(tǒng)具有高精度、高實時性的機動能力。在導航計算中，應用卡爾曼濾波算法對數據處理時，由于采用數學迭代計算，計算時間由狀態(tài)維數n決定，每一步迭代的計算量都與n3正比[1]。顯然21階誤差狀態(tài)給導航計算機帶來了很大的計算負擔，工程實現困難，難以滿足組合導航高實時性的要求，在設計組合導航計算機速度和復雜度方面都不允許。

由于SINS/GPS系統(tǒng)卡爾曼濾波的初始誤差協(xié)方差很容易滿足正定性條件，而且系統(tǒng)的狀態(tài)轉移矩陣又是滿秩的，因此系統(tǒng)完全可控，濾波的有效性就取決于系統(tǒng)的可觀測行性。關于降階濾波模型的研究，前人給出了很多研究方法，例如Model方法，Lyapunow函數方法，Perturbation方法等等[2]。文獻[2]中提出了一種時變動態(tài)系統(tǒng)可觀測性矩陣的奇異值分解分析方法，文獻[3]又進行了改進，本文是應用分段線性系統(tǒng)可觀性理論和SOM分析法[4]，對系統(tǒng)狀態(tài)可觀測性和可觀測度的分析，拋開不可觀測或觀測度較低的系統(tǒng)狀態(tài)因子，從而得到一個只有12階的狀態(tài)變量降階模型。采用Visual Studio 2010語言開發(fā)環(huán)境設計編寫了常規(guī)卡爾曼濾波和降階濾波的組合導航程序，模擬設計飛行軌跡，最后通過計算機仿真驗證了降階后的模型仍能滿足組合導航高精度的要求，并且實時性得到了顯著的增強。

1 組合導航濾波結構和濾波算法

1.1 常規(guī)卡爾曼濾波算法

卡爾曼濾波是一種線性最小方差估計，它能夠從一系列不完全且包含噪聲的量測值中估計動態(tài)系統(tǒng)的狀態(tài)，用于實時融合動態(tài)的低層次冗余多傳感器數據，是實現組合導航系統(tǒng)的核心技術，由于采用數學迭代的運算方式，非常適合計算機數據運算。主要有如下特點[5-6]：

1）卡爾曼濾波的估計準則是估計的均方誤差最小，即

2）卡爾曼濾波是無偏估計，即估計誤差的均值為零。

3）卡爾曼濾波具有連續(xù)型和離散型兩類算法。離散型算法可直接在數字計算機上實現，在計算上一般采取遞推形式，離散系統(tǒng)的數學模型可以用下式表示:

其中，X是n維系統(tǒng)狀態(tài)向量；Z為m維量測向量；φ為n×n維系統(tǒng)矩陣；Γ為n×r維系統(tǒng)噪聲矩陣；H為m×n維量測矩陣；卡爾曼濾波要求{Wk}和{Vk}是互不相關且均值為零的白噪聲序列[7]。

離散型卡爾曼濾波方程組如下：

1.2 降解卡爾曼濾波算法

在設計組合導航系統(tǒng)卡爾曼濾波器的過程中，必須保證濾波模型的可控性和可觀性，這樣設計出的濾波器才是一致穩(wěn)定的，濾波結果的可靠性才能得到保證[8]。文獻[2-3]總結并給出了組合導航系統(tǒng)同樣可以采用分段線性系統(tǒng)理論以及采用SOM分析方法進行系統(tǒng)的可觀測性分析的結論，并給出了一種適合于分析全程系統(tǒng)狀態(tài)的可觀測度方法。下面對基于奇異值的分解的系統(tǒng)狀態(tài)可觀測度分析方法做簡要闡述。

定理：對于任意一個m×n階矩陣A，若Rank（A）=r＜max（m，n），則存在兩個正交矩陣 Um×m和Vn×n，使

其中，σ1≥σ2≥…≥σr≥0稱為矩陣 A 的奇異值，V2和V1分別是 N（A）和 N（A）┴的標準正交基，N（A）為矩陣 A 的零空間，N（A）┴為 N（A）的正交補空間[9-10]。

對于 SOM 矩陣，將可觀測矩陣 Qs（r）i=[Q1Q2…Q]T進行奇異值分解：

其中：

U=[u1，u2，…，um]，V=[v1，v2，…，vm]均為正交矩陣。

上式表明，Yj為 X0在[σ1v1，σ2v2，…，σrvr]張成的子空間上的投影變換，所以以需要r個測量值才能唯一的確定 Xj0，如果 σr＞0，則表明 Rank（Qs（r）j）=n，即系統(tǒng)是隨機可觀測的[11]，此時：

當存在 σi+1=σi+2=…=σr=0 時 Rank（Qs（r）j）＜n，即系統(tǒng)是不完全隨機可觀測的，Xj0的某些狀態(tài)是不能從觀測量Yj中估計出來的。此時，可利用奇異值分解法構造出可觀測子空間及不可觀測子空間的標準正交基，并由此可以分析出哪些狀態(tài)是可以估計的，哪些狀態(tài)是不可以估計的[12]。

由上式可知，Xj0是由r個列向量相加得到的。對于Xj0的某一分量，各向量中對應分量的絕對值最大的那個分量，包含了最多的的信息，對于的貢獻最大，而此列向量中包含相應的因子，此奇異值σi越大，其倒數越小，則測量值σi對的變換越敏感，也就是說，的變換可以由測量值yi中更明顯的反映出來，所以，次奇異值在一定程序上反映了狀態(tài)分量的可觀測程序。奇異值σi越大，的可觀性越好[13]。

由于計算中存在誤差，用奇異值分解法計算的奇異值時，理論上為零的奇異值可能不為零，而是一個很小的數。此時需根據計算機的精度選取一個界限值ζ，對于小于ζ的奇異值均認為是零奇異值，大于ζ的便認為是非零的奇異值。非零奇異值的個數就是可觀測矩陣的秩。由以上敘述可知，基于奇異值的系統(tǒng)可觀測性分析方法的優(yōu)點是，在確定系統(tǒng)可觀測性的同時，也可以確定系統(tǒng)可觀測狀態(tài)的可觀測度[14]。

2 數字仿真及分析

2.1 飛行軌跡仿真的設定

為了驗證組合導航系統(tǒng)性能，設計了一組飛行軌跡：設計飛機的初始位置為：北緯32.05°，東經118.766 7°，高度 0m；飛機的初始速度為 0m/s，航向120°，飛行任務有上升、加速、下降、左拐、右拐、靜止等，飛行時間為3 700s。具體飛行軌跡如圖1所示。

圖1 飛行軌跡圖

2.2 常規(guī)卡爾曼濾波仿真分析

采用位置、速度組合模式，常規(guī)卡爾曼濾波仿真誤差曲線如圖2、圖3、圖4所示。

圖2 常規(guī)卡爾曼濾波組合的位置誤差曲線

圖3 常規(guī)卡爾曼濾波組合的速度誤差曲線

圖4 常規(guī)卡爾曼濾波組合的姿態(tài)角誤差曲線

2.3 降解卡爾曼濾波仿真分析

基于分段線性系統(tǒng)理論及采用SOM分析方法對松耦合SINS/GPS系統(tǒng)進行分段，求得系統(tǒng)可觀測性矩陣的秩為11，系統(tǒng)存在4個完全不可觀測狀態(tài)。15階組合導航濾波器中4個完全不可觀測的系統(tǒng)狀態(tài)分別是航向角誤差φz、y軸上的陀螺隨機漂移εby，加速度計x軸上和z軸上的零位誤差由于航向角為導航參數，因此不能剔除，故將其余3個不可觀測的狀態(tài)變量得到12階的簡化濾波方案，濾波效果如圖5～圖7所示。

圖5 降階卡爾曼濾波組合的位置誤差曲線

圖6 降階卡爾曼濾波組合的速度誤差曲線

圖7 降階卡爾曼濾波組合的姿態(tài)誤差曲線

3 結論

由圖2～4與圖5～7對比看出：簡化的濾波方案位置和速度誤差與原濾波算法相當，姿態(tài)誤差比原濾波方案濾波效果稍差。實質上降階設計總是一種次優(yōu)設計[15]，但由于精度波動的幅度在可以接受的范圍，完全滿足組合導航的精度要求。同時數學模型階數的降低，使卡爾曼濾波計算時間減少了大約20％，簡化的濾波方案大大減少了矩陣迭代給導航計算機的計算量，提高了組合導航系統(tǒng)實時性，因此簡化的濾波方案具有較好的理論和實用價值。

[1]秦永元，汪叔華.卡爾曼濾波與組合導航原理[M].3版.西安：西北工業(yè)大學出版社，2014.

[2]胡小毛，劉飛.MINS/GPS全組合系統(tǒng)可觀測性分析[J].中國慣性技術學報，2011，19（1）:1-4.

[3]周衛(wèi)東，蔡佳楠.GPS/SINS超緊組合導航系統(tǒng)可觀測性分析[J].北京航空航天大報，2013，39（9）:1-6.

[4]嚴濤，王躍鋼，楊波.SINS/DR組合導航系統(tǒng)可觀測性研究[J].現代防御技術，2012，40（3）:1-5.

[5]羅建軍，馬衛(wèi)華.組合導航原理及應用[M].西安：西北工業(yè)大學出版社，2012.

[6]王新龍，李亞峰，紀新春.SINS/GPS組合導航技術[M].北京：北京航空航天大學出版社，2014.

[7]雷浩然.彈載SINS/GNSS組合導航系統(tǒng)研究[D].南京：南京理工大學，2014.

[8]朱莉.旋轉式捷聯慣導系統(tǒng)晃動基座自對準方法研究[D].北京：北京工業(yè)大學，2015.

[9]張賢達.矩陣分析與應用[M].北京：清華大學出版社，2013.

[10]廖安平.矩陣論[M].北京：清華大學出版社，2013.

[11]楊建峰.捷聯慣導系統(tǒng)初始對準可觀測性分析.[J]航天控制，2014，32（1）:1-4.

[12]高葉葉，李華濱.基于觀測方程的捷聯系統(tǒng)工具誤差可觀測性分析[J].航天控制，2014，32（3）:1-5.

[13]馬朋，張福斌.基于條件數的多自主水下航行器協(xié)同定位系統(tǒng)可觀測度分析[J].兵工學報，2015:36（1）:1-6.

[14]吳松羽，王可東.低精度INS/GPS組合導航大方位失準角初始對準方案[J].全球定位系統(tǒng)，2014：39（6）:1-5.

[15]袁克非.組合導航系統(tǒng)多源信息融合關鍵技術研究[D].哈爾濱：哈爾濱工程大學，2012.

The application of reduced order Kalman filter algorithm in integrated navigation

HU Bin1，YANG Xin-min1，WANG Sheng-hong2
（1.Nanjing University of Science and Technology，Nanjing 210094，China；2.Huai Industrial Group，Changzhi 046000，China）

This paper briefly introduce the basic principle of Kalman filter and analyze the observability and the degree of observability of system based on the piecewise linear system theory and the SOM method.Using the reduced order Kalman filter algorithm to reasonably eliminate the state factors which are not observable or low observable and make a reduced-order design for the high-end system.Using the reduced order Kalman filter algorithm to reasonably eliminate the state factors which are not observable or low observable and make a reduced-order design for the high-end system.At the same time，the reduction of the degree， which has the important practical significance， significantly reduces the computational burden of navigation computer and enhances the real-time so that the algorithm can be more easily realized digitally.

inertial navigation； integrated navigation； Kalman filter； digital simulation

TN961

：A

：1674－6236（2017）15-0098-04

2016-06-07稿件編號：201606058

胡 彬（1990—），男，江蘇連云港人，碩士。研究方向：組合導航算法。