改進(jìn)的CKF在SINS/GPS組合導(dǎo)航中的應(yīng)用

黃春梅，　王　杰，　鄒慧瑩

(長春工業(yè)大學(xué) 計算機(jī)科學(xué)與工程學(xué)院， 吉林 長春　130012)

?

改進(jìn)的CKF在SINS/GPS組合導(dǎo)航中的應(yīng)用

黃春梅，王杰，鄒慧瑩

(長春工業(yè)大學(xué) 計算機(jī)科學(xué)與工程學(xué)院， 吉林 長春130012)

摘要：為了降低動態(tài)擾動誤差對容積卡爾曼濾波(Cubature Kalman Filter, CKF)精度的影響，采用新息自適應(yīng)估計(Innovation-based Adaptive Estimation, IAE)理論構(gòu)造改進(jìn)的CKF濾波模型，利用新息序列在線估計和修正噪聲統(tǒng)計特性，自適應(yīng)地調(diào)整量測噪聲。通過SINS/GPS組合導(dǎo)航系統(tǒng)對標(biāo)準(zhǔn)CKF與改進(jìn)的CKF進(jìn)行仿真驗證，結(jié)果表明,改進(jìn)的CKF算法能夠在一定程度上提高組合導(dǎo)航系統(tǒng)對不同隨機(jī)噪聲的適應(yīng)能力，有效地降低了濾波誤差，提高了解算精度。

關(guān)鍵詞：容積卡爾曼濾波； SINS/GPS組合導(dǎo)航； 非線性系統(tǒng)； 自適應(yīng)濾波

0引言

在組合導(dǎo)航系統(tǒng)中，提高導(dǎo)航定位的解算精度是導(dǎo)航領(lǐng)域的關(guān)鍵問題。由于SINS/GPS組合導(dǎo)航系統(tǒng)的狀態(tài)方程在直接濾波算法中具有非線性形式，所以常規(guī)的卡爾曼濾波算法已不再適用[1]。因此，擴(kuò)展卡爾曼濾波(Extend Kalman Filter, EKF)、無跡卡爾曼濾波(Unscented Kalman Filter, UKF)、粒子濾波(Particle Filter, PF)等非線性濾波方法被相繼提出。EKF算法實現(xiàn)簡單、應(yīng)用廣泛，但是計算量大，且濾波結(jié)果易發(fā)散。UKF采用UT變換，克服了EKF的局限性，但在高維非線性系統(tǒng)中，缺乏對異常擾動的自適應(yīng)調(diào)節(jié)能力[2]。CKF將基于Bayesian理論的非線性濾波歸結(jié)于求解非線性函數(shù)與高斯概率密度乘積的積分問題，相比EKF、UKF濾波方法，CKF具有更好的濾波性能，適用于SINS/GPS組合導(dǎo)航非線性濾波的要求[3]。

CKF是在狀態(tài)變量和量測變量符合相互獨立的高斯分布前提下得到的，它導(dǎo)出了狀態(tài)變量的預(yù)測密度函數(shù)和濾波密度函數(shù)。根據(jù)三階球面-相徑容積規(guī)則確定的均值、方差是對非線性系統(tǒng)狀態(tài)的最佳估計[4],其估計精度高、不容易發(fā)散且計算量小，使得CKF的應(yīng)用越來越廣泛。由于在濾波過程中存在異常擾動問題，為了降低動態(tài)異常擾動誤差對CKF濾波精度的影響，文中嘗試?yán)米赃m應(yīng)算法調(diào)整量測噪聲，通過仿真結(jié)果分析，改進(jìn)的CKF算法能夠消除異常誤差的出現(xiàn)，改善了濾波精度及可靠性。

1SINS/GPS緊組合導(dǎo)航模型

緊組合導(dǎo)航模型將捷聯(lián)慣導(dǎo)系統(tǒng)和GPS接收機(jī)進(jìn)行一體化設(shè)計，主要由SINS系統(tǒng)、GPS系統(tǒng)和組合導(dǎo)航濾波器3部分組成，SINS與GPS兩系統(tǒng)相互輔助[5-6]，如圖1所示。

圖1　SINS/GPS緊組合系統(tǒng)結(jié)構(gòu)框圖

1.1緊組合導(dǎo)航系統(tǒng)狀態(tài)方程

對于SINS/GPS緊組合導(dǎo)航模型，系統(tǒng)狀態(tài)方程建立如下：

加速度計誤差▽E、▽N、▽U主要含有零位偏置▽AE、▽AN、▽AU和白噪聲ωAE、ωAN、ωAU，即

(1)

可以得到濾波器的狀態(tài)方程為：

(2)

其中，狀態(tài)向量X含有9個狀態(tài)

(3)

W(t)為噪聲向量：

(4)

系數(shù)矩陣F為:

(5)

其中Fb(6×6)為慣導(dǎo)誤差方程式系數(shù)所確定的矩陣，F(xiàn)s(6×3)、FG(3×6)分別為

(6)

(7)

1.2緊組合導(dǎo)航系統(tǒng)量測方程

假設(shè)Lt、λt、ht、VSEt、VSNt、VSUt為載體的真實位置和速度，以慣導(dǎo)的定位誤差和GPS的定位誤差作為濾波器的輸入量，定義量測向量為

(8)

其中

(9)

(10)

2標(biāo)準(zhǔn)CKF

CKF算法通過選取2n個相同權(quán)值的容積點來計算推演出下一時刻系統(tǒng)狀態(tài)的濾波估計。采用三階球面-相徑容積規(guī)則來計算高斯加權(quán)積分，而貝葉斯濾波簡化為求解非線性函數(shù)與高斯概率密度乘積的積分問題。

針對離散非線性狀態(tài)空間模型：

(11)

xk∈Rn×1，zk∈Rm×1

vk-1～N(0,Qk),wk-1～N(0,Rk)

1)分解

(12)

2)形成容積點

(13)

(14)

其中，ei是單位列矢量的第i元，唯獨它等于1，其余元素等于0。

3)計算傳播的容積點

(15)

4)計算預(yù)測的狀態(tài)

(16)

5)計算預(yù)測的狀態(tài)誤差矩陣

(17)

量測更新過程如下：

1)分解

(18)

2)形成容積點

(19)

3)計算傳播的容積點

(20)

4)計算測量的預(yù)測值

(21)

5)計算新的量測誤差協(xié)方差矩陣

(22)

6)計算互協(xié)方差矩陣

(23)

7)計算濾波增益

(24)

8)計算濾波狀態(tài)

(25)

9)計算濾波后的協(xié)方差矩陣

(26)

3改進(jìn)的CKF算法

基于新息自適應(yīng)估計算法，以新息序列協(xié)方差匹配原理作為依據(jù)，將卡爾曼濾波器參數(shù)進(jìn)行自適應(yīng)調(diào)整[7-8]。用新息序列的統(tǒng)計特性來對濾波器的工作狀態(tài)進(jìn)行檢測，當(dāng)濾波器工作正常時，新息序列將是零均值的高斯白噪聲，但是當(dāng)濾波器工作不正常時，新息序列的統(tǒng)計就不滿足上述條件。正是基于這種思想，IAE方法要通過對量測新息統(tǒng)計的計算，自適應(yīng)的調(diào)整模型中的噪聲協(xié)方差陣。

假設(shè)估計窗口長度為m,tk時刻的新息序列為：

(27)

由IAE方法計算新息序列的協(xié)方差矩陣為：

(28)

利用tk-1時刻之前m個時刻的信息進(jìn)行求解，可避免利用t時刻新信息值，則

(29)

隨著系統(tǒng)的不斷運行，量測噪聲方差陣被自適應(yīng)的調(diào)整，系統(tǒng)模型的濾波效果更符合實際情況。

4仿真實驗及結(jié)果分析

對含有SINS/GPS組合導(dǎo)航系統(tǒng)的飛行器分別采用CKF算法和改進(jìn)的CKF算法進(jìn)行導(dǎo)航計算[9]。設(shè)定運載器工作的初始位置為: 東經(jīng)125°，北緯43°。飛行器初始狀態(tài)為:東、北、天三方向速度的初始偏差均為0，東向加速度初始偏差為1.0×10-4m/s2；北向加速度初始偏差為0；天向加速度初始偏差為0。三方向速度的測量誤差均為0.01 m/s。隨機(jī)偏差為1.0×10-6，刻度誤差為0.000 1，經(jīng)度λ、緯度L、高度h的初始誤差量分別為1′、1′、1 m。SINS與GPS的采樣周期分別為0.01、1 s,仿真時間為200 s。

標(biāo)準(zhǔn) CKF和文中提出的改進(jìn)的CKF計算得到的定位誤差如圖2～圖5所示。

圖2　CKF速度誤差仿真圖

圖3　改進(jìn)的CKF速度誤差仿真圖

圖4　CKF位置誤差仿真圖

圖5　改進(jìn)的CKF位置誤差仿真圖

具體實驗對比數(shù)據(jù)見表1和表2。

表1　兩種算法的速度誤差實驗仿真數(shù)據(jù)比較

表2　兩種算法的位置誤差實驗仿真數(shù)據(jù)比較

從濾波輸出結(jié)果中可以看出，改進(jìn)的CKF算法可以自適應(yīng)地更新量測噪聲的統(tǒng)計特性，能夠在一定程度上抑制異常量測對濾波精度的影響，因而得到的定位精度比標(biāo)準(zhǔn)CKF更高。由表1和表2可以看出，改進(jìn)的CKF的速度誤差與位置誤差均明顯小于標(biāo)準(zhǔn)的CKF，表明當(dāng)噪聲統(tǒng)計不準(zhǔn)確時，改進(jìn)的CKF濾波算法可以有效地提高濾波精度，減小SINS/GPS組合導(dǎo)航非線性濾波的定位誤差。

5結(jié)語

為了提高組合導(dǎo)航系統(tǒng)的濾波解算精度，基于IAE自適應(yīng)估計原理提出一種改進(jìn)的CKF濾波算法。該算法能夠在線地估計和調(diào)整量測噪聲的統(tǒng)計特性，使系統(tǒng)模型更加符合實際情況，提高了CKF的自適應(yīng)能力。

實驗結(jié)果表明，當(dāng)導(dǎo)航系統(tǒng)噪聲統(tǒng)計不準(zhǔn)確時，改進(jìn)的CKF濾波性能明顯優(yōu)于標(biāo)準(zhǔn)CKF濾波，可以有效地提高SINS/GPS組合導(dǎo)航系統(tǒng)的定位精度和可靠性。

參考文獻(xiàn)：

[1]李建文,郝順義.高動態(tài)GPS/SINS組合導(dǎo)航精確定位方法研究[J].信息與控制，2009，38(3)：304-308.

[2]黃春梅,尹新.GPS/SINS組合導(dǎo)航系統(tǒng)中自適應(yīng)濾波器[J].長春工業(yè)大學(xué)學(xué)報,2015，36(6):687-691.

[3]徐慧娟.EKF與UKF在INS/GPS組合導(dǎo)航中的應(yīng)用分析[J].航天控制，2006，24(6):7-10.

[4]葛磊.容積卡爾曼濾波算法研究及其在導(dǎo)航中的應(yīng)用[D].哈爾濱:哈爾濱工程大學(xué)，2013.

[5]唐季軍.Cubature卡爾曼濾波及其在導(dǎo)航中的應(yīng)用研究[D].哈爾濱:哈爾濱工程大學(xué)，2012.

[6]K Pakki, B Chandra, D W Gu. Cubature inforamtion filter and its applications[C]//Proceeding of the 2011 American Control Conference,2011.

[8]張靈利.捷聯(lián)慣導(dǎo)兩種姿態(tài)算法抗干擾性比較[J].長春工業(yè)大學(xué)學(xué)報:自然科學(xué)版,2012,33(6):30-35.

[9]Xiaoyu Zhan， Chunlei Song. Research and design of MEMS SINS/GPS ntegrated navigation system based on adaptive filter [J].Proceedings of the 2nd International Conference on Systems Engineering and Modeling，2013(1):229-233.

Improved CKF application in SINS/GPS integrated navigation system

HUANG Chunmei,WANG Jie,ZOU Huiying

(Chool of Computer Science & Engineering, Changchun University of Technology, Changchun 130012， China)

Abstract：An improved Cubature Kalman Filter (CKF) model using Innovation-based Adaptive Estimation (IAE) is implemented to reduce abnormal dynamic error of CKF. The innovation information is used to on-line estimate and correct noise statistical characteristics, automatically adjust measurement. Both the CKF and improved CKF are applied to the SINS/GPS integrated navigation system, and the simulation results show that the improved CKF algorithm can improve the adaptability to different stochastic disturbances, decrease filtering error and improve the calculation accuracy.

Key words：Cubature Kalman Filter (CKF); SINS/GPS integrated navigation; nonlinear system; adaptive filtering.

收稿日期：2016-01-07

作者簡介：黃春梅(1965-)，女，漢族，吉林長春人，長春工業(yè)大學(xué)副教授，博士，主要從事嵌入式控制方向研究,E-mail:huangchunmei@ccut.edu.cn.

DOI:10.15923/j.cnki.cn22-1382/t.2016.2.07

中圖分類號：TP 273

文獻(xiàn)標(biāo)志碼：A

文章編號：1674-1374(2016)02-0134-07