简化平方根容积卡尔曼滤波的INS/GPS紧组合算法*

沈 飞 郝顺义 吴训忠 郭 创 杨 彬

空军工程大学航空航天工程学院，西安 710038



简化平方根容积卡尔曼滤波的INS/GPS紧组合算法*

沈 飞 郝顺义 吴训忠 郭 创 杨 彬

空军工程大学航空航天工程学院，西安 710038

针对INS/GPS紧组合导航系统非线性模型解算的实时性问题，提出了一种用简化平方根容积卡尔曼滤波算法(RSCKF)提高紧组合导航系统运算速度的方法,它是在时间更新环节将平方根容积卡尔曼滤波(SCKF)简化，简化后直接用状态转移矩阵求取状态一步预测和预测协方差矩阵，避免了原算法中采用求容积点近似计算的复杂过程。仿真实验将RSCKF算法与SCKF滤波算法、扩展卡尔曼滤波算法(EKF)的结果对比。结果表明，RSCKF，SCKF两种算法的估计精度要明显高于EKF算法，而且在保证估计精度相当的情况下，RSCKF算法可大大降低系统运算量。 关键词 INS/GPS组合导航；紧组合；RSCKF；非线性模型

INS/GPS组合导航系统本质是非线性的，但为了减少计算量、提高其实时性，通过某些假设条件，可忽略系统的非线性因素，将其用线性化的数学模型来近似描述，这时的导航系统一般采用线性卡尔曼滤波算法进行状态估计[1]。然而，当假设条件不满足时，组合导航系统就必须采用能反映自身实际特性的非线性模型来描述[2-3]，事实上，采用非线性模型更能完整地传播系统误差特性，而此时要想对系统的状态进行准确估计，线性的卡尔曼滤波已不再适用，必须采用非线性滤波才能解决问题[4-5]。针对此问题，可以采用扩展卡尔曼滤波(EKF)，该算法主要是对非线性系统进行线性化。但是，当系统非线性度较强时，忽略泰勒展开的高阶项，会引入高阶项截断误差，导致滤波误差增大甚至发散。

最近Arasaratnam和Haykin提出了一种容积卡尔曼滤波[6-7](Cubature Kalman Filter，CKF)，其核心是基于spherical-radial cubature准则，计算出一组等权值的容积点，直接通过非线性系统方程传播这些点来进行状态估计。为了避免复杂的矩阵求逆和分解运算，两位学者提出了滤波精度和稳定性更高的平方根容积卡尔曼滤波[8](Square-root Cubature Kalman Filter，SCKF)算法。而上述算法的优越性，在文献[9-11]的仿真中已经得到验证。但当系统的状态方程或量测方程是线性时，采用上述方法会增大计算量，因此可通过对线性模型部分的一步预测采用卡尔曼滤波，非线性模型部分采用容积滤波对其进行简化[12]。

本文根据组合导航系统的特点，针对一般组合系统状态维数高、变量多，导致滤波实时性差的问题，提出了将一种新的简化SCKF算法应用到模型中，并通过仿真与SCKF算法和EKF进行了比较。结果表明，简化的算法在保证了组合滤波精度的同时，有效降低了系统运算量。

1 平方根容积卡尔曼滤波

考虑如下的非线性系统

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

(1)

zk=h(xk)+vk

(2)

式中，Φk,k-1∈Rn×n为线性状态转移矩阵；h(·)为非线性量测函数；系统状态向量xk∈Rn；量测向量zk∈Rm；wk-1，vk为互不相关的高斯白噪声，其方差分别为Qk-1，Rk。

计算步骤如下：

1)初始化

给定均值和协方差：

(3)

计算求容积点和权值，即

(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 简化的平方根容积卡尔曼滤波

考虑到系统的状态方程为线性，量测方程为非线性，在时间更新环节使用线性的卡尔曼滤波进行简化，在量测更新过程使用平方根求容积卡尔曼滤波。

简化算法表示如下：

1)时间更新

(22)

(23)

(24)

2)量测更新

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

(25)

3 算法复杂度分析

相比于SCKF算法，RSCKF算法是在时间更新过程用状态转移矩阵求取状态一步预测和预测协方差矩阵，而二者的算法步骤在量测更新过程基本一致，所以下面主要对时间更新过程进行定性和定量分析。

首先定性分析SCKF算法时间更新过程，观察式(5)～(11)，在前一时刻的状态估计和协方差阵的平方根阵已知的情况下，首先计算求容积点，然后再计算传播后的求容积点，再根据权值计算状态预测值，而对预测协方差阵平方根的求取不仅要用到矩阵分解，而且牵扯到矩阵γk的计算，运算时间与状态维数紧密相关，大大增加了计算量。然而观察式(22)～(23)，相比于SCKF算法，简化算法避免了计算求积点和矩阵γk，直接用状态转移矩阵来计算一步预测值，减小了运算成本。

下面对时间更新过程进行定量分析，假设矩阵乘积、加减、分解运算时间分别为m1，m2，m3，且状态为n维，则SCKF算法的运算时间可表示为:

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

(26)

同理可得:

TRSCKF=3m1+m2+m3

(27)

上面的运算均不考虑矩阵转置及分解运算后取非零方阵时间。通过对比发现，简化算法的运算时间远小于SCKF算法。

最后将2种算法用在仿真环境为：LenovoM4360i3-3.4GHzCPU,2G内存,MatlabR2009a32位版本的计算机，得到一次SCKF的滤波时间为0.01315s，一次RSCKF的滤波时间为0.006318s。通过比较可知，简化算法的时间消耗减少了50%。

4 INS/GPS紧组合系统非线性模型

4.1 系统状态方程线性模型

组合导航误差状态方程如下：

(28)

式中，X(t)是17×1维误差状态向量，W(t)为系统噪声，G(t)为噪声分配阵，X(t)表示为

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

其中,各状态量依次为东、北、天向的姿态误差、速度误差、位置误差、陀螺常值漂移和加计零偏；δtu是与时钟误差等效的距离误差，δtru是与时钟频率误差等效的距离误差。

4.2 系统量测方程非线性模型

(29)

式中，V(t)为量测噪声。

5 仿真实验及分析

标准EKF、平方根CKF和简化平方根CKF计算得到的定位误差如图2～4及表1所示。可以看到，在300～2000s时间段内，采用标准EKF得到的水平位置误差在[-11.3m，+12.1m]，高度误差在[-9.7m，+8.6m]内。CKF得到的定位精度比标准EKF有明显提高，为：水平位置误差在[-7.8m，+7.3m]，高度误差在[-6.0m，+7.2m]内。简化平方根CKF相对平方根CKF算法在时间更新环节进行了简化，减小了计算量，但定位精度相当，水平位置误差在[-7.4m，+7.1m]，高度误差在[-5.8m，+6.8m]。

图1 飞行轨迹

图2 标准EKF定位误差

图3 平方根CKF定位误差

图4 简化平方根CKF定位误差

表1 组合导航定位误差比较

滤波算法误差类型纬度/m经度/m高度/mEKFMeanerror3.8264.1124.831RMSE3.9044.6355.201SCKFMeanerror3.1133.2433.353RMSE3.2503.5483.424RSCKFMeanerror3.0613.1243.225RMSE3.1873.3253.341

6 结论

为提高INS/GPS紧组合导航系统解算实时性，针对状态方程为线性，量测方程为非线性的模型提出将RSCKF算法应用到系统中。简化算法对SCKF算法的时间更新过程进行了简化，简化后的时间更新过程与线性卡尔曼滤波算法的时间更新过程一致。计算复杂度分析和仿真分析表明，简化算法的运算量和一次运算时间要明显低于SCKF算法。将简化算法与EKF，SCKF算法进行对比，结果表明，SCKF算法精度要明显高于EKF，这是由于EKF算法在处理非线性模型时采用线性化，摒弃了高阶项，降低了滤波精度；RSCKF算法与SCKF相比，估计精度相当，但在算法实时性方面，RSCKF算法大大减少了计算量，使得一次滤波的时间消耗减少了50%，提高了导航解算的实时性。

[1] 刘建业，曾庆化，赵伟，等.导航系统理论与应用[M].西安:西北工业大学出版社,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] 程向红，王晓飞，刘峰丽.稀疏网格求积分滤波算法在SINS/GPS紧组合导航中的应用[J].中国惯性技术学报, 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].控制理论与应用,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组合导航滤波算法[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].哈尔滨工业大学学报, 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-)，男，沈阳人，硕士研究生，主要研究方向为组合导航与多源信息融合；郝顺义 (1973-)，男，山西临猗人，博士，副教授，主要研究方向为惯性导航与组合导航；吴训忠(1969-)，男，湖北襄阳人，博士，教授，主要研究方向导航、制导与控制；郭 创(1978-)，男，湖南益阳人，博士，副教授，主要研究方向为导航、制导与控制；杨 彬 (1990-)，男，山东潍坊人，硕士研究生，主要研究方向为惯性导航与组合导航。

V249.3

A

1006-3242(2016)01-0015-05