張玉龍， 王 茁， 楊 巍

(1.海軍駐哈爾濱地區(qū)艦船配套軍事代表室，黑龍江 哈爾濱 150046；2.哈爾濱工業(yè)大學(xué) 電氣工程及自動(dòng)化學(xué)院，黑龍江 哈爾濱 150001)

0 引 言

在常規(guī)卡爾曼濾波器中，根據(jù)先驗(yàn)知識(shí)確定系統(tǒng)噪聲和量測(cè)噪聲的協(xié)方差陣后，噪聲協(xié)方差陣就一直作為常值參與濾波遞推過程。但在實(shí)際應(yīng)用中，噪聲統(tǒng)計(jì)信息通常難以準(zhǔn)確獲取，而且其統(tǒng)計(jì)特性受環(huán)境溫度、載體機(jī)動(dòng)等外界因素影響會(huì)發(fā)生變化[1,2]。此時(shí)，常規(guī)卡爾曼濾波并不能根據(jù)外部量測(cè)數(shù)據(jù)來修正其噪聲參數(shù)，使得估計(jì)誤差不斷積累，導(dǎo)致濾波精度下降，甚至引起濾波發(fā)散[3,4]。

近年來，為了解決常規(guī)卡爾曼濾波在噪聲統(tǒng)計(jì)信息未知或者時(shí)變情況下存在的問題，相繼提出了各種自適應(yīng)濾波算法，主要包括基于極大后驗(yàn)估計(jì)的Sage-Husa自適應(yīng)濾波[5]、基于極大似然估計(jì)(maximum likelihood estimation,MLE)的自適應(yīng)濾波[6]以及各種漸消自適應(yīng)濾波[7]等。其中，Sage-Husa自適應(yīng)濾波能夠通過實(shí)時(shí)估計(jì)噪聲的統(tǒng)計(jì)特性提高濾波器的估計(jì)精度，但噪聲統(tǒng)計(jì)信息估值器常處于臨界穩(wěn)定狀態(tài)[8],容易導(dǎo)致濾波器發(fā)散；漸消自適應(yīng)濾波通過增大濾波一步預(yù)測(cè)均方誤差陣提高新近量測(cè)數(shù)據(jù)權(quán)重，但標(biāo)量漸消因子的計(jì)算過程較為繁瑣[9]，而且對(duì)各濾波通道具有相同調(diào)節(jié)能力，不利于改善濾波器的穩(wěn)定性和精度；常規(guī)的MLE自適應(yīng)濾波雖然能夠在線估計(jì)與修正噪聲統(tǒng)計(jì)特性的二階矩，但是需要依賴于準(zhǔn)確的新息協(xié)方差估計(jì)值，而目前利用開窗法得到的新息協(xié)方差估值器并不能突出滑動(dòng)窗口內(nèi)新近協(xié)方差序列的作用[10]，限制了進(jìn)一步提高其估計(jì)精度。

為了提高濾波器在噪聲統(tǒng)計(jì)信息未知或者時(shí)變情況下的估計(jì)精度，首先本文在得到基于極大似然估計(jì)的新息協(xié)方差估值器后，提出了一種基于限定記憶指數(shù)衰減加權(quán)的改進(jìn)算法, 利用新息協(xié)方差的指數(shù)加權(quán)值代替其算數(shù)平均值，提高了新息估計(jì)的精度。對(duì)捷聯(lián)慣性導(dǎo)航系統(tǒng)/全球定位系統(tǒng)(strapdown inertial navigation system/global positioning system,SINS/GPS)組合導(dǎo)航系統(tǒng)的仿真表明:與基于MLE自適應(yīng)濾波相比，本文算法在噪聲統(tǒng)計(jì)信息未知或者時(shí)變情況下具有更強(qiáng)的魯棒性，而且濾波精度得到了顯著提高。

1 基于MLE的新息協(xié)方差估值器

考查動(dòng)態(tài)離散系統(tǒng)，其狀態(tài)方程和量測(cè)方程分別滿足

Xk=Φk,k-1Xk-1+Γk-1Wk-1

(1)

Zk=HkXk+Vk

(2)

式中Xk為n維狀態(tài)序列；Φk,k-1為n×n階一步轉(zhuǎn)移矩陣;Γk為n×p階系統(tǒng)噪聲系數(shù)陣;Zk為m維量測(cè)序列;Hk為m×n階量測(cè)陣;Wk為p維系統(tǒng)噪聲序列;Vk為m維量測(cè)噪聲序列。Wk和Vk是不相關(guān)的高斯白噪聲，噪聲統(tǒng)計(jì)特性滿足

E[W(k)]=0,Cov[W(k),W(j)]=Q(k)δkj

E[V(k)]=0,Cov[V(k),V(j)]=R(k)δkj

Cov[W(k),V(j)]=0

式中δkj為Kronecker-δ函數(shù)；假定協(xié)方差陣Qk為m×n階非負(fù)定陣；Rk為m×m階正定陣。

設(shè)狀態(tài)一步預(yù)測(cè)值為

(3)

狀態(tài)估計(jì)值為

(4)

式中

(5)

εk=Zk-Hkk/k-1

(6)

式中Kk為濾波增益矩陣；Pk/k-1為一步預(yù)測(cè)均方誤差;εk為新息序列，且其協(xié)方差理論值滿足

(7)

(8)

將式(8)代入式(5)中，即可得到簡(jiǎn)化的濾波增益矩陣

(9)

在新息自適應(yīng)濾波器中由式(9)代替式(5)，即利用新息協(xié)方差的實(shí)際估計(jì)值代替其理論估計(jì)值，可以加快濾波器的收斂速度，同時(shí)修正由于噪聲統(tǒng)計(jì)特性未知或者時(shí)變而引入的估計(jì)誤差，改善濾波精度。下面將利用極大似然估計(jì)證明式(8)的結(jié)論。

極大似然估計(jì)是從系統(tǒng)量測(cè)量出現(xiàn)概率最大的角度進(jìn)行估計(jì)，在k時(shí)刻ζ條件下Z的條件概率密度函數(shù)滿足

(10)

式中ζ為系統(tǒng)參數(shù)；m為量測(cè)陣維數(shù)；|·|為矩陣行列式。

對(duì)式(10)取對(duì)數(shù)

(11)

對(duì)式(11)進(jìn)行累加運(yùn)算，求取和式的極大值可轉(zhuǎn)化為

(12)

(13)

最終得到基于極大似然估計(jì)的新息協(xié)方差估值器，其表達(dá)式如式(8)所示。

通過式(8)發(fā)現(xiàn)，新息協(xié)方差在k時(shí)刻的估計(jì)值k是對(duì)先前所有時(shí)刻新息協(xié)方差的算數(shù)平均，即先前所有數(shù)據(jù)具有相同的利用權(quán)重；但對(duì)于動(dòng)態(tài)系統(tǒng)而言，需要更加強(qiáng)調(diào)新近新息序列在濾波器中的作用，為此文獻(xiàn)[11]采用了開窗法求取新息協(xié)方差的估計(jì)值，即對(duì)限定長(zhǎng)度的滑動(dòng)窗口內(nèi)的新息協(xié)方差進(jìn)行算數(shù)平均，其表達(dá)式為

(14)

式中N為滑動(dòng)窗口長(zhǎng)度。

由式(14)可以看出，利用開窗法求取新息協(xié)方差估計(jì)值實(shí)質(zhì)上僅截取了新近一段長(zhǎng)度為N的新息協(xié)方差序列，并對(duì)其進(jìn)行算數(shù)平均，但仍未提高滑動(dòng)窗口內(nèi)新近數(shù)據(jù)的利用權(quán)重。

為此，本文提出了一種基于限定記憶指數(shù)衰減加權(quán)的新息協(xié)方差估值器,通過對(duì)滑窗內(nèi)數(shù)據(jù)進(jìn)行指數(shù)衰減加權(quán)，突出新近數(shù)據(jù)的作用。

2 基于限定記憶指數(shù)加權(quán)的新息協(xié)方差值器

考察限定記憶長(zhǎng)度為N的新息協(xié)方差序列，為提高該序列中新近數(shù)據(jù)的利用權(quán)重，將當(dāng)前時(shí)刻新息協(xié)方差權(quán)重看作基準(zhǔn)，并依次對(duì)滑動(dòng)窗口內(nèi)的先前協(xié)方差進(jìn)行指數(shù)衰減加權(quán)，最終得到新息協(xié)方差在當(dāng)前時(shí)刻的估計(jì)值。

當(dāng)k>N時(shí)，選取加權(quán)系數(shù){αi}，在k時(shí)刻使之滿足

于是有

(15)

(16)

對(duì)于k時(shí)刻的新息協(xié)方差估計(jì)值k滿足

(17)

將αi展開并代入式(17)，整理得到

(18)

式(18)即基于限定記憶指數(shù)加權(quán)的新息協(xié)方差估值器在k時(shí)刻的表達(dá)式。

同理，對(duì)于k-1時(shí)刻的新息協(xié)方差估計(jì)值k-1成立

(19)

由式(18)、式(19)可以得到限定記憶指數(shù)加權(quán)的新息協(xié)方差估值器的遞推形式

(20)

另外，當(dāng)k≤N時(shí)，新息協(xié)方差估計(jì)值則取其極大似然估計(jì)值，其表達(dá)式如式(8)所示。

將式(20)代入式(9)，用基于限定記憶指數(shù)加權(quán)的新息協(xié)方差估計(jì)值代替滑窗內(nèi)新息協(xié)方差序列的算數(shù)平均值，不僅可以增加新近量測(cè)數(shù)據(jù)在濾波器中的利用權(quán)重，同時(shí)還可以提高新息協(xié)方差估計(jì)值的精度。

3 仿真結(jié)果與分析

為了驗(yàn)證本文提出的基于極大似然估計(jì)的新息自適應(yīng)濾波算法在噪聲統(tǒng)計(jì)信息未知和時(shí)變情況下的濾波性能，設(shè)置在SINS/GPS組合導(dǎo)航系統(tǒng)中進(jìn)行仿真實(shí)驗(yàn)，并針對(duì)兩種情況，進(jìn)行了2組仿真實(shí)驗(yàn)。

實(shí)驗(yàn)選取SINS為主導(dǎo)航系統(tǒng)，速度誤差、位置誤差以及失準(zhǔn)角為狀態(tài)向量，并根據(jù)SINS誤差方程建立濾波器的狀態(tài)方程，GPS作為輔助導(dǎo)航系統(tǒng)提供量測(cè)信息，SINS/GPS組合導(dǎo)航系統(tǒng)模型具體參數(shù)參見文獻(xiàn)[12]，利用輸出反饋修正SINS輸出的導(dǎo)航參數(shù)。分別采用文獻(xiàn)[10]基于MLE自適應(yīng)濾波算法和本文提出的基于極大似然估計(jì)的新息自適應(yīng)濾波算法對(duì)載體的速度及位置誤差等參數(shù)進(jìn)行估計(jì)，濾波算法性能可以通過SINS/GPS組合導(dǎo)航系統(tǒng)速度及位置的估計(jì)誤差衡量。其中，本文算法的濾波參數(shù)中衰減因子取b=0.7，滑動(dòng)窗口長(zhǎng)度取N=15；MLE自適應(yīng)濾波的滑動(dòng)窗口長(zhǎng)度取M=20。仿真條件設(shè)置如下：載體運(yùn)動(dòng)速度為(3m/s,4m/s,0m/s),姿態(tài)角為(1°,1°,5°),初始經(jīng)緯度為(126.67°,45.77°);仿真時(shí)間為500s,采樣周期為0.1s；濾波器初始參數(shù)設(shè)置如下

P(0)=diag{(20m/Re)2,(20m/Re)2,(0.1m/s)2,

(0.1m/s)2,(0.1°)2,(0.1°)2,(0.1°)2}

Q(0)=diag{(100μg)2,(100μg)2,(0.01°/h)2,

(0.01°/h)2,(0.01°/h)2}

R(0)=diag{(0.1m/s)2,(0.1m/s)2}

式中Re為地球半徑，Re=6378393m。

3.1 量測(cè)噪聲協(xié)方差未知情況下仿真

由于缺乏相關(guān)的先驗(yàn)信息等因素導(dǎo)致無法得到實(shí)際量測(cè)噪聲的統(tǒng)計(jì)特性；針對(duì)量測(cè)噪聲協(xié)方差未知的情況，實(shí)驗(yàn)選取濾波器中量測(cè)噪聲協(xié)方差初值為R=0.01R(0)，濾波結(jié)果如圖1所示。

圖1 噪聲統(tǒng)計(jì)信息未知濾波性能

可以看出:在量測(cè)噪聲統(tǒng)計(jì)信息未知條件下，MLE自適應(yīng)濾波算法中速度估計(jì)誤差在前200s出現(xiàn)較大的波動(dòng)，這是由于濾波器中量測(cè)噪聲協(xié)方差初值設(shè)置不準(zhǔn)確，雖然經(jīng)過一段時(shí)間的自適應(yīng)調(diào)節(jié)后逐漸達(dá)到收斂，但受其影響，位置估計(jì)誤差不斷積累；而本文的新息自適應(yīng)濾波算法能夠利用新息協(xié)方差的估計(jì)值直接修正濾波增益矩陣，經(jīng)過較短時(shí)間的調(diào)節(jié)就能克服由量測(cè)噪聲不準(zhǔn)確引起的干擾，整個(gè)過程變化較為平穩(wěn)，位置估計(jì)誤差也較小。

3.2 量測(cè)噪聲協(xié)方差時(shí)變情況下仿真

受環(huán)境溫度、載體機(jī)動(dòng)等外界因素影響導(dǎo)致量測(cè)噪聲協(xié)方差不再保持常值，而是隨時(shí)間變化；為此，仿真實(shí)驗(yàn)設(shè)置了3組不同強(qiáng)度的量測(cè)噪聲：幅值在50～100s變?yōu)镽=40R(0)，在300～320s變?yōu)镽=20R(0)，其余時(shí)間段幅值R=R(0)，濾波結(jié)果如圖2所示。

圖2 噪聲統(tǒng)計(jì)信息時(shí)變?yōu)V波性能

可以看出:在量測(cè)噪聲時(shí)變情況下， MLE自適應(yīng)濾波的估計(jì)誤差波動(dòng)幅度較大：在50～150s處出現(xiàn)幅值較大的鋸齒狀速度估計(jì)誤差，而且位置誤差也出現(xiàn)較大幅值的波動(dòng)，緯度誤差在300s后顯著增大。對(duì)于本文的新息自適應(yīng)濾波，雖然在量測(cè)噪聲幅值變化時(shí)間段出現(xiàn)了一定的擾動(dòng)，但幅值較小，而且經(jīng)過很短時(shí)間的調(diào)節(jié)后便收斂于穩(wěn)態(tài)，這是因?yàn)槠浠谙薅ㄓ洃浿笖?shù)加權(quán)的新息協(xié)方差估值器能夠充分利用滑動(dòng)窗口內(nèi)新近新息協(xié)方差序列的作用，根據(jù)新近的量測(cè)信息及時(shí)地修正濾波增益矩陣，從而克服了由噪聲時(shí)變帶來的影響。

4 結(jié) 論

針對(duì)噪聲統(tǒng)計(jì)信息未知或者時(shí)變情況下，常規(guī)卡爾曼濾波估計(jì)精度下降甚至發(fā)散的問題，提出了一種基于極大似然估計(jì)的新息自適應(yīng)濾波算法。算法通過引入限定記憶指數(shù)衰減加權(quán)法來修正由基于極大似然估計(jì)得到的新息協(xié)方差估值器，利用新息協(xié)方差的指數(shù)加權(quán)值代替其算數(shù)平均值，增加滑動(dòng)窗口內(nèi)新近數(shù)據(jù)的權(quán)重，提高新息協(xié)方差估計(jì)值的準(zhǔn)確性。對(duì)SINS/GPS組合導(dǎo)航系統(tǒng)的仿真實(shí)驗(yàn)驗(yàn)證了本文算法具有更強(qiáng)的魯棒性和更高的濾波精度。

[1] Kwok C,Dev K,Seebauer E G,et al.Maximum a posteriori estimation of activation energies that control silicon self-diffu-sion[J].Automatica,2008,44(9):2241-2247.

[2] Karasalo M,Hu X.An optimization approach to adaptive Kalman filtering[J].Automatica,2011,47(8):1785-1793.

[3] Xiong K,Liu L D,Zhang H Y.Modified unscented Kalman filtering and its application in autonomous satellite navigation[J].Aerospace Science and Technology,2009,13(4):238-246.

[4] 王小旭，趙 琳.自適應(yīng)融合濾波算法及其在INS/GPS 組合導(dǎo)航中的應(yīng)用[J].宇航學(xué)報(bào),2010,31(11):2503-2511.

[5] Sage A P,Husa G W.Algorithms for sequential adaptive estimation of prior statistics[C]∥Proc of8th IEEE Symposium on Adaptive Processes,1969:61.

[6] 岳曉奎,袁建平.一種基于極大似然準(zhǔn)則的自適應(yīng)卡爾曼濾波算法[J].西北工業(yè)大學(xué)學(xué)報(bào),2005,23(4):469-474.

[7] Yang Y,Gao W.An optimal adaptive Kalman filter[J].Journal of Geodesy,2006,80(4):177-183.

[8] 張常云.自適應(yīng)濾波方法研究[J].航空學(xué)報(bào),1998,19(7):96-99.

[9] 徐定杰，賀 瑞，沈 峰,等.基于新息協(xié)方差的自適應(yīng)漸消卡爾曼濾波器[J]. 系統(tǒng)工程與電子技術(shù),2011,33(12):

2696-2699.

[10] Mohamed A H,Schwarz K P.Adaptive Kalman filtering for INS/GPS[J].Journal of Geodesy,1999,73(4):193-203.

[11] 卞鴻巍,金志華,王俊璞,等.組合導(dǎo)航系統(tǒng)新息自適應(yīng)卡爾曼濾波算法[J].上海交通大學(xué)學(xué)報(bào),2006,40(6):1000-1003.

[12] Gao W,Li J C.Adaptive Kalman filtering for the integrated SINS/DVL system[J].Journal of Computational Information Systems,2013,9(16):6443-6450.