一種改進的無跡Kalman濾波在SINS/GPS組合導航中的應用

陳國通，范圓圓，劉 琪

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

0 引言

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

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

1 SINS/GPS組合導航數學模型

SINS選取東北天地理坐標系n作為導航坐標系，采用20維的狀態參數來建立系統狀態方程。SINS/GPS的狀態量為

(1)

SINS/GPS組合系統的狀態方程為

(2)

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

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

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

(3)

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

2 無跡Kalman濾波算法

2.1 無跡變換

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

(1)計算2n+1個采樣點和權值

(4)

(5)

(2)計算非線性映射得到的Sigma點

利用非線性映射h(·)，對采樣得到的每個Sigma點做非線性變換，得到點集yi

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

(6)

(7)

2.2 無跡Kalman濾波算法實現

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

(1)初始化

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

(8)

(9)

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

(10)

(11)

(2)Sigma點采樣

(12)

(13)

均值和協方差的權值如式(14)和式(15)所示

(14)

(15)

(3)時間更新

首先，根據狀態方程傳遞采樣點

(16)

接著，根據預測采樣點、均值和協方差的權值計算預測均值和協方差

(17)

(18)

預測測量值和協方差

(19)

最后，得到預測測量值和協方差

(20)

(21)

(22)

(4)計算UKF增益，更新狀態向量和方差

(23)

(24)

k+1|k

(25)

3 改進的無跡Kalman濾波

傳統的無跡Kalman濾波根據估計量測方程和量測量的協方差矩陣來確定最佳增益，但在導航過程中會因為外界因素的干擾，無法得到準確的量測信息，致使增益有所偏差，導致最后的濾波精度降低。基于此，本文提出了一種改進的無跡Kalman濾波方法。首先對新息(即預測殘差向量)的信息進行觀察分析，判斷是否有異常的觀測量，并對異常的觀測量進行修正，此修正過程就是通過引入抗差因子來完成的。

設定新息向量νk

(26)

式中，diag(·)表示求取方陣的對角線元素為列向量，m表示m維觀測向量，σ為一參數，根據反查標準正態分布表，確定其取值范圍應為2.3～4.3。

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

基于預測殘差νk構建預測殘差判別統計量Δνk，表示為

(27)

IGGⅡ型權函數為

(28)

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

(29)

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

(30)

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

(31)

根據第2節無跡Kalman濾波過程，得到改進的無跡Kalman狀態與方差估計為

(32)

(33)

4 仿真與結果分析

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

圖1和圖2分別為UKF估計、改進的UKF估計和真實狀態比較，通過分析其狀態值和絕對偏差值，可以看出改進的UKF更接近真實值。

表1 仿真參數

圖1 UKF/改進的UKF狀態值估計Fig.1 UKF/ improved UKF state value estimation

圖2 UKF/改進的UKF絕對偏差值估計Fig.2 UKF/improved UKF absolute bias value estimation

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

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

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

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

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

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

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

5 結論

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