新的自适应渐消扩展卡尔曼滤波在GPS定位中的应用*

胡　辉，彭雄明，杨德进，李阳达

（1.华东交通大学信息工程学院，南昌　330013；2.中国科学院国家授时中心，西安　710000）



新的自适应渐消扩展卡尔曼滤波在GPS定位中的应用*

胡辉1，彭雄明1，杨德进2，李阳达1

（1.华东交通大学信息工程学院，南昌330013；2.中国科学院国家授时中心，西安710000）

摘要：当实际数据出现突变时，基于最小二乘、扩展卡尔曼滤波的GPS定位解算存在定位结果精度低和稳定性差的问题。提出一种自适应渐消扩展卡尔曼滤波算法，通过自适应渐消迭代系统噪声协方差，来实现抑制数据突变影响。试验结果表明：该算法相比最小二乘、扩展卡尔曼滤波，其定位精度有所提高;相比传统渐消扩展卡尔曼滤波，其收敛速度、稳定性有所提高。

关键词：GPS，自适应渐消，扩展卡尔曼滤波，定位精度，稳定性

0　引言

标准的卡尔曼滤波是在假设系统噪声和观测噪声已知的情况下进行滤波，只要噪声的特性描述正确，滤波就会产生优化的系统状态估计值。实际情况，受实验样本的限制，噪声的先验统计特性未知或不准确，即使已知噪声的先验统计特性，但系统处于实时的运行环境，受内外不确定因素的影响，噪声统计特性容易发生变化，而经典卡尔曼不具有应对噪声统计变化的自适应能力，不准确的噪声模型会导致滤波器不稳定和精度降低，甚至发散。

在导航定位中，系统模型都有一定程度的非线性，为了解决非线性滤波的问题，用的最广泛的滤波方法是扩展卡尔曼滤波（EKF），当模型参数存在较大的误差时，扩展卡尔曼滤波的估计精度会大大降低，甚至发散。为了克服现有EKF的缺点，有很多学者对EKF进行了自适应研究，见文献［1-11］。针对实际情况中数据突变，导致收敛速度、收敛稳定性差等问题，本文提出一种新的自适应渐消系统噪声算法来实现GPS接收机的定位，并与传统LS、EKF、渐消扩展卡尔曼滤波算法比较，选择最优的定位算法。

1定位解算模型

将EKF应用于GPS定位解算中，首要是建立状态模型和观测模型。定义Xk为系统状态向量，Zk为系统观测向量。

式（1）中系统状态向量由接收机的位置（xk，yk，zk）、速度（vxk，vyk，vzk）、接收机的钟差δtk和钟漂δ˙tk构成，式（2）中观测向量由伪距ρik和多普勒测量值dik构成。其中i表示第i颗卫星，i=1，2，3，…，n，n为可见卫星数，k为采样时刻。

1.1状态方程

在PV（Position，Velocity）模型中，状态量包括3个正交方向上的位置和速度、钟差、钟漂，其中每个方向上的位置和速度所含噪声是相互独立的，相关系数为0。系统的状态转移方程:

式（3）中F是离散时间域的状态转移系数矩阵，其推导是由连续时间域F（t）推导，F（t）满足

F系数矩阵满足［13］

将式（4）和式（6）带入式（5）化简得到F系数矩阵为：

因在［tk，tk+1］时间内噪声可以定义为

式（9）中G（λ）是转化矩阵，F（t）是连续时间域的状态转移矩阵，在［tk，tk+1］时间内近似为常量。

将式（9）带入式（8）中化简得

由式（4）可以推得

化简式（12）得到离散化的Qk如下:

1.2观测方程

系统的观测方程，伪矩和系统状态参数的关系为：

式（14）中，ρik是第i颗卫星k时刻的伪矩，rrk是接收机k时刻的位置，rik是第i颗卫星k时刻卫星的位置，εik是第i颗卫星k时刻总的测量误差，εik用白噪声进行建模。其伪矩方程中有4个未知的变量，接收机的位置（xrk，yrk，zrk）以及接收机时钟偏差δtk；其k时刻第i颗卫星的位置为（xik，yik，zik），其由导航电文提取后求得。

多普勒频率测量值的计算：

式（15）中vxk，vyk，vzk分别为接收机在X、Y、Z方向的速度。vxik，vyik，vzik分别为第i颗卫星X、Y、Z方向的速度。

对观测方程式（14）和式（15）而言，可以写成矩阵的形式：

式（16）中Xk是k时刻点的状态向量，Vk是观测噪声矩阵，Zk是在k时刻的观测值，Hk是观测方程矩阵。式（3）中状态噪声Wk和式（16）中量测噪声Vk的期望和协方差矩阵满足：

E为数学期望符号，Qk为n×n维的半正定矩阵，Rk是m×m维正定对称矩阵。

卫星与接收机几何距离

其观测方程矩阵Hk满足：

2自适应渐消扩展卡尔曼滤波

上述已建立GPS定位系统的状态方程和观测方程。现将建立的模型应用于EKF中，传统渐消扩展卡尔曼（FEKF）对系统的状态协方差以及系统噪声进行实时渐消，从而达到降低系统模型误差、抑制滤波发散、提高滤波的精度。因其渐消因子涉及的参数较多，渐消的针对性难以区分。为此，提出针对系统噪声进行自适应渐消的扩展卡尔曼滤波（AFEKF）方法。

2.1自适应渐消扩展卡尔曼滤波模型

和EKF估计算法过程一样，AFEKF主要由预测和更新两个过程构成，传统EKF流程如下：

2.1.1系统状态和状态协方差矩阵的时间更新

式（19）中X^k|k-1是k-1时刻的时间更新状态向量，F为状态转移矩阵，X^k-1是k-1时刻的状态向量；式（20）中Pk|k-1是状态误差的协方差时间更新矩阵，Pk-1是k-1时刻的状态误差的协方差矩阵，Qk-1为k-1时刻的系统噪声方差矩阵。

2.1.2观测量更新

卡尔曼增益计算：

式（21）中Rk是观测量的方差矩阵，Kk是卡尔曼增益矩阵，Hk是量测矩阵。

状态估计和均方误差估计：

式（22）中X^k是估计的状态向量，式（23）Pk是估计的状态协方差矩阵

估计的残差向量：

EKF系统中，实际数据出现突变时估计的残差Vk会增大，而增益阵Kk因离线计算的原因，未能根据实时的状态在线调整增益矩阵Kk，Kk仍将趋于极小值，不会随残差Vk的增大而变大，导致EKF滤波精度下降，甚至发散。

传统FEKF［3］在式（20）中引入λk

其中未进行式（26）处理的系统噪声为未自适应系统噪声渐消扩展卡尔曼滤波算法。

式（25）中λk为k时刻的渐消因子。本文处理过程中，提出利用自适应渐消因子对时变系统噪声Qk进行自适应，并同扩展卡尔曼滤波、传统渐消扩展卡尔曼滤波方法进行比较。

2.2自适应渐消因子的确定

渐消子的选取，主要是用渐消因子抑制卡尔曼滤波的记忆长度，利用当前的观测数据，提高滤波器的动态性能。如下针对系统噪声进行自适应渐消，并推导渐消因子的计算方法。

将式（20）和式（21）中Pk|k-1、Kk的表达式代入式（28）中，化简整理得：

式（29）中:

对式（31）两边求迹，得到如下表达式：

上求渐消因子的步骤，相比现有求渐消因子的方法，没有标量因子的计算，其计算简单且易实现，实时性效果也能得到保证。在状态突变时，估计的观测误差Vk会相应增大，会引起误差方差矩阵Ck增大，则λ0会相应地增大，则能确保式（28）满足。

3试验结果分析

本文进行了如下3组实验，一组是静态未突变场景实验，一组匀速圆周运动实验，一组数据突变场景实验；设置初始化状态协方差，P0=diag［106，106，106，106，106，106，106，106］，观测量伪矩协方差和多普勒速度协方差分别为R=diag［9，0.25，9，0.25…］，采样时间间隔T=0.1 s。第1组静态实验，钟差的噪声功率谱密度取值为σb=0，钟漂的噪声功率谱密度取值为σd=0.25，位置的噪声功率谱密度σp=0，速度的噪声功率谱密度σv=1-4；第2组实验，设置载体以1 m/s恒定速度做匀速圆周运动，运动半径为10 m，其参数设置和静态场景设置相同；第3组数据突变实验，突变的数据来源于加州大学河滨分校实际采集的数据［13］，采样时间长度是51 s时长的数据，前0～7.3 s数据为稳定起始收敛时间段，在6 s时数据较大跳动，在7.3 s开始收敛，其参数设置如下σb= 0，σd=0.25，σp=0，σv=9；自适应渐消扩展卡尔曼滤波的渐消因子初始取值为λ=1。

3.1 LS、EKF、AFEKF静态未突变场景比较

数据从开始定位，LS、EKF和自适应渐消扩展卡尔曼滤波（AFEKF）在静态场景情况下，分别是其X、Y、Z方向上的定位结果误差见图1～图3所示。

图1　x方向的定位结果误差　图2　y方向的定位结果误差

图3　z方向的定位结果误差

从图1～图3中可以看出，LS的定位误差有较大的扰动，AFEKF定位结果和EKF相比，定位效果很接近。

3.2 EKF、AFEKF匀速圆周运动场景比较

在匀速圆周运动情况下，EKF和AFEKF其X、Y、Z方向上的定位结果误差如图4所示。

图4 x、y、z方向的定位结果误差

从图4可以看出，EKF波动较大，不稳定性强，而AFEKF稳定性强。在匀速圆周运动中，载体速度大小不会变化，而速度方向在时刻变化，此时EKF建立的静态模型难以复合复杂运动的场景，而AFEKF通过自适应系统噪声能稳定工作。

3.3 LS、EKF、AFEKF动态突变场景比较

数据从开始定位，EKF和自适应渐消扩展卡尔曼滤波（AFEKF）起始收敛时间是从第7.3 s开始，图5～图7分别是X、Y、Z方向上的定位结果误差图。

图5 x方向的定位结果误差图6 y方向的定位结果误差

图7 z方向的定位结果误差

从图5～图7中可以看出，LS的定位误差有较大的扰动，在有突变的情况下，EKF和AFEKF处理效果，比LS效果要好；AFEKF在收敛性上，较EKF更能收敛到更加准确的值。结合图5～图7，可以看出，起始时间0 s～3.8 s内，数据未发生跳变其EKF和 AFEKF效果相同，其稳定性比最小二乘好；结合图5～图7及表1中，12 s～51 s内AFEKF、EKF误差均值比LS效果要好，AFEKF定位误差均值比EKF好。

表1　数据稳定后（12 s～51 s）的LS、EKF和AFEKF比较

3.4传统FEKF、未自适应系统噪声FEKF、自适应

系统噪声AFEKF比较

下面是在数据突变的场景，传统FEKF、未自适应系统噪声FEKF、自适应系统噪声AFEKF算法定位结果对比图，图8～图10分别是X、Y、Z方向上的定位结果误差图。

图8　x方向的定位结果误差　图9　y方向的定位结果误差

图10　z方向的定位结果误差

从图8～图10中可以看出，在起始时间段传统FEKF、未自适应系统噪声FEKF、自适应系统噪声AFEKF算法定位效果相差不大。在6 s时刻，数据出现突变，此时传统FEKF、未自适应系统噪声FEKF出现较大的突变，收敛稳定后出现较大振动，而AFEKF抑制突变效果明显，其稳定性高于传统FEKF、未自适应系统噪声FEKF。相比传统渐消扩展卡尔曼，该算法稳定性更强，其整体定位精度也有所提高。

4　结论

在卫星导航定位解算中，本文提出一种自适应渐消扩展卡尔曼滤波方法应用于数据突变或状态突变，通过自适应渐消系统噪声来实现突变的抑制。根据实际的数据进行试验，结果表明在没有突变的情况下，自适应渐消扩展卡尔曼滤波定位效果和渐消FEKF、EKF效果一致，比最小二乘定位效果更好；在数据出现突变时，该算法定位精度比扩展卡尔曼滤波要好；相比传统渐消、未自适应系统噪声渐消扩展卡尔曼滤波，其稳定性和整体定位精度都有所提高。因此，本文提出的自适应系统噪声渐消扩展卡尔曼滤波方法针对数据出现突变的情况，能为卫星导航定位中实时快速稳定收敛提供一定参考。

参考文献：

［1］夏启军，孙优贤，周春晖.渐消卡尔曼滤波器的最佳自适应算法及其应用［J］.自动化学报，1990，16（3）:210-216.

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

［3］徐定杰，贺瑞，沈锋，等.基于新息协方差的自适应渐消卡尔曼滤波器［J］.系统工程与电子技术，2011，33（12）:2696-2699.

［4］马龙，李晓明.基于GPS/INS组合导航的改进自适应渐消卡尔曼滤波算法［J］.科学技术与工程，2013，13（33）:9973-9977.

［5］徐天河，杨元喜.改进的Sage自适应滤波方法［J］.测绘科学，2000，25（3）:22-24+2-1.

［6］王冲，曾庆军.基于Sage-husa自适应滤波算法的AUV组合导航系统设计［J］.电子设计工程，2013，21（15）:75-77+80.

［7］魏伟，秦永元，张晓冬，等.对Sage-Husa算法的改进［J］.中国惯性技术学报，2012，20（6）:678-686.

［8］MEHRA R.Approaches to adaptive filtering［J］.IEEE Trans.On Automatic Control.1972，17（5）：693-698.

［9］赵琳.非线性系统滤波理论［M］.北京：国防工业出版社，2012.

［10］GENG Y，WANG J.Adaptive estimation of multiple fading factors in Kalman filter for navigation applications［J］.GPS Solutions，2008，12（4）:273-279.

［11］KIM K H，LEE J G，PARK C G.Adaptive two-stage extended Kalman filter for a fault-tolerant INS-GPS loosely coupled system［J］.Aerospace and Electronic Systems，IEEE Transactions on，2009，45（1）:125-137.

［12］MAO X C，WADA M，HASHIMOTO H.Investigation on nonlinear filtering algorithms for GPS［C］// Proc.of Intelligent Vehicle Symposium，Versailles，France:IEEE，2002:64-70.

［13］鲁郁.GPS全球定位接收机—原理与软件实现［M］.北京：电子工业出版社，2009.

Based on Adaptive Extended Kalman Filter Positioning Module

HU Hui1，PENG Xiong-ming1，YANG De-jin2，LI Yang-da1
（1.School of Information Engineering，East China Jiaotong University，Nanchang 330013，China；2.National Time Service Center，Chinese Academy of Sciences，Xi’an 710000，China）

Abstract：In GPS positioning computation，when the actual data is changed，the Least Squares，the Extended Kalman Filter positioning results have low precision and poor stability problems.This paper proposes an Adaptive Fading Extended Kalman Filtering algorithm，through adaptive iterative system noise covariance，the modification to achieve less data mutation.Experimental results show that the algorithm is compared with the Least Squares，the Extended Kalman Filter，the positioning accuracy is improved.Compared with the traditional Fading Extended Kalman Filter，its convergence speed and stability are improved.

Key words：GPS，adaptive fading，extended kalman filter，positioning accuracy，stability

作者简介：胡辉（1970-），男，江西南昌人，博士，教授。研究方向：卫星导航定位，并行算法与分析处理，机器视觉。

*基金项目：江西省自然科学基金（20142BAB207001）；江西省教育厅科学技术基金资助项目（GJJ14369）

收稿日期：2015-03-16修回日期：2015-05-11

文章编号：1002-0640（2016）03-0177-06

中图分类号：TN96

文献标识码：A