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

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

0 引言

時至今日，隨著現(xiàn)代導(dǎo)航系統(tǒng)的不斷發(fā)展，導(dǎo)航模式也越來越多樣化，例如，已經(jīng)相當完善的SINS系統(tǒng)、無線電導(dǎo)航系統(tǒng)以及精度更高的GPS導(dǎo)航系統(tǒng)等。要提升整個導(dǎo)航系統(tǒng)的性能，有必要將兩個或兩個以上的導(dǎo)航系統(tǒng)技術(shù)加以組合應(yīng)用，以達到優(yōu)勢互補的目的，這就是組合導(dǎo)航技術(shù)。

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

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

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

1 卡爾曼濾波基本原理

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

設(shè)系統(tǒng)狀態(tài)方程和測量方程為

式中，Xk是n維的系統(tǒng)狀態(tài)變量；Zk是m維的測量向量；F是n階的狀態(tài)轉(zhuǎn)移矩陣，G是系統(tǒng)噪聲向量的分布矩陣，H是m×n階的測量矩陣，F(xiàn)、G、H都是已知的系統(tǒng)結(jié)構(gòu)參數(shù)；wk是l維的系統(tǒng)噪聲向量，vk是m維的量測噪聲向量，這兩個噪聲向量都是高斯白噪聲向量序列，均值為零，服從正態(tài)分布。滿足

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

對狀態(tài)變量進行預(yù)估計：

對預(yù)估計協(xié)方差矩陣進行計算：

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

更新對狀態(tài)變量的估計：

計算更新后的估計協(xié)方差矩陣：

2 基于最大相關(guān)熵的卡爾曼濾波

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

2.1 最大相關(guān)熵準則（mcc）

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

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

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

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

2.2 最大相關(guān)熵卡爾曼濾波（mcckf）

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

加權(quán)矩陣在最小二乘法中R-k1和Pk-|k1-1導(dǎo)致最小方差估計，而引入相關(guān)熵后會使用高階矩，結(jié)合最大相關(guān)熵和最小二乘法的思想來重新定義一個新的目標函數(shù)：

為了將這個目標函數(shù)最小化，對進行求導(dǎo)，計算可得：

由式（14）進行數(shù)學(xué)推導(dǎo)可得：

即

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

由上式推導(dǎo)出新的估計量如下：

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

3 仿真與分析

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

為了測試基于最大相關(guān)熵的卡爾曼濾波的性能，對如下所示的組合導(dǎo)航系統(tǒng)進行仿真。

3.1 狀態(tài)方程的建立

系統(tǒng)狀態(tài)變量共有13維，狀態(tài)方程為

GPS/INS組合導(dǎo)航系統(tǒng)的系統(tǒng)狀態(tài)變量為

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

式中，F(xiàn)為狀態(tài)X的系數(shù)矩陣；系統(tǒng)噪聲為

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

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

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

則組合導(dǎo)航系統(tǒng)的量測方程為

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

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

仿真結(jié)果如圖1和圖2所示。

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

4 結(jié)語

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