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

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

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

?

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

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

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

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

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

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

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

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(·)為非線性量測函數(shù)；系統(tǒng)狀態(tài)向量xk∈Rn；量測向量zk∈Rm；wk-1，vk為互不相關(guān)的高斯白噪聲，其方差分別為Qk-1，Rk。

計算步驟如下：

1)初始化

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

(3)

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

(4)

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

2)時間更新

Sk-1=chol(Pk-1)

(5)

(6)

(7)

(8)

(9)

(10)

(11)

3)量測更新

(12)

Zj,k=h(Xj,k)

(13)

(14)

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

(15)

(16)

(17)

(18)

(19)

(20)

(21)

2 簡化的平方根容積卡爾曼濾波

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

簡化算法表示如下：

1)時間更新

(22)

(23)

(24)

2)量測更新

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

(25)

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

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

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

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

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

(26)

同理可得:

TRSCKF=3m1+m2+m3

(27)

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

最后將2種算法用在仿真環(huán)境為：LenovoM4360i3-3.4GHzCPU,2G內(nèi)存,MatlabR2009a32位版本的計算機，得到一次SCKF的濾波時間為0.01315s，一次RSCKF的濾波時間為0.006318s。通過比較可知，簡化算法的時間消耗減少了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)誤差、速度誤差、位置誤差、陀螺常值漂移和加計零偏；δtu是與時鐘誤差等效的距離誤差，δtru是與時鐘頻率誤差等效的距離誤差。

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

(29)

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

5 仿真實驗及分析

標(biāo)準(zhǔn)EKF、平方根CKF和簡化平方根CKF計算得到的定位誤差如圖2～4及表1所示?？梢钥吹剑?00～2000s時間段內(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)。簡化平方根CKF相對平方根CKF算法在時間更新環(huán)節(jié)進行了簡化，減小了計算量，但定位精度相當(dāng)，水平位置誤差在[-7.4m，+7.1m]，高度誤差在[-5.8m，+6.8m]。

圖1 飛行軌跡

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

圖3 平方根CKF定位誤差

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

[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].中國慣性技術(shù)學(xué)報, 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] 張鑫春,郭承軍.均方根嵌入式容積卡爾曼濾波[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é)報, 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

*國家973計劃資助(6132180403-1)

2015-09-21

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

V249.3

A

1006-3242(2016)01-0015-05