变增益联邦KF组合导航定位算法研究

张怡，赵凯华，姚斌

西北工业大学电子信息学院，西安 710072

变增益联邦KF组合导航定位算法研究

张怡，赵凯华，姚斌

西北工业大学电子信息学院，西安 710072

1 引言

GPS导航系统虽然具有定位精度高，误差不随时间累积，良好的通用性等优点。但是易受到外界的干扰以及多径效应的影响，使得GPS系统在某些信号强度低、干扰大的场合导航效果表现的并不理想。INS导航系统可以很好地解决这个问题，但INS导航系统的误差会随时间积累。因此，用INS辅助GPS进行导航定位越来越受到青睐，INS/GPS组合后可以大大提高系统的综合性能，适应各种复杂环境下的定位导航[1-3]，并且紧耦合组合方式在整体性能上较松耦合组合方式有较大提高[4]。

滤波算法在卫星导航定位中发挥着非常大的作用，针对GPS/INS组合导航系统导航定位算法，已有大量研究工作[5-7]。在传统的GPS/INS紧耦合组合导航系统中，由于伪距和多普勒频移误差的存在，系统存在一定的误差偏移。针对这种误差偏移，为进一步提高GPS/INS紧耦合组合导航算法的定位精度，以联邦卡尔曼滤波为基础的GPS/INS紧耦合组合导航定位算法，针对伪距测量误差对GPS/INS组合导航系统定位精度的影响，对伪距进行预滤波，提高了定位精度[8]。

传统的卡尔曼滤波是无限增长记忆的滤波，某时刻的最优估计要用到该时刻以前的观测数据。因此，随着步数的增大，滤波值中的老数据比重太大，而新数据比重太小，当系统存在模型误差和计算误差时，系统预测误差方差阵逐渐趋于零，新的观测数据对修正状态估计的作用太小，不能有效地抑制误差对状态估计值的影响，从而导致滤波发散。为解决这个问题，Fagin与Sorenson等人提出了衰减记忆滤波算法，该算法通过引入衰减因子限制卡尔曼滤波器的记忆长度，提高新量测值的利用权重从而抑制滤波发散[9]。

联邦卡尔曼滤波算法虽然提高了定位精度，随着滤波步数的增加，同样存在发散问题。本文研究了一种改进的联邦卡尔曼滤波算法，通过深入研究衰减记忆滤波算法，引入一种可变加权系数来持续地增加误差状态估计中新量测值的修正作用。仿真表明，该算法能够进一步提高组合导航系统的定位精度，有效抑制了滤波的发散作用，对工程应用有一定参考价值。

2 变增益联邦卡尔曼滤波器设计

2.1 滤波器结构设计

组合导航系统的主要误差来源就是GPS接收模块解算出的伪距误差，为削弱该误差带来的影响，运用一种二级卡尔曼滤波器进行导航定位解。滤波器结构如图1所示，在数据送到主卡尔曼滤波器进行融合前先对伪距进行预滤波，削弱伪距误差对整个系统定位精度的影响[8]。由于INS系统短时间定位精度较高，为降低整个系统的复杂程度，不对惯导信息进行预滤波。在完成预滤波之后，将经过预滤波的伪距信息和由INS得到的惯导信息导入到主滤波器中，两路数据在主滤波器中进行信息数据融合。

图1 联邦卡尔曼滤波器结构图

2.2 状态方程的建立

为了全面反映载体运动状态的变化，并充分考虑载体运动中的位置、速度、加速度等，状态向量设为：

式中，δr为位置误差矢量，δν为速度误差矢量，δε为姿态角误差，δω为陀螺仪零偏，δa为加速度计零偏；假设δω、δa为常值偏差。

系统状态误差方程为[10]：

式中，Fi是比力向量fi的反对称矩阵，代表由载体坐标系到惯性坐标系的方向余弦矩阵，Ni代表引力系数矩阵。

将该连续的状态方程离散化，即可得到离散系统的卡尔曼滤波状态方程：

2.3 量测方程的建立

在系统量测方程中，若直接采用伪距与多普勒频移测量残余作为观测量，则观测量中存在着本地时钟与卫星时钟的钟差。因此，将相对于不同卫星得到的伪距与多普勒频移测量残余相减[11]，得到伪距与多普勒频移单差：

2.4 主卡尔曼滤波器滤波方程的建立

在主滤波器中，采用应用卡尔曼滤波算法作为数据融合算法。卡尔曼滤波首先利用上一个状态的状态量，通过状态方程估计出这一个状态下量测量的值，完成预测过程，然后利用预测过程得到的信息量以及系统的观测量即可完成卡尔曼滤波的校正过程[11]，滤波方程为：

理论上，联邦卡尔曼滤波随着时间的推移，观测数据数量不断增多，滤波估计的精度应当越来越高。但是，在实际工程应用中，系统模型描述和噪声统计特性的误差会使得系统预测误差方差阵随着滤波时间的推移逐渐趋于零，从而状态估计会过分依赖旧量测值，新量测值对预测值的修正作用下降，导致滤波发散，影响系统定位精度。因此，可以通过逐渐降低旧量测值的权值，相应增大新量测值的权值，从而抑制滤波的发散。

式（10）中，增益矩阵取时刻k=N，则：

经分析，逐渐减小N时刻以前的KN值，相对地突出KN，便能在一定程度上抑制滤泡发散，考虑增大Pk及P0的值来减小Zi(i＜N)及对的影响。在Pk/k-1方程中引入可变权值系数：(1-sk)/(1-s)，(0＜s＜1)，可以看出随k值增大，权值系数会不断增大并大于1，这样便可保证对新量测值的利用，于是Pk/k-1方程可以改写为[12]：

本文采用伪距与多普勒频移测量残余为观测量，量测方程为非线性方程，需要采用扩展卡尔曼滤波的方法来进行滤波。改进后的变增益联邦卡尔曼主滤波器解算过程如下所示[13-15]。

3 仿真结果与分析

为验证变增益联邦卡尔曼滤波算法相对于原始联邦卡尔曼滤波算法对导航系统定位精度的改善效果，本文采用Matlab进行一组仿真测试，初始位置设为：（1 545 203.817 775 705，4 245 412.597 793 822，4 487 631.251 578 394），初始速度为（0，0，0），初始方向余弦矩阵为{1 0 0；0 1 0；0 0 1}。

载体轨迹仿真如图2所示。由图2可知，模拟载体经历了加速、爬升、滚动、转弯等多个过程，涵盖了现有大部分的运动状态。

图2 载体仿真轨迹

原始组合导航算法的位置误差和速度误差仿真如图3所示；原始联邦卡尔曼滤波算法与改进后的联邦卡尔曼滤波算法分别应用在GPS/INS紧耦合组合导航系统后，对于位置误差和速度误差的仿真如图4和图5所示。

图3 原始组合导航位置和速度误差

图4 位置误差对比

图5 速度误差对比

从图4可以明显看出，在X、Y、Z轴方向上位置偏差均有一定的减小，Z轴方向上的位置偏差减小量最为明显，这说明联邦卡尔曼滤波算法利用紧耦合方法计算伪距与多普勒频移单差可以有效提高组合导航系统的整体性能，其中Z轴上的改善更加明显。并且还可以看出，改进后的变增益联邦卡尔曼滤波算法位置误差减小量更大，说明改进的变增益卡尔曼滤波算法在加大对新量测值的利用权值后，能有效抑制系统的发散，可以进一步提高系统的定位精度。

从图5可以明显看出，两种联邦卡尔曼滤波算法均能有效减小速度误差，并且本文的变增益联邦卡尔曼滤波算法在速度定位上的误差相对于传原始的联邦卡尔曼滤波算法有了一定的提高。另外，从图中速度误差的偏移还可以看出，当载体位置出现较大变化时，系统可以很快速地修正这个速度偏差。这主要是由于本文方法在主滤波器之前，已经完成了修正量的计算，大大减小了运算量，因此可以快速地修正速度误差偏移。

4 结束语

提出了一种变增益联邦卡尔曼滤波算法，以二级联邦卡尔曼滤波器为基础，通过研究衰减卡尔曼滤波算法，引入变增益加权系数。针对伪距测量误差对于GPS/INS组合导航系统定位精度的影响，利用子滤波器对GPS接收模块解算出的伪距数据先进行预滤波，然后在主滤波器数据融合过程中引进变增益加权系数，加大新量测值对预测值的修正作用。本文算法吸收了联邦卡尔曼滤波算法的优点，考虑了最新量测值。仿真结果表明，该算法能有效改善滤波性能，提高导航系统的定位精度，抑制滤波发散。

[1]Li Yong，Wang Jinling.Low-cost tightly coupled GPS/INS integration based on a nonlinear Kalman filtering design[C]// Proceedings of IONNTM Conference，Monterey，CA，2006.

[2]Kim K，Park C G.INS/GPS tightly coupled approach using an INS error predictor[C]//Proc of GNSS 18th International Technical Meeting of the Satellite Division，Long Beach，CA，2005.

[3]Dikshit VG，MahapatraP R.Medium-coupledbusbased INS/GPS sensor fusion for accurate and reliable positioning[C]// Proceedings of ESAV Conference，Capri，Italy，2008.

[4]赵思浩，陆明泉，冯振明.EKF与UKF在紧耦合组合导航系统中的应用[J].系统工程与电子技术，2009，31（10）：2450-2454.

[5]向礼.非线性滤波方法及其在导航中的应用研究[D].哈尔滨：哈尔滨工业大学，2009.

[6]杨柳庆，肖前贵，牛妍.基于渐消卡尔曼滤波器的定位系统设计[J].南京航空航天大学学报，2012，44（1）：134-138.

[7]宋清昆，刘小磊.神经网络修正动态GPS卡尔曼滤波算法研究[J].计算机工程与应用，2011，47（15）：152-155.

[8]何伟，廉保旺，冯晓明.基于联邦卡尔曼滤波的GPS/INS紧耦合组合导航定位算法[J].火力与指挥控制，2012，36（8）：149-152.

[9]李杨，胡柏青.基于改进衰减记忆滤波的组合导航算法研究[J].计算机仿真，2012，29（3）：117-119.

[10]董绪荣，张守信，华仲春.GPS/INS组合导航定位及其应用[M].长沙：国防科学技术大学出版社，1998：245-248.

[11]谢刚.GPS原理与接收机设计[M].北京：电子工业出版社，2009.

[12]王虎，王解先，白贵霞.改进的渐消卡尔曼滤波在GPS动态定位中的应用[J].同济大学学报，2011，39（1）：124-127.

[13]吴志华，丁杨斌，申功勋.改进的非线性鲁棒EKF算法及其应用[J].计算机工程与应用，2011，47（3）：207-209.

[14]Guo L，Ljung L.Exponential stability of general tracking algorithms[J].IEEE Tran on Autom Contr，1995，40：1376-1387.

[15]Lippiello V，Siciliano B，Villani L.Adaptive extended Kalman filtering for visual motion estimation of 3D objects[J].Control Engineering Practice，2007，15：123-134.

ZHANG Yi,ZHAO Kaihua,YAO Bin

School of Electronics and Information,Northwestern Polychechnical University,Xi’an 710072,China

The pseudorange error worked out by the GPS receiver module is the major errors of GPS/INS navigation system, which can be weakened by using one federal Kalman filter algorithm of integrated navigation.This algorithm has two Kalman filters.The first Kalman filter filters the pseudo range and Doppler shift got from the GPS module.Then,the results got from the first Kalman filter is filtered by the second Kalman filter with the data got from the INS module.And it gets the correction and the optimum estimate.With the filter step increasing,the system prediction error variance tends to zero gradually,then the state estimation will excessively dependence on the old measurements,which will cause filter divergence and affect the position accuracy. To effectively increase the correction amount of the new measurements,this paper introduces a variable weighting factor in federal Kalman filter integrated navigation algorithm.The simulation results show that the improved variable gain federal Kalman filtering algorithm has the advantages of federal Kalman filter,and the filtering effect of this algorithm is improved obviously,which can effectively restrain the filter divergence and improve the positioning precision of the system.

federation Kalman filter;variable gain;location algorithm;accuracy;divergence

GPS接收模块解算出的伪距误差是GPS/INS组合导航系统的主要误差，采用一种二级联邦卡尔曼滤波组合导航算法加以削弱，将卫星接收模块解算出的伪距信息和多普勒频移信息在第一级卡尔曼滤波后，再通过主滤波器与INS模块解算出的信息进行修正处理，得到校正量和定位位置最优估计。随着滤波步数增加，系统预测误差方差阵逐渐趋于零，状态估计会过分依赖旧量测值，从而导致滤波发散，影响系统定位精度。为有效提高新量测值的修正作用，在联邦卡尔曼滤波组合导航算法中引入一种可变加权系数。仿真结果表明，改进后的变增益联邦卡尔曼滤波算法具备联邦卡尔曼滤波的优点，并且该算法滤波效果有较明显的改善，能有效抑制滤波发散，提高系统的定位精度。

联邦卡尔曼滤波；变增益；定位算法；精度；发散

A

TP301.6

10.3778/j.issn.1002-8331.1304-0099

ZHANG Yi,ZHAO Kaihua,YAO Bin.Research on variable gain federation Kalman filter location algorithm of coupled system.Computer Engineering and Applications,2013,49（19）：196-199.

国家自然科学基金（No.61202394）；西北工业大学研究生创业种子基金（No.Z2013095）。

张怡（1958—），女，教授，主要研究方向为通信、导航、遥测、遥控系统的信息传输与处理，多媒体通信与计算机通信；赵凯华（1988—），男，硕士，主要研究方向为GPS的导航与定位。E-mail：huakai051838@163.com

2013-04-08

2013-06-01

1002-8331（2013）19-0196-04

CNKI出版日期：2013-06-08http://www.cnki.net/kcms/detail/11.2127.TP.20130608.1001.028.html