盧 航，郝順義，彭志穎，黃國榮

空軍工程大學 航空工程學院，西安710038

1 引言

捷聯(lián)慣導系統(tǒng)（SINS）通過慣性測量元件（IMU）測量運載體相對于慣性系的運動信息，經(jīng)過導航計算機解算得到運載體的姿態(tài)、速度、位置等導航信息，是一種具有較強自主性的導航系統(tǒng)，其具有短時導航精度高，但是長時間導航精度低的特點。全球定位系統(tǒng)（GPS）是一種衛(wèi)星導航系統(tǒng)，它可以精確的提供運載體的速度、位置等導航信息，其長時間導航精度高，但是戰(zhàn)時自主性較差。SINS/GPS組合導航是一種結合兩種導航系統(tǒng)優(yōu)點的導航方式，可以提高導航系統(tǒng)的整體性能[1]。

在組合導航系統(tǒng)定位中，高精度的濾波算法對導航定位的解算精度有重要影響[2]?？柭鼮V波（KF）是最小均方差條件下的線性最優(yōu)濾波器，其可以較好地處理高斯噪聲條件下線性模型的狀態(tài)估計問題。然而，非線性問題在實際中更為普遍，組合導航系統(tǒng)是典型的強非線性系統(tǒng)，此時非線性濾波算法成為提高組合導航精度的關鍵。擴展卡爾曼濾波器（EKF）采用對非線性函數(shù)進行一階泰勒展開的原理，對非線性函數(shù)進行近似，顯然其會引入線性化截斷誤差，濾波精度只能達到一階，同時求取繁瑣的雅各比矩陣會造成較大的計算量和較低的數(shù)值穩(wěn)定性[3]。粒子濾波（PF）是一種無需對狀態(tài)方程近似、也無需對噪聲統(tǒng)計特性進行假設的的濾波算法，但是通常需要巨大的粒子數(shù)以犧牲計算量換取滿意的濾波精度，同時重要性概率密度的選擇也是一個難題。目前，較為常用的非線性濾波算法為高斯近似求和濾波，不同于EKF對狀態(tài)方程近似的思想，其認為近似后驗概率密度比近似非線性函數(shù)更容易，通過一定的規(guī)則精心選取采樣點和權值達到狀態(tài)估計的目的，其中以無跡卡爾曼濾波（UKF）和容積卡爾曼濾波（CKF）為代表，兩者無需計算雅各比矩陣且精度高于EKF[3-4]。文獻[4]研究對比了UKF和CKF的濾波精度和數(shù)值穩(wěn)定性問題，指出CKF的濾波精度可以精確到三階，同時UKF的數(shù)值穩(wěn)定性要低于CKF。文獻[5]在傳統(tǒng)CKF 的基礎上，提出了利用高容積規(guī)則計算高斯求和積分的HCKF算法，其濾波精度可以達到五階。

在實際物理系統(tǒng)中，量測信息通常會受到外部環(huán)境、儀器故障等因素的干擾，此時量測噪聲不再滿足高斯分布的前提假設[7-8]，所以基于噪聲服從高斯分布假設法是解決這一問題的有效方法[18]，文獻[19]將MCC 和KF 結合提出了一種基于MCC 的魯棒KF 算法，并且通過目標跟蹤仿真實驗驗證了其可以較好的應對高斯分布附近存在對稱干擾問題。為了改善組合導航算法的濾波精度和魯棒性，本文將MCC 方法應用于HCKF 算法，利用MCC對量測信息進行重新構建，提出一種基于MCC的魯棒HCKF濾波算法。當量測噪聲表現(xiàn)為高斯混合分布時，該算法在魯棒性、濾波精度等方面優(yōu)于傳統(tǒng)CKF，通過SINS/GPS 組合導航系統(tǒng)驗證了所提算法的優(yōu)越性。

2 SINS/GPS組合導航系統(tǒng)模型

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

記慣性坐標系為i 系，地球坐標系為e 系，載體坐標系為b 系，選取當?shù)氐乩碜鴺讼导础皷|、北、天”坐標系為導航坐標系n 系，SINS 計算所得的導航坐標系為p系。由于SINS 受各種誤差源的影響，導致導航計算機所計算出來的p 系與n 系之間存在著失準角誤差?=[?x?y?z]T，它表示n 系轉至p 系的一組歐拉角，轉動順序定義為?z、?x、?y。那么從n 系至p 系的坐標變換矩陣為：

用GPS/SINS松組合導航系統(tǒng)為濾波模型，以SINS的姿態(tài)、速度、位置、陀螺零偏和加速度計零偏的誤差方程作為非線性濾波模型的狀態(tài)方程，以SINS 和GPS 輸出的速度、位置誤差作為量測值。SINS 的姿態(tài)、速度、位置非線性誤差方程為[21]：

的濾波算法不能表現(xiàn)出良好的濾波性能。針對高斯分布附近存在對稱干擾問題，Huber提出了M估計同時針對非高斯噪聲的問題給出了Huber 方法[9-10]，Huber 用一種基于范數(shù)的混合代價函數(shù)取代基于l2范數(shù)的代價函數(shù)來處理干擾干高斯分布的問題，其魯棒性要優(yōu)于基于l2范數(shù)的估計方法[11]，文獻[12-16]提出了基于Huber的魯棒濾波算法并在目標跟蹤和無人機相對導航中取得了成功應用，但是Huber 在面對非高斯噪聲時，其量測更新過程仍然會受到較大的影響，最終導致濾波精度下降[17]。除此之外，近幾年提出的基于最大相關熵MCC（Maximum Correntropy Criterion）的魯棒濾波算

定義15 維狀態(tài)向量X=[?x?y?zδvxδvyδvzδLδλδh εbxεbyεbz?bx?by?bz]T，可由式（2）構成SINS/GPS 組合導航系統(tǒng)的狀態(tài)方程：

式中f()· 為非線性函數(shù)，w 為系統(tǒng)噪聲。

2.2 組合導航系統(tǒng)量測方程

取SINS 和GPS 輸出的速度、位置信息之差作為濾波器的量測值，即

那么組合導航系統(tǒng)的量測方程為：

3 基于MCC的魯棒高階CKF算法

3.1 最大相關熵

假設兩個隨機變量x,y ∈R，二者聯(lián)合概率密度函數(shù)為pxy(x,y)，定義相關熵為：

式中，E 表示期望，κ( x,y )表示高斯核函數(shù)，具體表達式為：

其中e=x-y，σ 為核寬且σ ＞0，在實際應用中，通常只能獲得有限的數(shù)據(jù)并且聯(lián)合概率密度是未知的，可以用一系列的采樣點得到V( x,y )的近似解[17]，此時式（8）可以寫為：

圖1為σ=1 時的相關熵示意圖，可以發(fā)現(xiàn)相關熵衡量兩個隨機變量之間的廣義相似程度，誤差e 的貢獻會隨著核寬σ 的變化出現(xiàn)不同程度的指數(shù)形式的衰減，當e=0 時κ( x,y )=1 取最大值。定義基于MCC 準則的代價函數(shù)為：

圖1 相關熵κ( x,y )示意圖

3.2 高階容積卡爾曼濾波

（1）濾波器初始化

（2）時間更新過程

①計算容積點Xi,k-1|k-1( i=0,1,…,2n2)：采用具有更高數(shù)值穩(wěn)定性的奇異值分解（SVD）代替了傳統(tǒng)的Cholesky 分解，該分解方法可以更好地解決協(xié)方差矩陣病態(tài)條件的問題，使得整個算法具有更高的數(shù)值穩(wěn)定性和濾波精度，對協(xié)方差矩陣Pk-1/k-1使用SVD 分解，即

式中，n 為系統(tǒng)的狀態(tài)維數(shù)，ei`表示n 維單位向量，第i個元素為1，的具體表達式為：

式中，ωi為容積點的權值，如下式所示：

④計算k 時刻的預測誤差協(xié)方差陣Pk/k-1：

（3）量測更新過程

①計算更新后的狀態(tài)容積點Xi,k|k-1：

其中，Pk/k-1=Sk|k-1(Sk|k-1)T。

②計算經(jīng)過量測方程傳遞的容積點Zi,k|k-1：

③計算k 時刻的測量預測值z?k|k-1：

④計算k 時刻的量測誤差協(xié)方差陣Pzz,k|k-1和預測互相關協(xié)方差陣Pxz,k|k-1：

（4）濾波更新過程

計算k 時刻的濾波增益矩陣Kk、濾波狀態(tài)值x?k|k和協(xié)方差陣Pk|k：

3.3 統(tǒng)計線性回歸的MCC方法

MCC方法的核心是使得定義的代價函數(shù)取得最大值。將MCC應用于HCKF，改變量測更新的方式，得到一種基于MCC 的魯棒HCKF 算法。首先，定義k 時刻的狀態(tài)預測誤差為：

式中xk為k 時刻的狀態(tài)真值，為k 時刻的一步預測值。根據(jù)式（6）可知組合導航系統(tǒng)的量測方程為線性，由文獻[21]，此時式（22）～（24）可以表示為：

由以上三式，可以構造線性回歸方程：

定義：

那么式（32）可以改寫為：

定義代價函數(shù)：

式（38）可以等價表示為式（10）所示的MCC形式：

其中，N=n+m，ei,k為殘差向量ei,k=yi,k-Bkxi,k的第i個分量。

通過對代價函數(shù)求取偏導數(shù)得到其最大值，對其求偏導得到：

記φk( ei)=ei,k?Gσ( ei,k)，上式可以改寫為：

定義權函數(shù)Ck=φk(ei/ei)，式（41）可以進一步寫為：

上式可以寫成關于狀態(tài)變量xk的迭代方程：

相較于梯度下降法，不動點迭代法無需選取迭代步長并且可以快速收斂到最優(yōu)解附近，對上式采用不動點迭代算法進行求解得到：

迭代結束后求得的方差為：

式（43）可以進一步表示為：

其中：

3.4 基于MCC的高階容積卡爾曼濾波算法

將上述的MMC 方法和高階容積卡爾曼濾波相結合，使得HCKF中的量測更新過程轉化為求解線性回歸方程的問題，得到新的魯棒濾波算法，算法具體流程總結如下。

（1）初始化

（2）時間更新

MCC-HCKF的時間更新過程與3.2節(jié)中傳統(tǒng)HCKF的時間更新過程一致，利用式（11）～（19）和容積點及其權重（14）、（18）得到k 時刻的一步狀態(tài)預測xk|k-1和協(xié)方差Pk|k-1，然后采用Cholesky分解得到矩陣Sk。

（3）量測更新

①根據(jù)式（29）計算k 時刻的量測預測值z?k|k-1，通過式（32）構造線性回歸方程。

②將線性回歸方程改寫為式（37）的形式，令迭代次數(shù)t=1，同時設置初始迭代值為：

③代價函數(shù)取得最大值時，根據(jù)式（46）、（47）、（50）～（52）得到k 時刻的第t 次迭代狀態(tài)為：

⑤最后，根據(jù)式（48）求得k 時刻的狀態(tài)誤差協(xié)方差陣Pk|k。

由上述算法流程可以看到，基于MCC 的魯棒HCKF 的時間更新過程仍然采用了濾波精度高達五階的高階容積準則，這延續(xù)了HCKF 高精度的優(yōu)點。MCC-HCKF和傳統(tǒng)HCKF的區(qū)別在于前者建立了統(tǒng)計線性回歸模型對傳統(tǒng)量測更新過程進行了重構，并通過MCC 準則計算得到當前狀態(tài)的估計值，相比于傳統(tǒng)HCKF 更好地克服了非高斯噪聲的影響，所以MCCHCKF 是一種兼具HCKF 高精度特點和MCC 方法魯棒性的濾波算法。值得說明的是，當核寬σ →∞時，此時MCC-HCKF變?yōu)閭鹘y(tǒng)的HCKF，具體證明如下。

將上式代入式（45）可得：

顯然此時MCC-HCKF和HCKF是等價的。

3.5 算法魯棒性分析

文獻[7-9]給出了基于Huber 魯棒濾波算法的代價函數(shù)J( e )和權函數(shù)ψ( e )，由式（39）、（41）可知基于MCC的代價函數(shù)J( e )和權函數(shù)ψ( e )，具體表達式如表1和圖2所示。

表1 代價函數(shù)與權函數(shù)

圖2 權函數(shù)示意圖

由圖2 可以看到，傳統(tǒng)濾波算法的權函數(shù)ψMSE( e)為常值，基于Huber 的權函數(shù)ψHuber( e )是由二段權函數(shù)構成，基于MCC 的權函數(shù)ψMCC( e )是由指數(shù)函數(shù)構成。由于ψMSE( e )=1，當出現(xiàn)量測異常時，傳統(tǒng)算法無法減弱異常干擾，所以會影響算法的濾波精度和魯棒性。當|e |＞α 時，ψHuber( e )會隨著e 的增加逐漸減小以達到減小量測異常干擾的目的，然而ψMCC( e )會隨著e 的增加呈現(xiàn)指數(shù)形式的衰減，相比于ψHuber( e )可以迅速減小到0附近，具有更快的衰減過程，所以可以有效抑制量測異常的影響，因此算法具有更強的魯棒性。

4 仿真實驗與分析

設置飛行器的初始位置為東經(jīng)108°，北緯34°，高度300 m，載體初始速度大小為10 m/s，方向正北，載體經(jīng)過加速、勻速、橫滾、右盤旋、左盤旋、爬升、下降、減速等機動組成，具體機動如表2 所示；飛行軌跡如圖3 所示，紅色箭頭表示飛機的起始位置。

SINS 的初始位置誤差為10 m，初始速度誤差為0.5 m/s，東向、北向初始失準角為1°，天向失準角為1°。GPS的水平位置誤差均方根為10 m，高度誤差均方根為3 m，速度誤差均方根為0.1 m/s。SINS 的解算周期為0.01 s，GPS信號采樣周期為0.1 s，飛行時間600 s。初始方差陣P(0)、系統(tǒng)噪聲陣Qk設置為：

為了驗證本文提出的魯棒濾波算法的有效性，設置兩種情況對提出的算法進行實驗，結果及分析如下。

表2 飛行器軌跡仿真

圖3 飛行軌跡圖

定義k 時刻的位置RMSE為：

定義k 時刻的位置ARMSE為：

實驗1 量測信息為高斯分布。

在飛行過程中，假設GPS 的量測信息正常，此時量測噪聲滿足如下形式的高斯分布：

圖4 高斯噪聲條件下的位置RMSE

表3 高斯噪聲條件下的位置ARMSE m

結合圖4和表3的實驗結果可以看出，100 s后組合導航系統(tǒng)濾波器收斂，此時HCKF 的定位誤差小于CKF，由此可知采用高階容積規(guī)則的HCKF表現(xiàn)優(yōu)于采用三階容積規(guī)則的CKF。MCC-HCKF表現(xiàn)和HCKF大致相同，但是定位精度略低于HCKF這是因為在高斯噪聲環(huán)境下，HCKF 是基于最小均方差的濾波器，當噪聲服從高斯分布時其表現(xiàn)自然最優(yōu)，此時MCC-HCKF 的魯棒性是以犧牲一定的濾波精度來換取的。

實驗2 量測信息為受污染的高斯混合分布。

在飛行過程中，假設GPS的量測信息為受污染的高斯白噪聲，此時量測信息服從高斯混合分布，而且方差是原高斯分布的10倍。式（61）給出了受污染的高斯白噪聲的概率分布表達式，式中ε 為混合百分比，其取值范圍為0～1，顯然當ε=0 時，噪聲服從理想的高斯分布，本文取ε=0.2，同時增加文獻[14]中的H-HCKF作為對比算法，參數(shù)α=1.345。

從圖5 可以看出，當量測噪聲不再滿足高斯分布時，傳統(tǒng)的HCKF 的濾波精度均下降，說明算法無法有效抑制非高斯噪聲對系統(tǒng)的影響，而MCC-HCKF 濾波此時表現(xiàn)出了良好的濾波性能，精度高于HCKF。這是因為MCC-HCKF濾波算法在進行與HCKF相同的時間更新后，在量測更新過程中通過數(shù)值迭代求解的方法，進行了一次基于MCC 的線性回歸估計，這一過程可以減弱觀測噪聲與假設分布之間的偏差對濾波器的影響，但同時也會增加一定的計算量。隨著核寬σ 的減小，濾波器的魯棒性增強，精度也隨之提高。隨著核寬σ 增大，魯棒性下降，濾波器的表現(xiàn)越接近HCKF，這與上文的理論證明是一致的。對比圖中H-HCKF和MCC-HCKF的估計曲線，可以發(fā)現(xiàn)H-HCKF 也具有較高的濾波精度，高于MCC-HCKF(σ=4)，低于MCC-HCKF(σ=3)，說明此時MCC-HCKF(σ=3)表現(xiàn)略優(yōu)于H-HCKF。

圖5 非高斯噪聲條件下的位置RMSE

為了比較上述五種算法的計算時間，仿真軟件采用Matlab2014a，仿真處理器為Intel Inside Core i5處理器，將HCKF、H-HCKF、MCC-HCKF(σ=6)、MCC-HCKF(σ=4)、MCC-HCKF(σ=3)五種算法的單次蒙特卡洛仿真實驗所消耗的運行時間進行統(tǒng)計，結果如表4所示。

表4 五種濾波算法蒙特卡洛實驗時間統(tǒng)計

為了進一步分析核寬σ 對組合導航定位精度的影響，圖6為不同σ 條件下的位置ARMSE。由圖可知，在σ=2.5 附近經(jīng)度、緯度和高度的ARMSE達到最小，大于或者小于這個值都會增大定位誤差，需要指出當σ →0此時濾波器將會出現(xiàn)數(shù)值穩(wěn)定性問題，濾波精度急劇下降，說明選取合適的σ 對組合導航的定位精度有著較大的影響，σ=2.5 時系統(tǒng)受到非高斯噪聲的影響更小。需要說明的是，當組合導航系統(tǒng)工作環(huán)境惡劣或者儀器發(fā)生故障時，量測噪聲的分布與高斯分布會存在較大的偏差，根據(jù)3.4節(jié)中的證明過程，當σ →∞，MCC-HCKF是和傳統(tǒng)HCKF等價的，所以此時應需要盡可能選取較小的核寬σ 來抑制非高斯噪聲的影響。值得說明的是，通過上述實驗結果可以發(fā)現(xiàn)非高斯噪聲主要通過量測值來影響組合導航系統(tǒng)的工作性能，所以下一步可以研究核寬σ 與殘差間的關系，進而達到自適應調(diào)整核寬的目的，使得算法具有更強的自適應性和實用性。

圖6 不同σ 下的位置ARMSE

5 結論

本文以SINS/GPS 組合導航為研究背景，給出了組合導航系統(tǒng)的非線性誤差模型以及濾波器的狀態(tài)方程和量測方程，針對組合導航量測噪聲不服從高斯分布的問題，將高階容積準則和統(tǒng)計線性回歸模型結合，提出了一種新的基于MCC方法的魯棒高階容積卡爾曼濾波算法，并進行了仿真實驗。仿真結果表明，在選取適當核寬的條件下，該方法可以有效解決系統(tǒng)非線性和量測噪聲非高斯的問題，能夠提高組合導航的定位精度。