消除模型误差的卡尔曼滤波在GPS定位中的应用研究

张兰勇，陈辉煌，孟 坤

(1.哈尔滨工程大学 自动化学院， 黑龙江 哈尔滨 150001) (2.东南大学 毫米波国家重点实验室， 江苏 南京 210096)

如今车载GPS定位导航已经广泛应用。随着技术的提高和应用需求的不断扩大，而后出现的差分GPS(DGPS)已经能够很好的克服伪距测量中出现的误差，如卫星时钟误差、星历误差等，使GPS定位精度提高到分米级别。但是，DGPS需要建立基站，成本高，存在基站和移动站通信半径限制，而且在GPS测量误差中仍然存在着由于接收设备自身产生的热噪声颤动和干扰的影响。

针对卡尔曼滤波降低误差的研究，国内外学者取得了比较显著的成果。比如，孙罡等[1]通过改良参数和稳定模型的方法获得静态的精准的GPS定位；SHI E[2]通过获得大量数据，实现动态的精准的GPS定位；王虎等[3]根据不同的情况建立不同的卡尔曼滤波器，保证了动态定位的动态性和准确性。现有的卡尔曼滤波器要么无法适用于动态定位，要么不具有普适性。

本文在前人研究基础上，以农用机动车辆的GPS定位为研究对象。设计卡尔曼滤波器既能适用于动态定位，又具有广泛应用性，该滤波器同样适用DGPS。卡尔曼滤波器通过建立状态方程和量测方程描述系统的动态过程。它具有实时数据处理，无需存储数据以及最小无偏差性等优点，是当前应用最广的一种动态数据处理方法[4]。

1 卡尔曼滤波器建模

卡尔曼滤波要求对应用对象数学建模，而且模型的精确程度将影响滤波结果的优劣。

本文针对农垦联合收割机建立相应的数学模型。在农用机械实际作业时，由于机械运动速度较低，相对于GPS接收机的采样频率而言，通过车载的定位系统和车辆的位移变化就能够计算出车辆的实时位置。下面建立经度方向的滤波递推方程，纬度方向同理[5-6]。

设采样时间为T，数学模型如下：

Xk=φk,k-1Xk-1+Wk-1

(1)

ZGPSE(k)=Xk+Vk

(2)

其中WK和Vk是零均值的白噪声随机向量序列，即

E{Wk}=0

(3)

Rww(k,j)=Qkδkj

(4)

E{Vk}=0

(5)

Rvv(k,j)=Rkδkj

(6)

式(1)中，Xk为k时刻车辆的状态：Xk=[EkVk]，Ek为k时刻车辆的位置经度，Vk为k时刻车辆的向东的速度分量；φk,k-1为状态转移矩阵，φk,k-1=[1a]，a为车辆位移相对于位置经度的变化参数

(7)

2 卡尔曼滤波器设计

根据上述的数学模型建立卡尔曼滤波器模型。

(8)

(9)

(10)

Kk=Pk/k-1[Pk/k-1+Rk]-1

(11)

(12)

Pk/k=[I-Kk]Pk/k-1

(13)

本文利用卡尔曼滤波尽量准确地估计出系统的真实状态，其必要前提是滤波过程具有某种稳定性，从而证明实际的滤波器有效[7]。由于模型可能存在错误，仅根据建立的系统状态模型是无法判断实际的滤波状态估计是否正确，因此只能根据系统的量测估计误差来间接地检验这种滤波的有效性[8]。为此，本文从实际的滤波效果出发定义这种特性。

量级上与正常量测值相差很大或者误差超越了正常状态所允许的误差范围的，工程上称之为野值。必须先将这些野值剔除掉，否则会给数据处理带来很大的误差。下面讨论野值的自动剔除方法。

设对状态Xk的测量值为Z1，Z2，…，Zk-1，…，预测值为Xk/k-1，预测残差为dk，有：

dk=ZK-XK/K-1

(14)

其协方差矩阵为：

(15)

(16)

(17)

式(17)中，Pk/k-1为预测误差协方差矩阵；RK为量测噪声协方差矩阵。利用预测残差的统计性质可对ZK的每个分量进行判别，判别式为：

(18)

式(18)中，Rk(i,i)表示对角线上的第i个元素；dk(i)表示dK的第i个分量，C为常量，可取3或4。如果上式成立，判别Zk(i)为正确的测量；反之，判别Zk(i)为野值，其中Zk(i)为Zk的第i个分量。在递推滤波过程中，野值进行自动剔除。其基本思想是：当判别Zk(i)为野值时，在递推滤波求得增益矩阵Kk后，在软件中自动把Kk的第i行元素全部冲为零，计算滤波值Xk/k和滤波误差协方差矩阵Pk/k的公式不变[9]。

此方法在应用过程中最重要的特点就是快速。在实际应用中可以用到稳态滤波，将Pk/k和Pk/k-1取为固定值，并利用软件程序自动剔除野值点[10]。

图1 卡尔曼滤波算法流程

在准确知道数学模型基础上，可以采用以上模型。然而实际情况很难准确知道，因此滤波的效果会出现瑕疵。于是提出另一种卡尔曼滤波，能够消除模型误差以增强滤波的效果[11]。

对系统的非线性高斯特性进行分析，即系统噪声和测量噪声为不相关的高斯白噪声。但实际GPS测量中设备受到自身产生的热噪声以及环境的影响[12]。而基于空间的卡尔曼滤波要求线性系统，所以本文的自适应卡尔曼滤波利用非线性函数一阶泰勒级数展开思想，通过求运动过程方程和运动观测过程的偏导数观测数据，实现非线性系统方程近似的滤波算法，满足系统的线性要求，再利用卡尔曼滤波进行处理。

3 自适应卡尔曼滤波及仿真

针对数学建模中存在误差，本部分主要讨论消除误差方法。结合上述提出的数学模型，由于车辆在机动状态下噪声干扰可能是有色噪声，而非理想的白噪声；再者，匀速的数学模型在动态GPS定位中未必能够表达车辆的状态等[13]。基于此，本文引入改进措施：通过引进虚拟噪声，把问题归结为带未知变噪声统计系统的卡尔曼滤波自适应滤波问题。

假定第一节的数学模型式(1)、式(2)含有未知的模型误差Δφk,k-1和ΔHk，即真实系统的数学模型为

Xk=(φk,k-1+Δφk,k-1)Xk-1+Wk

(19)

ZGPSE(k)=(I+ΔHk)Xk+Vk

(20)

令

ηk-1=Δφk,k-1Xk-1+Wk-1

(21)

εk=ΔHkXk+Vk

(22)

称ηk-1和εk为虚拟噪声，其均值和协方差阵是时变的，即

Eηk=μη(k)

cov(ηk,ηj)=Q(k)δkj

Eεk=με(k)

(23)

cov(εk,εj)=R(k)δkj

(24)

按照文献[4]中介绍的方法推导，得出自适应卡尔曼滤波公式为

(25)

(26)

(27)

(28)

(29)

(30)

(31)

(32)

(33)

式中，dk=(1-b)/(1-bk+1)，0

对于模型中的误差Δφk,k-1Xk-1，一般考虑是不稳定的加速度模型。仿真中将一定范围内随机的加速度值加入到模型中，为了不增加状态的维数模型，将这一部分算为虚拟误差，由于虚拟误差是时变的，事先不可知，因此采用上述的自适应卡尔曼滤波算法将噪声的协方差和期望实时算出。

消除模型误差的卡尔曼滤波与一般卡尔曼滤波[14]的最大不同在于：算法中需要计算动态误差的期望和方差，它们不再是常值矩阵。当然，这种滤波方法也可用于时变噪声的滤波问题中。

基于以上的研究，本文使用软件建模仿真。包括一般卡尔曼滤波算法和消除误差的自适应卡尔曼滤波算法。仿真结果如图2，图3所示。

图2 一般卡尔曼滤波仿真曲线

图3 自适应滤波仿真曲线

图2是一般卡尔曼滤波算法的仿真曲线。图中虚线是根据数学模型计算的真实值，实线是由滤波算法得到的估计值。估计误差等于真实值与估计值的差。仿真结果表明，在200s的仿真时间内，位置误差在[-2,2]之间。卡尔曼滤波递推算法能够比较精确的确定模型的位置，有效的去除了随机噪声。

图3是消除误差的卡尔曼滤波算法仿真曲线。卡尔曼滤波的推算结果与数学模型的准确性有关，而数学模型的建立存在误差。为了更进一步提高估计值的精度，将误差模型考虑在内并进行了仿真。为进一步证明本文自适应卡尔曼滤波的有效性，将本文方法和一般卡尔曼滤波以及改进卡尔曼滤波仿真结果进行对比。从图3可见，200 s的自适应滤波后误差在[-1,1]之间，与一般滤波相比，误差减小、滤波效果加强。

表1 卡尔曼滤波和自适应卡尔曼滤波仿真误差结果 %

将仿真结果进行统计得表1，可以看到各误差段所占时间比。一般卡尔曼滤波在微小误差段0～±0.2 m所占比较低，而本文的自适应卡尔曼滤波的微小误差段所占比例极大增加，改进卡尔曼滤波在一定程度上缩小了误差，但效果明显不如自适应卡尔曼滤波。同时，一般卡尔曼滤波误差较大的所占时间较多，而自适应卡尔曼滤波基本消除了大误差，误差仅为一般卡尔曼滤波器误差的34%。

经过200 s仿真证明上述选用的自适应滤波与GPS定位组合，能够除去噪声干扰并解决模型参数漂移引起的误差，将定位精度进一步提高。运用单点GPS无法消除伪距误差，所以仿真结果中定位精度不理想，若能够与DGPS联合使用，效果将更加明显。在实际应用中收割机的运动速度缓慢，完全能够达到实时性的要求。

4 结论

本文将GPS定位与卡尔曼滤波结合，建立车辆无规则运动数学模型。不仅降低了模型维数，在保证可观性的同时，降低了算法的复杂度，提高实时性和降低了成本。而在算法方面，首先采用一般的滤波器算法和防止发散的方法仿真，其次针对卡尔曼滤波应用的实际问题，引入消除误差模型的卡尔曼滤波，进一步提高了定位精度，与以往采用的卡尔曼滤波相比，本文方法在去除误差方面给出了更加有效，对于农业车辆的定位已经足够。这样不仅降低了车辆导航定位的成本，而且增强了系统的实用性。

[1] 孙罡,王昌明,张爱军.GPS静态单点定位的滤波算法比较[J].南京理工大学学报(自然科学版),2011,35(1):80-85.

[2] SHI E.An improved real-time adaptive Kalman filter for low-cost integrated GPS/INS navigation[C]// International Conference on Measurement,Information and Control.IEEE,2012:1093-1098.

[3] 王虎,王解先,白贵霞,等.改进的渐消卡尔曼滤波在GPS动态定位中的应用[J].同济大学学报(自然科学版),2011,39(1):124-128.

[4] 胡振涛,胡玉梅,刘先省.量测提升卡尔曼滤波[J].电子学报,2016,44(5):1149-1155.

[5] LIU Y,GUO S,HOU R.Research on Kalman Filter in Error Processing of GPS[J].Electronic Science & Technology,2011.

[6] FAKHARIAN A,GUSTAFSSON T.Adaptive Kalman filtering based navigation[C]// IEEE International Conference on Networking,Sensing and Control.2015:181-185.

[7] 赵欣,王仕成,廖守亿,等.基于抗差自适应容积卡尔曼滤波的超紧耦合跟踪方法[J].自动化学报,2014,40(11):2530-2540.

[8] 刘胜,张青春,张兰勇.聚类算法的GPS静态单点定位方法[J].哈尔滨工业大学学报,2012,44(11):71-74.

[9] 王忠,游志胜,杜传利,等.GPS/INS动态卡尔曼滤波优化算法[J].四川大学学报(工程科学版),2006,38(4):141-144.

[10] ZHOU Z,LI Y,LIU J,et al.Equality Constrained Robust Measurement Fusion for Adaptive Kalman-Filter-Based Heterogeneous Multi-Sensor Navigation[J].IEEE Transactions on Aerospace &Electronic Systems,2013,49(4):2146-2157.

[11] CARVALHO J P D,ASSAD E D,PINTO H S.Kalman filter and correction of the temperatures estimated by PRECIS model[J].Atmospheric Research,2011,102(1):218-226.

[12] 葛磊.容积卡尔曼滤波算法研究及其在导航中的应用[D].哈尔滨工程大学,2013.

[13] BARRIOS C,MOTAI Y.Improving Estimation of Vehicle’s Trajectory Using the Latest Global Positioning System With Kalman Filtering[J].IEEE Transactions on Instrumentation & Measurement,2011,60(12):3747-3755.

[14] 张海,常艳红,车欢.基于GPS/INS不同测量特性的自适应卡尔曼滤波算法[J].中国惯性技术学报,2010,18(6):696-701.

[15] 胡辉，彭雄明，杨德进，等.新的自适应渐消扩展卡尔曼滤波在GPS定位中的应用[J].火力与指挥控制，2016(3):177-182.