改進的CKF算法及其在BDS/INS中的應用*

楊宗舉， 黃國勇

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

?

改進的CKF算法及其在BDS/INS中的應用*

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

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

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

容積卡爾曼濾波； 奇異值分解； 強跟蹤濾波； 漸消因子； 組合導航

0 引 言

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

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

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

1 改進的SVD-CKF算法

對于組合導航系統(tǒng)來說，為了方便其計算，可以對CKF算法的觀測方程進行適當?shù)暮喕?，考慮非線性離散系統(tǒng)

(1)

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

為了方便計算，根據(jù)式(1)，可對標準CKF算法進行簡化，如下

(2)

(3)

(4)

(5)

(6)

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

(7)

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

1.1 改進的SVD-CKF算法

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

1.1.1SVD

在數(shù)值代數(shù)計算中，為保證其穩(wěn)定性和精度，可以采用SVD的方法，該方法在計算機上易于實現(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的右奇異向量。

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

(9)

1.1.2 改進的SVD-CKF算法

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

1)計算容積采樣點和對應的權值[7]

本文使用三階球面徑向容積準則獲得如下的基本容積點和相應權值

(10)

(11)

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

2)時間更新

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

(12)

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

(13)

(14)

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

(15)

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

(16)

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

(17)

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

計算量測預測值

(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 改進的強跟蹤SVD-CKF濾波算法

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

2.1 STF原理

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

(24)

(25)

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

Qk

(26)

2.2 改進的強跟蹤SVD-CKF濾波算法

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

引入多重漸消因子，如下

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

(27)

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

(28)

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

(29)

(30)

將等價的漸消因子計算方法帶入強跟蹤SVD-CKF濾波中，則強跟蹤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)驗選取。

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

3 仿真驗證

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

假設飛機做機動飛行，飛機的初始位置為東經(jīng)103.003°，北緯32.719°，高度320 m；初始速度為40 m/s，方向正北。設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。

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

組合導航系統(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為陀螺隨機漂移。組合導航系統(tǒng)的狀態(tài)方程如式(38)

(38)

式中f(·)為非線性函數(shù)，由捷聯(lián)慣導力學編排方程和姿態(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為量測噪聲。

對兩種濾波算法進行解算。圖1為飛機理想飛行航跡圖。

圖1 飛機理想飛行航跡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

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

4 結束語

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

2)改進的強跟蹤SVD-CKF算法優(yōu)于標準CKF算法，改進算法能夠提高濾波器的穩(wěn)定性，能夠有效提高組合導航的定位精度。

[1] 王 堅,劉 超,高井祥,等.基于抗差EKF的GNSS/INS緊組合算法研究[J].武漢大學學報,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] 崔乃剛,張 龍,王小剛,等.自適應高階容積卡爾曼濾波在目標跟蹤中的應用[J].航空學報,2015,36(12):3885-3895.

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

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

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

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

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

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

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

楊宗舉(1989 - )，男，河南平頂山人，碩士研究生，主要研究方向為北斗衛(wèi)星定位、組合導航研究及多傳感器數(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

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

TH 165.3

A

1000—9787(2016)12—0156—05