楊宗舉， 黃國勇

(1．昆明理工大學(xué) 信息工程與自動化學(xué)院，云南 昆明 650500；2．復(fù)雜工業(yè)過程檢測、控制與優(yōu)化重點實驗室，云南 昆明 650500)

?

改進(jìn)的CKF算法及其在BDS/INS中的應(yīng)用*

楊宗舉1,2， 黃國勇1,2

(1．昆明理工大學(xué) 信息工程與自動化學(xué)院，云南 昆明 650500；2．復(fù)雜工業(yè)過程檢測、控制與優(yōu)化重點實驗室，云南 昆明 650500)

針對標(biāo)準(zhǔn)容積卡爾曼濾波(CKF)在組合導(dǎo)航系統(tǒng)模型不確定情況下濾波精度下降甚至發(fā)散的問題，將奇異值分解(SVD)與CKF算法相結(jié)合，并引入強(qiáng)跟蹤濾波(STF)理論，提出一種改進(jìn)的強(qiáng)跟蹤SVD—CKF算法。為提高數(shù)值計算的穩(wěn)定性，采用SVD代替標(biāo)準(zhǔn)CKF中的Cholesky分解；引入STF理論框架，通過漸消因子對預(yù)測誤差協(xié)方差陣進(jìn)行在線修正，在系統(tǒng)模型不確定或系統(tǒng)發(fā)生大的突變時，能夠提高系統(tǒng)的強(qiáng)魯棒性。通過仿真結(jié)果驗證了改進(jìn)算法的有效性。

容積卡爾曼濾波； 奇異值分解； 強(qiáng)跟蹤濾波； 漸消因子； 組合導(dǎo)航

0 引 言

非線性系統(tǒng)狀態(tài)估計在慣性導(dǎo)航、制導(dǎo)系統(tǒng)、目標(biāo)跟蹤、信號處理等領(lǐng)域具有重要的應(yīng)用，北斗導(dǎo)航定位系統(tǒng)(BDS)和慣性導(dǎo)航系統(tǒng)(INS)的組合導(dǎo)航系統(tǒng)采用的濾波方法主要是擴(kuò)展?fàn)柭鼮V波(extended Kalman filtering,EKF)[1]和無跡卡爾曼濾波(unscented Kalman filtering,UKF)[2,3]。文獻(xiàn)[4]采用一組等權(quán)值的容積點集來解決Bayesian濾波積分問題，提出了容積卡爾曼濾波(cubature Kalman filtering,CKF)算法。在濾波精度方面，CKF相比于EKF具有更高的精度，有效克服了EKF的理論局限性[1,2]。由于CKF中各積分點權(quán)值相同且為正數(shù)，因此其數(shù)值穩(wěn)定性優(yōu)于UKF[5]，更適用于解決高維組合導(dǎo)航系統(tǒng)的狀態(tài)估計問題。然而 CKF在運(yùn)算過程中會因為計算的誤差，導(dǎo)致協(xié)方差矩陣為負(fù)，導(dǎo)致系統(tǒng)出現(xiàn)發(fā)散問題；且系統(tǒng)在強(qiáng)非線性狀態(tài)下，由于系統(tǒng)模型及噪聲統(tǒng)計特性的不確定性，也會對濾波效果產(chǎn)生嚴(yán)重影響，導(dǎo)致濾波精度降低。

針對CKF存在濾波發(fā)散的問題，許多學(xué)者進(jìn)行了深入的研究。其中，文獻(xiàn)[6]在標(biāo)準(zhǔn)CKF的基礎(chǔ)上，引入了矩陣的QR分解，采用平方根迭代思想，結(jié)合強(qiáng)跟蹤濾波(strong tracking filtering,STF)的理論框架，提出了一種強(qiáng)跟蹤自適應(yīng)平方根CKF算法，有效改善了系統(tǒng)的濾波性能；文獻(xiàn)[7]中提出采用奇異值分解(singular value decomposition,SVD)的方法代替Cholesky分解，能夠提高數(shù)值計算的穩(wěn)定性。當(dāng)系統(tǒng)發(fā)生較大突變或系統(tǒng)模型的不確定性，CKF沒有較好的自適應(yīng)能力;文獻(xiàn)[8]中在CKF算法中引入STF框架，通過漸消因子在線調(diào)整增益矩陣，可以自適應(yīng)減少估計偏差，具有較強(qiáng)的魯棒性。

本文提出一種改進(jìn)的強(qiáng)跟蹤SVD-CKF算法，以CKF為框架，利用SVD代替CKF中的Cholesky分解，保證其算法的穩(wěn)定性。同時引入STF理論，能有效抑制濾波的發(fā)散，進(jìn)一步提高濾波器的穩(wěn)定性和自適應(yīng)能力。

1 改進(jìn)的SVD-CKF算法

對于組合導(dǎo)航系統(tǒng)來說，為了方便其計算，可以對CKF算法的觀測方程進(jìn)行適當(dāng)?shù)暮喕紤]非線性離散系統(tǒng)

(1)

式中xk+1∈Rn為狀態(tài)向量，zk+1∈Rm為量測向量，f(·)為非線性函數(shù)，Hk+1為線性量測陣,wk和vk+1分別為互不相關(guān)的高斯白噪聲。

為了方便計算，根據(jù)式(1)，可對標(biāo)準(zhǔn)CKF算法進(jìn)行簡化，如下

(2)

(3)

(4)

(5)

(6)

Pk+1=[I-Kk+1Hk+1]Pk+1/k

(7)

Qk為非負(fù)定矩陣，Rk為正定矩陣，且相互獨立。

1.1 改進(jìn)的SVD-CKF算法

CKF算法進(jìn)行多次濾波后，會因為計算舍入等誤差容易導(dǎo)致協(xié)方差失去正定性，導(dǎo)致濾波發(fā)散。為了解決其發(fā)散的問題，將SVD引入CKF算法中，構(gòu)成SVD-CKF算法。

1.1.1SVD

在數(shù)值代數(shù)計算中，為保證其穩(wěn)定性和精度，可以采用SVD的方法，該方法在計算機(jī)上易于實現(xiàn)[9]。定義如下：

假定A∈Rm×n(m≥n),則矩陣A的奇異值分解可表示為

(8)

式中U∈Rm×n；Λ∈Rm×n；V∈Rn×n；S=diag(s1,s2,…,sr)，s1≥s2≥…≥sr≥0，為矩陣A的奇異值；U的列向量是矩陣A的左奇異向量，V的列向量為矩陣A的右奇異向量。

當(dāng)矩陣A為對稱正定矩陣時，式(8)可簡化為

(9)

1.1.2 改進(jìn)的SVD-CKF算法

基于SVD-CKF算法步驟如下：

1)計算容積采樣點和對應(yīng)的權(quán)值[7]

本文使用三階球面徑向容積準(zhǔn)則獲得如下的基本容積點和相應(yīng)權(quán)值

(10)

(11)

式中 i=1,2,…，m,m為容積采樣點個數(shù)，根據(jù)三階球面徑向容積準(zhǔn)則，容積采樣點的總數(shù)是狀態(tài)維數(shù)的2倍，即m=2n；n為系統(tǒng)狀態(tài)維數(shù)；[1]i表示點集[]中的第i個點，其中,符號表示完整全對稱點集，具體含義是對n維單位向量e=[1,0,…,0]T的元素進(jìn)行全排列和改變元素符號所產(chǎn)生的點集。

2)時間更新

利用SVD對CKF的協(xié)方差矩陣Pk取平方根，即

(12)

式中S為對角矩陣，S=diag(s1,s2,…,sr)經(jīng)過協(xié)方差矩陣的SVD為

(13)

(14)

計算通過非線性狀態(tài)方程傳播的容積點

(15)

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

(16)

計算狀態(tài)預(yù)測協(xié)方差陣

(17)

3)簡化的CKF算法量測更新

計算量測預(yù)測值

(18)

計算新息方差陣

(19)

計算互協(xié)方差陣

(20)

計算增益矩陣

(21)

計算狀態(tài)更新值

(22)

計算系統(tǒng)狀態(tài)協(xié)方差陣

Pk+1=[I-Kk+1Hk+1]Pk+1/k

(23)

式(10)～式(23)即為基于SVD的容積卡爾曼濾波。

2 改進(jìn)的強(qiáng)跟蹤SVD-CKF濾波算法

相比于CKF算法，改進(jìn)的SVD-CKF算法具有很強(qiáng)的矩陣分解能力，且能夠保證迭代的穩(wěn)定性，但是改進(jìn)的SVD-CKF本身具有局限性，沒有較好的自適應(yīng)能力，當(dāng)系統(tǒng)發(fā)生大的突變，或者系統(tǒng)模型不精確時，可能會引起濾波器的發(fā)散。因此，為了抑制其發(fā)散，將多重漸消因子引入該算法中，構(gòu)成改進(jìn)的強(qiáng)跟蹤的SVD-CKF算法，改進(jìn)的算法的優(yōu)勢是誤差協(xié)方差能夠保證在濾波過程中的(半)正定性，亦能在系統(tǒng)發(fā)生大的突變時對濾波器發(fā)散進(jìn)行抑制，實現(xiàn)對真實狀態(tài)的強(qiáng)跟蹤。

2.1 STF原理

STF通過對預(yù)測協(xié)方差矩陣引入漸消因子，在線實時調(diào)整增益矩陣，使得任意時刻的新息殘差序列保持正交[10]，即

(24)

(25)

引入漸消因子后的的預(yù)測協(xié)方差矩陣如下

Qk

(26)

2.2 改進(jìn)的強(qiáng)跟蹤SVD-CKF濾波算法

本文在SVD-CKF的基礎(chǔ)上，在STF框架上引入多重漸消因子，保證系統(tǒng)的穩(wěn)定性。

引入多重漸消因子，如下

Δk+1=diag(λ1,k+1,λ2,k+1,…,λn,k+1)

(27)

將多重漸消因子Δk+1引入預(yù)測協(xié)方差矩陣Pk+1/k，其公式如下

(28)

但在STF框架下的SVD-CKF算法，在計算漸消因子的時候，需要計算狀態(tài)轉(zhuǎn)移方程的Jacobian矩陣，導(dǎo)致濾波系統(tǒng)的計算量增大。

(29)

(30)

將等價的漸消因子計算方法帶入強(qiáng)跟蹤SVD-CKF濾波中，則強(qiáng)跟蹤SVD-CKF濾波的漸消因子Δk+1計算公式如下

(31)

Ck+1,i=tr[Nk+1]/tr[Mk+1]

(32)

(33)

則式(33)可改寫為

(34)

(35)

(36)

新息序列中ρ為遺忘因子，降低舊數(shù)據(jù)對新息序列的影響，通常ρ取為0.95，κ≥1為弱化因子，一般情況靠經(jīng)驗選取。

針對線性量測方程的組合導(dǎo)航系統(tǒng)采用改進(jìn)的強(qiáng)跟蹤SVD-CKF算法，首先對CKF算法進(jìn)行簡化，為了保證數(shù)值計算的穩(wěn)定性和優(yōu)化協(xié)方差迭代的過程，將SVD引入CKF，同時為了解決在某些極端情況下CKF發(fā)散的問題；將STF框架引入改進(jìn)的SVD-CKF算法中，構(gòu)成強(qiáng)跟蹤SVD-CKF算法，當(dāng)系統(tǒng)狀態(tài)發(fā)生大的突變時，能夠?qū)嶋H狀態(tài)進(jìn)行強(qiáng)跟蹤，使系統(tǒng)具有較強(qiáng)的魯棒性，且改進(jìn)的強(qiáng)跟蹤SVD-CKF算法并不需要計算泰勒級數(shù)展開式來實現(xiàn)強(qiáng)跟蹤框架，能夠降低算法的復(fù)雜度，提高算法的穩(wěn)定性和精度。

3 仿真驗證

為了表明提出的算法的優(yōu)越性，在BDS/INS系統(tǒng)進(jìn)行仿真驗證，并與標(biāo)準(zhǔn)的CKF算法進(jìn)行對比驗證。根據(jù)文獻(xiàn)[12]中的系統(tǒng)模型進(jìn)行仿真。

假設(shè)飛機(jī)做機(jī)動飛行，飛機(jī)的初始位置為東經(jīng)103.003°，北緯32.719°，高度320 m；初始速度為40 m/s，方向正北。設(shè)INS采樣周期為0.02 s，BDS采樣周期0.1 s，濾波周期0.1 s，仿真時間1 800 s。INS系統(tǒng)采用了中等精度的陀螺儀和加速度計，陀螺的常值漂移為0.1°/h，加速度計常值誤差1.0～4gn。INS初始水平位置誤差50 m，高度誤差為100 m；初始速度誤差0.5 m/s。BDS水平位置誤差均方根10 m，高度誤差均方根20 m，速度誤差均方根0.1 m/s。

仿真過程中以慣性導(dǎo)航為主系統(tǒng)，對慣導(dǎo)系統(tǒng)進(jìn)行閉環(huán)修正，采用標(biāo)準(zhǔn)CKF算法和改進(jìn)的強(qiáng)跟蹤SVD-CKF算法進(jìn)行仿真。

組合導(dǎo)航系統(tǒng)的狀態(tài)向量X(t)選取如式(37)

X(t)=[φE,φN,φU,VE,VN,VU,λ,L,h,εbx,εby,εbz]

(37)

式中 φE,φN,φU為INS輸出的姿態(tài)角，VE,VN,VU為速度，L,λ,h為位置，εbx,εby,εbz為陀螺隨機(jī)漂移。組合導(dǎo)航系統(tǒng)的狀態(tài)方程如式(38)

(38)

式中f(·)為非線性函數(shù)，由捷聯(lián)慣導(dǎo)力學(xué)編排方程和姿態(tài)誤差方程得到，w(t)為系統(tǒng)噪聲。

系統(tǒng)的量測向量z為BDS輸出的速度VEB,VNB,VUB和位置λB,LB,hB如式(39)

z=[VEB,VNB,VUB,LB,λB,hB]

(39)

BDS/INS組合的量測方程如下

zk+1=Hk+1xk+1+vk+1

(40)

式中 Hk+1=[I6×6,O6×6],vk+1為量測噪聲。

對兩種濾波算法進(jìn)行解算。圖1為飛機(jī)理想飛行航跡圖。

圖1 飛機(jī)理想飛行航跡Fig 1 Ideal flight trajectory of aircraft

圖2為兩種算法對比仿真解果。

圖2 兩種算法位置誤差比較Fig 2 Comparison of position error of two algorithms

圖3 兩種算法姿態(tài)角誤差比較Fig 3 Comparison of attitude angle error of two algorith ms

圖4 兩種算法速度誤差比較Fig 4 Comparison of two algorithms for speed error

從仿真結(jié)果可知，對比兩種算法，本文提出的改進(jìn)的強(qiáng)跟蹤SVD-CKF算法比標(biāo)準(zhǔn)CKF算法具有較高的精度。從圖3中可以看出，兩種算法在姿態(tài)角的比較上，改進(jìn)的算法具有明顯優(yōu)勢，系統(tǒng)在658～728s時間內(nèi)由于出現(xiàn)大的轉(zhuǎn)彎加爬坡的強(qiáng)非線性飛行狀態(tài)，對于系統(tǒng)的真實狀態(tài)，CKF算法并不能及時跟蹤，導(dǎo)致出現(xiàn)較大幅度的波動，對比改進(jìn)的算法，則不難看出，在此時間段內(nèi)，采用改進(jìn)算法的姿態(tài)角度誤差較小，依然保持平穩(wěn)的狀態(tài)，在系統(tǒng)發(fā)生突變狀況時，改進(jìn)的算法在強(qiáng)跟蹤能力上優(yōu)于CKF算法；有效提高了濾波的精度。從圖2中可以看出，在50～850s時間段內(nèi)，CKF算法得到的天向位置誤差[-5.5，4.4]m，北向位置誤差在[-6，4.4]m內(nèi)，東向誤差在[-15.7，12.5]m內(nèi)；而本文提出的改進(jìn)算法得到的天向位置誤差[-4.1，3.2]m,北向位置誤差在[-4.1，2.9]m內(nèi)，東向誤差在[-11，5.5]m內(nèi)。通過在位置上的比較來看，改進(jìn)的算法和標(biāo)準(zhǔn)的算法相差不是太大，但是改進(jìn)的算法的精度略高。對于速度誤差來說，從圖4中可以看出，改進(jìn)算法對于標(biāo)準(zhǔn)算法精度略高，但是優(yōu)勢并不明顯。

4 結(jié)束語

1)當(dāng)系統(tǒng)發(fā)生較大突變或者系統(tǒng)模型不確定時，改進(jìn)的強(qiáng)跟蹤SVD-CKF算法能夠提高組合導(dǎo)航的精度和穩(wěn)定性。該算法在簡化的CKF算法基礎(chǔ)上引入SVD，能夠提高協(xié)方差迭代過程的穩(wěn)定性，且能夠保證數(shù)值計算的穩(wěn)定性；同時為了保證SVD-CKF在強(qiáng)非線性或者噪聲統(tǒng)計不明確下，系統(tǒng)具有較強(qiáng)的跟蹤能力，在此基礎(chǔ)上引如了STF框架，保證系統(tǒng)具有很強(qiáng)的魯棒性。

2)改進(jìn)的強(qiáng)跟蹤SVD-CKF算法優(yōu)于標(biāo)準(zhǔn)CKF算法，改進(jìn)算法能夠提高濾波器的穩(wěn)定性，能夠有效提高組合導(dǎo)航的定位精度。

[1] 王 堅,劉 超,高井祥,等.基于抗差EKF的GNSS/INS緊組合算法研究[J].武漢大學(xué)學(xué)報,2011,36(5):596-600.

[2] 王小旭,潘 泉,黃 鶴,等.非線性系統(tǒng)確定采樣型濾波算法綜述[J].控制與決策,2012,27(6):801-812.

[3]JulierSJ,UhlmannJK,Durrant-WhyteHF.Anewmethodforthenonlineartransformationofmeansandcovariances[J].IEEETransactionsonAutomaticControl,2000,45(3):477-482.

[4]ArasaratnamI,HaykinS.CubatureKalmanfilters[J].IEEETransactionsonAutomaticControl,2009,54(6):1254-1269.

[5] 崔乃剛,張 龍,王小剛,等.自適應(yīng)高階容積卡爾曼濾波在目標(biāo)跟蹤中的應(yīng)用[J].航空學(xué)報,2015,36(12):3885-3895.

[6] 徐樹生,林孝工,李新飛.強(qiáng)跟蹤自適應(yīng)平方根容積卡爾曼濾波算法[J].電子學(xué)報,2014(12):2394-2400.

[7] 張秋昭,張書畢,鄭南山,等.GPS/INS組合系統(tǒng)的多重減削魯棒容積卡爾曼濾波[J].中國礦業(yè)大學(xué)學(xué)報2014,43(1):162-168.

[8] 丁家琳,肖 建,趙 濤,等.自適應(yīng)CKF強(qiáng)跟蹤濾波器及其應(yīng)用[J].電機(jī)與控制學(xué)報，2015(11):111-120.

[9] 高社生,王建超,焦雅琳.自適應(yīng)SVD-UKF算法及在組合導(dǎo)航的應(yīng)用[J].中國慣性技術(shù)學(xué)報,2010,18(6):737-741.

[10] 徐 毓,金以慧,楊瑞娟.基于強(qiáng)跟蹤濾波器的多目標(biāo)跟蹤方法[J].傳感器與微系統(tǒng),2002,21(3):17-20.

[11] 李 陽,孔 毅,趙現(xiàn)斌.抗野值強(qiáng)跟蹤Kalam濾波在風(fēng)場估計中的應(yīng)用[J].傳感器與微系統(tǒng),2015,34(7):147-150.

[12] 王春霞,趙 偉,劉瑞華.雙重自適應(yīng)聯(lián)邦濾波在SINS—衛(wèi)星組合中的應(yīng)用研究[J].傳感器與微系統(tǒng),2008,27(3):66-69.

楊宗舉(1989 - )，男，河南平頂山人，碩士研究生，主要研究方向為北斗衛(wèi)星定位、組合導(dǎo)航研究及多傳感器數(shù)據(jù)融合、信號處理。

黃國勇，通訊作者，E—mail:448078230@qq.com。

Improved CKF algorithm and its application in BDS/INS*

YANG Zong-ju1,2, HUANG Guo-yong1,2

(1.Faculty of Information Engineering & Automation,Kunming University of Science and Technology,Kunming 650500,China; 2.Key Laboratory of Complex Industrial Process Detection,Control and Optimization,Kunming 650500, China)

To overcome problem of precision decreasing of conventional cubature Kalman filtering(CKF)when BDS/INS integrated navigation system model is not certain,combine CKF with singular value decomposition(SVD),and introducing strong tracking filtering(STF)theory,an improved strong tracking SVD-CKF algorithm is proposed.To improve stability of numerical calculation,SVD is used instead of the traditional Cholesky decomposition of standard CKF;fading factors are introduced in order to modify covariance of predicted error online based on strong tracking filtering(STF)theory framework,while system model is uncertain or system status is mutating,the proposed strong tracking SVD-CKF can improve robustness of system.Simulation results show the effectiveness of the presented algorithm.

cubature Kalman filtering(CKF); singular value decomposition(SVD); strong tracking filtering(STF); fading factor; integrated navigation

10.13873/J.1000—9787(2016)12—0156—05

2016—10—10

國家自然科學(xué)基金資助項目(61663017)； 云南省重大科技專項項目(2015ZC005)

TH 165.3

A

1000—9787(2016)12—0156—05