基于最大相關熵的GPS/INS組合導航融合算法研究

徐開俊，張 榕，楊 泳，徐照宇，趙津晨，林浩冬，肖成坤，曹海波

（1.中國民用航空飛行學院，廣漢 618300；2.四川大學華西醫院，成都 610000；3.成都天府新區建設投資有限公司，成都 610000）

0 引言

時至今日，隨著現代導航系統的不斷發展，導航模式也越來越多樣化，例如，已經相當完善的SINS系統、無線電導航系統以及精度更高的GPS導航系統等。要提升整個導航系統的性能，有必要將兩個或兩個以上的導航系統技術加以組合應用，以達到優勢互補的目的，這就是組合導航技術。

INS主要提供的導航信息數據有姿態角（包括航向角、俯仰角和橫滾角三個量），方位角，東、北、天三個方向上的速度和位置，以及對應方向上的加速度和角速率，利用這些數據信息可對運載體進行準確操控。它具有很好的自主性和動態性能，可以在高頻下輸出更全面的導航信息，但是其缺點是隨著工作時間的增加，誤差會持續疊加，長時間運行的情況下精度會下降［1］。GNSS具有精度高、誤差不隨工作時間增加而增加的優點，但存在輸出導航信息不夠全面、信號易受干擾等問題。在自主導航系統向以信息融合為基礎的組合導航技術發展的今天，其發展趨勢是全自主、高精度、高可靠性［2］。從上述對慣性導航系統和全球導航衛星系統的優劣勢的分析可以得出，這兩種導航系統的優缺點可以達到接近完美的互補，因此GNSS/INS被公認為是最佳的組成組合導航系統的方式。當進行組合導航系統的設計時，特別是導航數據融合處理時，一般都會采用卡爾曼濾波器，即利用衛星導航設備測得的導航數據作為輔助觀測，估計組合導航系統的狀態變量，得到最優解，從而獲得更高精度的導航信息。在導航領域應用最早、最成功的濾波器就是卡爾曼濾波器。

隨著組合導航系統的蓬勃發展，越來越多的導航傳感器被當作集成控制系統的局部子感應器，對于具有多個子導航系統的組合導航系統，在對組合導航進行綜合分析研究時，應該從信息整體融合的觀點出發。

本文選擇以SINS系統為主、GPS為輔系統的組合方式，研究基于卡爾曼濾波的多源數據信息融合算法，針對傳統卡爾曼濾波在長時間工作狀態下存在濾波發散的問題，提出一種基于最大相關熵的卡爾曼濾波算法，該算法不僅可以提高濾波精度，且整個系統的魯棒性也有所提升，并給出了整個濾波融合的仿真結果。

1 卡爾曼濾波基本原理

遞歸運算和運算效率高是卡爾曼濾波的顯著優點［3］，經過濾波后的解是最小均方誤差意義上的最優解。一般來說，卡爾曼濾波器的每次更新迭代可以按照以下兩個步驟進行：第一步是根據系統狀態轉移方程及時預測和更新系統狀態，與此同時，對狀態協方差矩陣進行更新；第二步是更新觀測量，在觀測量到來以后，必須首先更新狀態協方差矩陣，以便通過計算得到卡爾曼增益矩陣，也就是卡爾曼因數，最后系統狀態的變量被重新更新，循環往復。這樣，得到的最新估計值即為下一時刻的系統新一輪迭代的起始值。總之，卡爾曼濾波器是通過系統狀態預測方程和測量方程準確估計系統數據的實時狀態。

設系統狀態方程和測量方程為

式中，Xk是n維的系統狀態變量；Zk是m維的測量向量；F是n階的狀態轉移矩陣，G是系統噪聲向量的分布矩陣，H是m×n階的測量矩陣，F、G、H都是已知的系統結構參數；wk是l維的系統噪聲向量，vk是m維的量測噪聲向量，這兩個噪聲向量都是高斯白噪聲向量序列，均值為零，服從正態分布。滿足

在給定狀態初始估計及噪聲協方差矩陣Q和R之后，按下式進行卡爾曼濾波更新。

對狀態變量進行預估計：

對預估計協方差矩陣進行計算：

對卡爾曼增益矩陣進行計算：

更新對狀態變量的估計：

計算更新后的估計協方差矩陣：

2 基于最大相關熵的卡爾曼濾波

近幾年來，隨著信息論的不斷發展，人們對于其中的優化準則也越發重視，這種優化準則是直接采用了從數據中所估計的信息論量（如熵）［4］，而不是常用的二階統計量（如方差、協方差）作為優化成本函數。最大相關熵準則就是來源于信息論領域，它既可以獲得誤差的方差、協方差等關鍵信息，還可以對濾波的誤差進行計算得到高階統計量，能夠大大改善系統的跟蹤性能，提高系統的魯棒性［5-7］。

2.1 最大相關熵準則（mcc）

考慮隨機變量X和Y，兩者之間的相關熵定義為

式中，E表示求期望值；p(x,y)表示隨機變量X和Y的聯合概率密度函數；k(x,y)表示核函數。

由于實際情況中的聯合概率密度函數p(x,y)難以獲得，一般采用樣本的平均值來計算相關熵的估計值：

在上式中使用高斯核［8-10］，因此寫成：

2.2 最大相關熵卡爾曼濾波（mcckf）

卡爾曼濾波器的基本方程通常有以下幾種推導方式，例如：正交原理法，最小方差無偏估計法，極大后驗估計法和加權最小二乘估計法。在加權最小二乘估計［11］中，二次目標函數為

加權矩陣在最小二乘法中R-k1和Pk-|k1-1導致最小方差估計，而引入相關熵后會使用高階矩，結合最大相關熵和最小二乘法的思想來重新定義一個新的目標函數：

為了將這個目標函數最小化，對進行求導，計算可得：

由式（14）進行數學推導可得：

即

為了得到狀態估計，在式（15）的右邊對（LkHTR-k1Hx?k）進行加減運算，并通過推導得出下式：

由上式推導出新的估計量如下：

將這種估計器稱為最大相關熵卡爾曼濾波器，它使用相關熵來處理數據信號，并在增益矩陣的計算中使用協方差。

3 仿真與分析

需要建立SINS和GPS的組合導航系統的系統狀態方程和量測方程，來實現GPS/INS組合導航系統的卡爾曼濾波器。選擇以誤差作為系統的被估計量建立模型，根據被給定估計狀態和一步預測均方差的初值X?0與P0，并采用隨時間變化的增益子回路和濾波計算回路，可以得到隨時間更新的導航參數以及濾波輸出。

為了測試基于最大相關熵的卡爾曼濾波的性能，對如下所示的組合導航系統進行仿真。

3.1 狀態方程的建立

系統狀態變量共有13維，狀態方程為

GPS/INS組合導航系統的系統狀態變量為

其中，δL，δλ分別表示經度、緯度的誤差；δvE，δvN分別是東向、北向移動速度的誤差；φE，φN，φU表示東向、北向、天向三個方向上航向姿態角的誤差；εrx，εry，εrz表示陀螺儀隨機漂移；?x，?y，?z表示加速度計零偏。

式中，F為狀態X的系數矩陣；系統噪聲為

其中，ωgx，ωgy，ωgz為陀螺漂移白噪聲；ωbx，ωby，ωbz為隨機漂移馬爾可夫過程白噪聲；ωax，ωay，ωaz為角速度計零偏一階馬爾可夫白噪聲。

3.2 位置速度組合量測方程建立

取INS和GPS輸出的速度、位置數據的差值作為濾波器的量測值，即

則組合導航系統的量測方程為

在利用卡爾曼濾波算法進行迭代時，首先要做的就是對所獲取的狀態變量進行初始化來得到初值，對于系統初值X0，要求與wk和vk互不相關。初始化后可根據卡爾曼濾波進行預期估計，在傳統卡爾曼濾波算法迭代過程中，環境觀測噪聲R是已知的，并且不隨時間的變化而變化，由于導航系統的運行狀態十分復雜，因此考慮將最大相關熵引入到組合導航系統內部中來求得出全局最優解。通過Matlab軟件進行仿真，驗證算法的可靠性。

整個濾波仿真參數如下：初始經度L=114°，初始緯度λ=35°；重力加速度g=9.7804 m/s2，加速度計零偏為10-4g；一階馬爾可夫相關時間τs=300 s，采樣間隔T=1 s；陀螺漂移取0.1°/h，地球自轉角度ωie=15°/h。

仿真結果如圖1和圖2所示。

從圖1和圖2可以看出，在非線性系統中傳統卡爾曼濾波器的誤差隨著工作時間的推移逐漸增大，且所表現出的穩定性不夠好，相比之下，基于最大相關熵的卡爾曼濾波對位置和速度的估計精度在一定程度上有所提升，東向、北向誤差逐漸變大，而經過最大相關熵卡爾曼濾波器數據處理后，速度誤差在0.5 m/s附近，精度提升效果顯著。圖3、圖4表明，對于位置的精度提升不是特別明顯，但是傳統卡爾曼濾波算法隨著工作時間的增加已有發散的跡象，而基于最大相關熵的卡爾曼濾波器依舊能夠穩定地工作，可以明顯看出，新算法能夠有效消減環境變化或運行狀態所引起的測量噪聲發生 的變化，表現出更好的魯棒性。

4 結語

針對傳統卡爾曼濾波在GPS/INS組合導航系統中，由于長時間工作所導致的濾波發散的問題，本文建立了數學模型并進行仿真與分。結果表明，本文所提出的基于最大相關熵的卡爾曼濾波算法在組合導航應用中是一種有效的算法，可以有效消減量測噪聲變化所引起的干擾，在一定程度上提高了濾波精度和濾波器的魯棒性，對后續改進卡爾曼濾波器具有一定的參考價值。