沈 飛 郝順義 吳訓(xùn)忠 郭 創(chuàng) 楊 彬

空軍工程大學(xué)航空航天工程學(xué)院，西安 710038

?

簡(jiǎn)化平方根容積卡爾曼濾波的INS/GPS緊組合算法*

沈 飛 郝順義 吳訓(xùn)忠 郭 創(chuàng) 楊 彬

空軍工程大學(xué)航空航天工程學(xué)院，西安 710038

針對(duì)INS/GPS緊組合導(dǎo)航系統(tǒng)非線性模型解算的實(shí)時(shí)性問題，提出了一種用簡(jiǎn)化平方根容積卡爾曼濾波算法(RSCKF)提高緊組合導(dǎo)航系統(tǒng)運(yùn)算速度的方法,它是在時(shí)間更新環(huán)節(jié)將平方根容積卡爾曼濾波(SCKF)簡(jiǎn)化，簡(jiǎn)化后直接用狀態(tài)轉(zhuǎn)移矩陣求取狀態(tài)一步預(yù)測(cè)和預(yù)測(cè)協(xié)方差矩陣，避免了原算法中采用求容積點(diǎn)近似計(jì)算的復(fù)雜過(guò)程。仿真實(shí)驗(yàn)將RSCKF算法與SCKF濾波算法、擴(kuò)展卡爾曼濾波算法(EKF)的結(jié)果對(duì)比。結(jié)果表明，RSCKF，SCKF兩種算法的估計(jì)精度要明顯高于EKF算法，而且在保證估計(jì)精度相當(dāng)?shù)那闆r下，RSCKF算法可大大降低系統(tǒng)運(yùn)算量。 關(guān)鍵詞 INS/GPS組合導(dǎo)航；緊組合；RSCKF；非線性模型

INS/GPS組合導(dǎo)航系統(tǒng)本質(zhì)是非線性的，但為了減少計(jì)算量、提高其實(shí)時(shí)性，通過(guò)某些假設(shè)條件，可忽略系統(tǒng)的非線性因素，將其用線性化的數(shù)學(xué)模型來(lái)近似描述，這時(shí)的導(dǎo)航系統(tǒng)一般采用線性卡爾曼濾波算法進(jìn)行狀態(tài)估計(jì)[1]。然而，當(dāng)假設(shè)條件不滿足時(shí)，組合導(dǎo)航系統(tǒng)就必須采用能反映自身實(shí)際特性的非線性模型來(lái)描述[2-3]，事實(shí)上，采用非線性模型更能完整地傳播系統(tǒng)誤差特性，而此時(shí)要想對(duì)系統(tǒng)的狀態(tài)進(jìn)行準(zhǔn)確估計(jì)，線性的卡爾曼濾波已不再適用，必須采用非線性濾波才能解決問題[4-5]。針對(duì)此問題，可以采用擴(kuò)展卡爾曼濾波(EKF)，該算法主要是對(duì)非線性系統(tǒng)進(jìn)行線性化。但是，當(dāng)系統(tǒng)非線性度較強(qiáng)時(shí)，忽略泰勒展開的高階項(xiàng)，會(huì)引入高階項(xiàng)截?cái)嗾`差，導(dǎo)致濾波誤差增大甚至發(fā)散。

最近Arasaratnam和Haykin提出了一種容積卡爾曼濾波[6-7](Cubature Kalman Filter，CKF)，其核心是基于spherical-radial cubature準(zhǔn)則，計(jì)算出一組等權(quán)值的容積點(diǎn)，直接通過(guò)非線性系統(tǒng)方程傳播這些點(diǎn)來(lái)進(jìn)行狀態(tài)估計(jì)。為了避免復(fù)雜的矩陣求逆和分解運(yùn)算，兩位學(xué)者提出了濾波精度和穩(wěn)定性更高的平方根容積卡爾曼濾波[8](Square-root Cubature Kalman Filter，SCKF)算法。而上述算法的優(yōu)越性，在文獻(xiàn)[9-11]的仿真中已經(jīng)得到驗(yàn)證。但當(dāng)系統(tǒng)的狀態(tài)方程或量測(cè)方程是線性時(shí)，采用上述方法會(huì)增大計(jì)算量，因此可通過(guò)對(duì)線性模型部分的一步預(yù)測(cè)采用卡爾曼濾波，非線性模型部分采用容積濾波對(duì)其進(jìn)行簡(jiǎn)化[12]。

本文根據(jù)組合導(dǎo)航系統(tǒng)的特點(diǎn)，針對(duì)一般組合系統(tǒng)狀態(tài)維數(shù)高、變量多，導(dǎo)致濾波實(shí)時(shí)性差的問題，提出了將一種新的簡(jiǎn)化SCKF算法應(yīng)用到模型中，并通過(guò)仿真與SCKF算法和EKF進(jìn)行了比較。結(jié)果表明，簡(jiǎn)化的算法在保證了組合濾波精度的同時(shí)，有效降低了系統(tǒng)運(yùn)算量。

1 平方根容積卡爾曼濾波

考慮如下的非線性系統(tǒng)

xk=Φk,k-1xk-1+Γk-1wk-1

(1)

zk=h(xk)+vk

(2)

式中，Φk,k-1∈Rn×n為線性狀態(tài)轉(zhuǎn)移矩陣；h(·)為非線性量測(cè)函數(shù)；系統(tǒng)狀態(tài)向量xk∈Rn；量測(cè)向量zk∈Rm；wk-1，vk為互不相關(guān)的高斯白噪聲，其方差分別為Qk-1，Rk。

計(jì)算步驟如下：

1)初始化

給定均值和協(xié)方差：

(3)

計(jì)算求容積點(diǎn)和權(quán)值，即

(4)

式中，[1]=[I,-I]，I為n維單位方陣，[1]為n×2n維矩陣，[1]j表示取[1]第j列。

2)時(shí)間更新

Sk-1=chol(Pk-1)

(5)

(6)

(7)

(8)

(9)

(10)

(11)

3)量測(cè)更新

(12)

Zj,k=h(Xj,k)

(13)

(14)

Szz,k=qr([γkSR,k])

(15)

(16)

(17)

(18)

(19)

(20)

(21)

2 簡(jiǎn)化的平方根容積卡爾曼濾波

考慮到系統(tǒng)的狀態(tài)方程為線性，量測(cè)方程為非線性，在時(shí)間更新環(huán)節(jié)使用線性的卡爾曼濾波進(jìn)行簡(jiǎn)化，在量測(cè)更新過(guò)程使用平方根求容積卡爾曼濾波。

簡(jiǎn)化算法表示如下：

1)時(shí)間更新

(22)

(23)

(24)

2)量測(cè)更新

計(jì)算式(12)～(21)，然后計(jì)算

(25)

3 算法復(fù)雜度分析

相比于SCKF算法，RSCKF算法是在時(shí)間更新過(guò)程用狀態(tài)轉(zhuǎn)移矩陣求取狀態(tài)一步預(yù)測(cè)和預(yù)測(cè)協(xié)方差矩陣，而二者的算法步驟在量測(cè)更新過(guò)程基本一致，所以下面主要對(duì)時(shí)間更新過(guò)程進(jìn)行定性和定量分析。

首先定性分析SCKF算法時(shí)間更新過(guò)程，觀察式(5)～(11)，在前一時(shí)刻的狀態(tài)估計(jì)和協(xié)方差陣的平方根陣已知的情況下，首先計(jì)算求容積點(diǎn)，然后再計(jì)算傳播后的求容積點(diǎn)，再根據(jù)權(quán)值計(jì)算狀態(tài)預(yù)測(cè)值，而對(duì)預(yù)測(cè)協(xié)方差陣平方根的求取不僅要用到矩陣分解，而且牽扯到矩陣γk的計(jì)算，運(yùn)算時(shí)間與狀態(tài)維數(shù)緊密相關(guān)，大大增加了計(jì)算量。然而觀察式(22)～(23)，相比于SCKF算法，簡(jiǎn)化算法避免了計(jì)算求積點(diǎn)和矩陣γk，直接用狀態(tài)轉(zhuǎn)移矩陣來(lái)計(jì)算一步預(yù)測(cè)值，減小了運(yùn)算成本。

下面對(duì)時(shí)間更新過(guò)程進(jìn)行定量分析，假設(shè)矩陣乘積、加減、分解運(yùn)算時(shí)間分別為m1，m2，m3，且狀態(tài)為n維，則SCKF算法的運(yùn)算時(shí)間可表示為:

TSCKF=n(2m1+5m2+2m3)

(26)

同理可得:

TRSCKF=3m1+m2+m3

(27)

上面的運(yùn)算均不考慮矩陣轉(zhuǎn)置及分解運(yùn)算后取非零方陣時(shí)間。通過(guò)對(duì)比發(fā)現(xiàn)，簡(jiǎn)化算法的運(yùn)算時(shí)間遠(yuǎn)小于SCKF算法。

最后將2種算法用在仿真環(huán)境為：LenovoM4360i3-3.4GHzCPU,2G內(nèi)存,MatlabR2009a32位版本的計(jì)算機(jī)，得到一次SCKF的濾波時(shí)間為0.01315s，一次RSCKF的濾波時(shí)間為0.006318s。通過(guò)比較可知，簡(jiǎn)化算法的時(shí)間消耗減少了50%。

4 INS/GPS緊組合系統(tǒng)非線性模型

4.1 系統(tǒng)狀態(tài)方程線性模型

組合導(dǎo)航誤差狀態(tài)方程如下：

(28)

式中，X(t)是17×1維誤差狀態(tài)向量，W(t)為系統(tǒng)噪聲，G(t)為噪聲分配陣，X(t)表示為

X(t)=[φEφNφUδvEδvNδvUδLδλ
δhεxεyεz▽x▽y▽zδtuδtru]T

其中,各狀態(tài)量依次為東、北、天向的姿態(tài)誤差、速度誤差、位置誤差、陀螺常值漂移和加計(jì)零偏；δtu是與時(shí)鐘誤差等效的距離誤差，δtru是與時(shí)鐘頻率誤差等效的距離誤差。

4.2 系統(tǒng)量測(cè)方程非線性模型

(29)

式中，V(t)為量測(cè)噪聲。

5 仿真實(shí)驗(yàn)及分析

標(biāo)準(zhǔn)EKF、平方根CKF和簡(jiǎn)化平方根CKF計(jì)算得到的定位誤差如圖2～4及表1所示?？梢钥吹?，在300～2000s時(shí)間段內(nèi)，采用標(biāo)準(zhǔn)EKF得到的水平位置誤差在[-11.3m，+12.1m]，高度誤差在[-9.7m，+8.6m]內(nèi)。CKF得到的定位精度比標(biāo)準(zhǔn)EKF有明顯提高，為：水平位置誤差在[-7.8m，+7.3m]，高度誤差在[-6.0m，+7.2m]內(nèi)。簡(jiǎn)化平方根CKF相對(duì)平方根CKF算法在時(shí)間更新環(huán)節(jié)進(jìn)行了簡(jiǎn)化，減小了計(jì)算量，但定位精度相當(dāng)，水平位置誤差在[-7.4m，+7.1m]，高度誤差在[-5.8m，+6.8m]。

圖1 飛行軌跡

圖2 標(biāo)準(zhǔn)EKF定位誤差

圖3 平方根CKF定位誤差

圖4 簡(jiǎn)化平方根CKF定位誤差

表1 組合導(dǎo)航定位誤差比較

濾波算法誤差類型緯度/m經(jīng)度/m高度/mEKFMeanerror3.8264.1124.831RMSE3.9044.6355.201SCKFMeanerror3.1133.2433.353RMSE3.2503.5483.424RSCKFMeanerror3.0613.1243.225RMSE3.1873.3253.341

6 結(jié)論

為提高INS/GPS緊組合導(dǎo)航系統(tǒng)解算實(shí)時(shí)性，針對(duì)狀態(tài)方程為線性，量測(cè)方程為非線性的模型提出將RSCKF算法應(yīng)用到系統(tǒng)中。簡(jiǎn)化算法對(duì)SCKF算法的時(shí)間更新過(guò)程進(jìn)行了簡(jiǎn)化，簡(jiǎn)化后的時(shí)間更新過(guò)程與線性卡爾曼濾波算法的時(shí)間更新過(guò)程一致。計(jì)算復(fù)雜度分析和仿真分析表明，簡(jiǎn)化算法的運(yùn)算量和一次運(yùn)算時(shí)間要明顯低于SCKF算法。將簡(jiǎn)化算法與EKF，SCKF算法進(jìn)行對(duì)比，結(jié)果表明，SCKF算法精度要明顯高于EKF，這是由于EKF算法在處理非線性模型時(shí)采用線性化，摒棄了高階項(xiàng)，降低了濾波精度；RSCKF算法與SCKF相比，估計(jì)精度相當(dāng)，但在算法實(shí)時(shí)性方面，RSCKF算法大大減少了計(jì)算量，使得一次濾波的時(shí)間消耗減少了50%，提高了導(dǎo)航解算的實(shí)時(shí)性。

[1] 劉建業(yè)，曾慶化，趙偉，等.導(dǎo)航系統(tǒng)理論與應(yīng)用[M].西安:西北工業(yè)大學(xué)出版社,2010.(Liu Jianye, Zeng Qinghua, Zhao Wei, et al. Flying Control System [M]. Xi’an: Press of Northwestern Polytechnical University, 2010.)

[2] ALI J, MIRZA MRUB. Performance comparision among some nonlinear filters for a low cost SINS/GPS integrated solution [J]. Nonlinear Dynamics, 2010, 61(3):491-502.

[3] 程向紅，王曉飛，劉峰麗.稀疏網(wǎng)格求積分濾波算法在SINS/GPS緊組合導(dǎo)航中的應(yīng)用[J].中國(guó)慣性技術(shù)學(xué)報(bào), 2014, 22(6): 799-804.(Cheng Xianghong, Wang Xiaofei, Liu Fengli. Application of Sparsegrid Quadrature Filter to Tightly-coupled SINS/GPS Integrated Navigation System [J]. Journal of Chinese Inertial Technology, 2014, 22 (6): 799-804.)

[4] LIM Jaechan, HONG Daehyoung. Gaussian Particle Filtering Approach for Carrier Frequency Offset Estimation in OFDM System [J]. IEEE Signal Processing Letters, 2013, 20(4): 367-370.

[5] De MARINA H G, PEREDA F J, GIRON-SIERRA J M. UAV Attitude Estimation Using Unscented Kalman Filter and TRIAD [J]. IEEE Trans. Ind. Electron. 2012, 59(11):4465-4474.

[6] Arasaratnam I, Haykin S. Cubature Kalman Filter [J]. IEEE Trans on Automatic Control, 2009, 54(6): 1254-1269.

[7] ZHANG X C，GUO C J. Cubature Kalman Filter: Derivation and extension [J]. Chinese Physics B, 2013, 22(12): 501-506.

[8] 張?chǎng)未?郭承軍.均方根嵌入式容積卡爾曼濾波[J].控制理論與應(yīng)用,2013, 30(9): 1116-1121.(Zhang Xinchun, Guo Chengjun. Square-root Imbedded Cubature Kalman Filtering[J]. Control Theory & Applications, 2013, 30(9): 1116-1121.)

[9] 孫楓,唐李軍.基于Cubature Kalman Filter的INS/GPS組合導(dǎo)航濾波算法[J].控制與決策, 2012, 27(7): 1032-1036.(Sun Feng, Tang Lijun. INS/GPS Integrated Navigation Filter Algorithm Based on Cubature Kalman Filter [J]. Control and Decision, 2012, 27(7): 1032-1036.)

[10] Dai H D, Dai S W, Cong Y C, et al. Performance Comparision of EKF/UKF/CKF for the Tracking of Ballistic Target [J]. Telkomnika Indonesian J of Electrical Engineering, 2012,10(7):1692-1699.

[12] Mohammed D, Abdelkrim M, Mokhtar K, Abdelziz O. Reduced Cubature Kalman filtering applied to target tracking[C]// The 2nd International Conference Control, Instrumentation and Automation(ICCIA), Shiraz: IEEE, 2011: 1097-1101.

[13] 于永軍,徐錦法，熊智.高斯粒子濾波的慣性/GPS緊組合算法[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào), 2015, 47(5): 81-85.(Yu Yongjun, Xu Jinfa, Xiong Zhi. SINS/GPS Tightly Integrated Algorithm with Gaussian Particle Filter [J]. Journal of Harbin Institute of Technology, 2015, 47(5): 81-85.)

INS/GPS Tightly Integrated Algorithm Based on Reduced Square-Root Cubature Kalman Filter

Shen Fei, Hao Shunyi, Wu Xunzhong, Guo Chuang, Yang Bin

Aeronautics and Astronautics Engineering College, Air Force Engineering University, Xi’an 710038，China

Byfocusingontheproblemoftherealtimeofnonlinearmodelcalculation,thereducedsquarerootcubatureKalmanfilter(RSCKF)isproposedtouseforimprovingtheoperatingrateofINS/GPStightlyintegratednavigationsystem.ThisreducedarithmeticsimplifiesthesquarerootcubatureKalmanfilter(SCKF)intimeupdatestep,andthestate-transitionmatrixisdirectlyusedforcalculatingtheone-steppredictionmatrixofstateandcovariancebyavoidingthecomplexapproximatecalculationprocessofcalculatingcubature.Duringthesimulationexperiments,theRSCKFresultsarecomparedwithSCKFandEKFresultsandshowthattheRSCKFandSCKFperformwellthanEKF.TheRSCKFalgorithmperformsnearlythesameastheSCKFbyprecisionandcaneffectivelyreducetheamountofcalculation.

INS/GPSintegratednavigation；Tightlycoupling；RSCKF；Nonlinearmodel

*國(guó)家973計(jì)劃資助(6132180403-1)

2015-09-21

沈 飛 (1992-)，男，沈陽(yáng)人，碩士研究生，主要研究方向?yàn)榻M合導(dǎo)航與多源信息融合；郝順義 (1973-)，男，山西臨猗人，博士，副教授，主要研究方向?yàn)閼T性導(dǎo)航與組合導(dǎo)航；吳訓(xùn)忠(1969-)，男，湖北襄陽(yáng)人，博士，教授，主要研究方向?qū)Ш?、制?dǎo)與控制；郭 創(chuàng)(1978-)，男，湖南益陽(yáng)人，博士，副教授，主要研究方向?yàn)閷?dǎo)航、制導(dǎo)與控制；楊 彬 (1990-)，男，山東濰坊人，碩士研究生，主要研究方向?yàn)閼T性導(dǎo)航與組合導(dǎo)航。

V249.3

A

1006-3242(2016)01-0015-05