一種改進(jìn)的無跡Kalman濾波在SINS/GPS組合導(dǎo)航中的應(yīng)用

陳國(guó)通，范圓圓，劉 琪

(河北科技大學(xué)信息科學(xué)與工程學(xué)院，石家莊 050018)

0 引言

隨著航空航天技術(shù)的發(fā)展，對(duì)導(dǎo)航與定位系統(tǒng)的性能要求越來越高，單一的導(dǎo)航系統(tǒng)已經(jīng)無法滿足工程實(shí)踐的需要，組合導(dǎo)航系統(tǒng)應(yīng)運(yùn)而生，并且得到了廣泛應(yīng)用[1]。應(yīng)用最廣泛的就是捷聯(lián)慣導(dǎo)系統(tǒng)(strap-down inertial navigation system, SINS)與全球定位系統(tǒng)(global position system, GPS)的組合[2]。SINS是一種自主式導(dǎo)航系統(tǒng)，不需要外部信息就可以完成導(dǎo)航，在導(dǎo)航過程中也不會(huì)向外部輻射能量，具有很好的隱蔽性，但隨著時(shí)間的推移會(huì)有誤差的積累[3]。GPS可以全天時(shí)、全天候工作，且定位精度高，導(dǎo)航誤差不會(huì)積累，但是易受干擾和建筑物遮擋[4]。這兩個(gè)導(dǎo)航系統(tǒng)可以優(yōu)勢(shì)互補(bǔ)，能夠提供非常完整的導(dǎo)航數(shù)據(jù)和精確的導(dǎo)航信息[5]。

1960年，R. E. Kalman提出了Kalman濾波算法[6]。但是它只適用于系統(tǒng)模型為線性，且過程噪聲和量測(cè)噪聲都是Gauss分布時(shí)的情況[7-8]。當(dāng)系統(tǒng)模型為非線性，且過程噪聲和量測(cè)噪聲都是非Gauss分布時(shí)，該方法就不再適用[9-11]。此時(shí)就出現(xiàn)了無跡Kalman濾波(Unscented Kalman Filtering， UKF)。本文針對(duì)無跡Kalman濾波導(dǎo)致濾波性能急劇下降和發(fā)散的問題，提出了基于抗差因子的無跡Kalman濾波。一方面通過擴(kuò)維，增加了系統(tǒng)的輸入信息，減小了噪聲對(duì)系統(tǒng)的影響；另一方面可以減小異常觀測(cè)量對(duì)狀態(tài)估計(jì)值的影響。

1 SINS/GPS組合導(dǎo)航數(shù)學(xué)模型

SINS選取東北天地理坐標(biāo)系n作為導(dǎo)航坐標(biāo)系，采用20維的狀態(tài)參數(shù)來建立系統(tǒng)狀態(tài)方程。SINS/GPS的狀態(tài)量為

(1)

SINS/GPS組合系統(tǒng)的狀態(tài)方程為

(2)

式中，X(t)為20維的系統(tǒng)狀態(tài)量，f(·)為狀態(tài)量的非線性函數(shù)，W(t)為20維的系統(tǒng)過程噪聲。

SINS/GPS組合系統(tǒng)的量測(cè)部分的觀測(cè)值，需要選取SINS與GPS的位置、速度之差。因此，系統(tǒng)的量測(cè)方程為

Z(t)=h(X(t))+V(t)

(3)

式中，h(t)表示偽距、偽距率的非線性觀測(cè)方程，V(t)表示偽距、偽距率的白噪聲。

2 無跡Kalman濾波算法

2.1 無跡變換

無跡變換的具體步驟如下：

(1)計(jì)算2n+1個(gè)采樣點(diǎn)和權(quán)值

(4)

(5)

(2)計(jì)算非線性映射得到的Sigma點(diǎn)

利用非線性映射h(·)，對(duì)采樣得到的每個(gè)Sigma點(diǎn)做非線性變換，得到點(diǎn)集yi

yi=hxii=0,1,…,2n

(6)

(7)

2.2 無跡Kalman濾波算法實(shí)現(xiàn)

無跡Kalman濾波是一種近似線性的最小方差估計(jì)方法，它的依據(jù)就是無跡變換，以經(jīng)典Kalman濾波算法為框架，采用確定性采樣來完成整個(gè)過程。UKF不需要對(duì)系統(tǒng)的狀態(tài)和量測(cè)方程提出任何要求，就可以計(jì)算出最佳增益陣，因而既適用于線性系統(tǒng)模型，也適用于非線性系統(tǒng)模型。其具體實(shí)現(xiàn)步驟如下：

(1)初始化

初始狀態(tài)和初始方差如式(8)和式(9)所示

(8)

(9)

將式(8)和式(9)擴(kuò)維得到

(10)

(11)

(2)Sigma點(diǎn)采樣

(12)

(13)

均值和協(xié)方差的權(quán)值如式(14)和式(15)所示

(14)

(15)

(3)時(shí)間更新

首先，根據(jù)狀態(tài)方程傳遞采樣點(diǎn)

(16)

接著，根據(jù)預(yù)測(cè)采樣點(diǎn)、均值和協(xié)方差的權(quán)值計(jì)算預(yù)測(cè)均值和協(xié)方差

(17)

(18)

預(yù)測(cè)測(cè)量值和協(xié)方差

(19)

最后，得到預(yù)測(cè)測(cè)量值和協(xié)方差

(20)

(21)

(22)

(4)計(jì)算UKF增益，更新狀態(tài)向量和方差

(23)

(24)

k+1|k

(25)

3 改進(jìn)的無跡Kalman濾波

傳統(tǒng)的無跡Kalman濾波根據(jù)估計(jì)量測(cè)方程和量測(cè)量的協(xié)方差矩陣來確定最佳增益，但在導(dǎo)航過程中會(huì)因?yàn)橥饨缫蛩氐母蓴_，無法得到準(zhǔn)確的量測(cè)信息，致使增益有所偏差，導(dǎo)致最后的濾波精度降低?；诖?，本文提出了一種改進(jìn)的無跡Kalman濾波方法。首先對(duì)新息(即預(yù)測(cè)殘差向量)的信息進(jìn)行觀察分析，判斷是否有異常的觀測(cè)量，并對(duì)異常的觀測(cè)量進(jìn)行修正，此修正過程就是通過引入抗差因子來完成的。

設(shè)定新息向量νk

(26)

式中，diag(·)表示求取方陣的對(duì)角線元素為列向量，m表示m維觀測(cè)向量，σ為一參數(shù)，根據(jù)反查標(biāo)準(zhǔn)正態(tài)分布表，確定其取值范圍應(yīng)為2.3～4.3。

當(dāng)k時(shí)刻m維的γm(k)全都小于0，則證明新息向量正常；若存在某一值使得γm(k)大于0，則k時(shí)刻的某行量測(cè)新息存在異常,此時(shí)需要引入抗差因子。本文采用的抗差因子是根據(jù)IGGⅡ型權(quán)函數(shù)生成的。

基于預(yù)測(cè)殘差νk構(gòu)建預(yù)測(cè)殘差判別統(tǒng)計(jì)量Δνk，表示為

(27)

IGGⅡ型權(quán)函數(shù)為

(28)

式(28)中，k0、k1均為常數(shù)[11]，取值范圍分別為1.0～1.5、2.5～8.0。根據(jù)式(28)生成的抗差因子函數(shù)為

(29)

將式(29)帶入式(26)得到

(30)

此時(shí)可以根據(jù)抗差因子函數(shù)來調(diào)整新息向量觀測(cè)到的異常值，使式(30)的結(jié)果不大于0，即γm(k)得到的結(jié)果全都不大于0。通過抗差因子調(diào)整后，得到的Sigma點(diǎn)集為

(31)

根據(jù)第2節(jié)無跡Kalman濾波過程，得到改進(jìn)的無跡Kalman狀態(tài)與方差估計(jì)為

(32)

(33)

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

為了證明改進(jìn)算法的有效性，本文設(shè)計(jì)了基于matlab的仿真實(shí)驗(yàn)，并將此算法應(yīng)用到SINS/GPS組合導(dǎo)航系統(tǒng)中進(jìn)行仿真。仿真實(shí)驗(yàn)中的各初始參數(shù)設(shè)定如表1所示。設(shè)運(yùn)載體初始所在位置為東經(jīng)114.26°，北緯38.03°，地球的自轉(zhuǎn)角速為ωie=7.292×10-5rad/s，初始速度為v=100m/s，仿真時(shí)間為1000s。

圖1和圖2分別為UKF估計(jì)、改進(jìn)的UKF估計(jì)和真實(shí)狀態(tài)比較，通過分析其狀態(tài)值和絕對(duì)偏差值，可以看出改進(jìn)的UKF更接近真實(shí)值。

表1 仿真參數(shù)

圖1 UKF/改進(jìn)的UKF狀態(tài)值估計(jì)Fig.1 UKF/ improved UKF state value estimation

圖2 UKF/改進(jìn)的UKF絕對(duì)偏差值估計(jì)Fig.2 UKF/improved UKF absolute bias value estimation

圖3～圖5為EKF、UKF和改進(jìn)的UKF在水平和垂直方向上的誤差曲線。分析比較可以看出，改進(jìn)的UKF誤差曲線更加平滑，更加收斂。

圖3 EKF水平/垂直方向誤差Fig.3 EKF horizontal and vertical errors

圖4 UKF水平/垂直方向誤差Fig.4 UKF horizontal and vertical error

圖5 改進(jìn)的UKF水平/垂直方向誤差Fig.5 Improved UKF horizontal and vertical error

本文采用的是東北天坐標(biāo)系，所以對(duì)東向速度誤差和北向速度誤差進(jìn)行分析，如圖6和圖7所示。可以看出3種算法得到的速度誤差的濾波精度逐漸提高。EFK算法得到的速度誤差在(-0.5m/s，0.5m/s)以內(nèi)，UKF算法得到的速度誤差在(-0.3m/s，0.3m/s)以內(nèi)，改進(jìn)的UKF算法得到的速度誤差在(-0.1m/s，0.1m/s)以內(nèi)。

圖6 東向速度誤差曲線Fig.6 Eastward velocity error curve

圖7 北向速度誤差曲線Fig.7 North direction velocity error curve

5 結(jié)論

傳統(tǒng)的無跡Kalman濾波在非線性系統(tǒng)中能夠很好地提高魯棒特性和收斂速度，但是在觀測(cè)過程中容易出現(xiàn)量測(cè)新息不規(guī)則的情況。因此，本文提出了一種改進(jìn)的無跡Kalman濾波方法，一方面根據(jù)新息向量的概率特性，能夠及時(shí)發(fā)現(xiàn)異常信息；另一方面通過引入抗差因子來對(duì)異常信息進(jìn)行修正。通過仿真實(shí)驗(yàn)證明，本文提出的改進(jìn)算法能夠提高組合導(dǎo)航系統(tǒng)的濾波解算精度，對(duì)SINS/GPS組合導(dǎo)航的研究具有一定的參考價(jià)值。