CN105549049B

CN105549049B - 一种应用于gps导航的自适应卡尔曼滤波算法

Info

Publication number: CN105549049B
Application number: CN201510883751.7A
Authority: CN
Inventors: 靳标; 郭交; 王胜兰; 苏宝峰; 朱学卫
Original assignee: Northwest A&F University
Current assignee: Northwest A&F University
Priority date: 2015-12-04
Filing date: 2015-12-04
Publication date: 2018-10-02
Anticipated expiration: 2035-12-04
Also published as: CN105549049A

Abstract

本发明公开了一种应用于GPS导航的自适应卡尔曼滤波算法，主要解决现有的自适应卡尔曼滤波算法无法随着滤波过程自适应调节的问题，其过程是：(1)设置算法参数，并对目标状态进行初始化；(2)建立AR模型，套用卡尔曼滤波框架计算出AR模型的系数；(3)利用基于AR模型的卡尔曼滤波来预测目标状态，包括预测均值和误差协方差；(4)使用测量数据更新目标状态,计算增益矩阵、后验估计均值以及后验估计误差协方差；(5)在线自适应地计算状态噪声协方差，输出目标位置的状态值和协方差，令k增加1,返回步骤2；本发明与现有自适应卡尔曼滤波算法相比，能够实现更加精确的目标状态估计，可用于实际的GPS导航系统。

Description

一种应用于GPS导航的自适应卡尔曼滤波算法

技术领域

本发明属于GPS导航技术领域，特别涉及一种应用于GPS导航的自适应卡尔曼滤波算法，可用于GPS导航系统中的数据处理。

背景技术

随着精准农业的迅猛发展，以先进科学技术为主要支撑的智能农业成为农业生产中的中坚力量，在大幅度减轻农民工作强度和伤害的同时，也大大地提高了工作效益和劳动生产率。然而，要实现农业设备的精准作业，主要依赖于其设备对农田位置的精确感知，GPS(Global positioning System)作为农田位置感知的首选方式，在现代农业生产中得到了极其广泛的应用。精准农业要求GPS有更高的跟踪精度、更准的目标识别能力、更强的抗干扰能力、更快的反应速度。因此，GPS导航需要以智能、稳健、高效的工作模式来应对这些挑战。

GPS是一种无线电导航定位系统，它利用导航卫星进行测时和测距，确定用户在空间特定坐标系中的位置。但是在GPS导航过程中包含许多误差，使定位精度受到严重的影响，并且利用传统的方法很难消除。而GPS自适应跟踪滤波是消除GPS定位跟踪误差的重要方法，即利用特定的滤波方法消除导航过程中的各种误差，从而达到提高GPS导航跟踪精度的目的。近些年来，卡尔曼滤波理论被普遍应用于动态定位数据处理中，尤其在GPS实时定位跟踪方面。卡尔曼滤波技术是20世纪60年代产生的一种估计技术，是一种线性最小方差滤波估计，它只需要通过上一时刻状态向量的最优估计和当前时刻的观测量就可以得到现在时刻状态向量的最优估计值。

从卡尔曼滤波算法诞生到现在，GPS导航定位的数据处理主要围绕多模型的自适应处理方法和基于新息序列的自适应处理方法等两方面展开研究。目前，在国际刊物上发表的较为有效的方法是X.Rong Li等学者采用图论的思想，提出了变结构的多模型(Variable Structrure Multiple Model，VSMM)跟踪算法。该算法首先要建立一个模型库，其中包含目标可能出现的各种状态模型，利用模型的先验概率和转移概率来计算每一个滤波器的交互估计，且多个模型间的交互比较灵活。但是，此算法的性能在很大程度上依赖于所使用的模型集，即为了提高估计性能就需要用更多的模型来匹配目标的各种运动状态。

另外，现代农业要求使用高精度、低成本的定位跟踪算法。对于精准农业生产中的机动目标来说，它们的方位、距离和径向速度有可能发生不同程度的变化，或快或慢。目前的单模型机动目标跟踪算法不能做到自适应的跟踪，而多模型机动目标跟踪算法在跟踪弱机动目标时，会有很多额外的模型不仅会大大增加系统的运算量，还会降低跟踪器的性能，造成了资源的浪费。因此，这些算法很难直接应用在精准农业的GPS跟踪系统中。

发明内容

为了克服上述现有技术的缺点，本发明的目的在于提供一种应用于GPS导航的自适应卡尔曼滤波算法，基于AR(Autoregressive，AR)模型，通过设计滤波跟踪算法，实现GPS系统中对运动目标的自适应系统。

为了实现上述目的，本发明采用的技术方案是：

一种应用于GPS导航的自适应卡尔曼滤波算法，包括如下步骤：

(1)设置模型参数：状态变量的维数M，多项式的阶数N，且满足M≥N+2，状态噪声方差初始矩阵Q₀，并对目标状态的估计均值x_k-1|k-1和协方差P_k-1|k-1进行初始化，k表示离散时间的采样点；初始状态估计x_0|0＝[z₀ z_-1 … z_-M+1]^T，其中z_i(i＝-M+1,-M+2,…,-1,0)为滤波器起始之前各个时刻的目标位置观测量；

(2)建立AR模型，在卡尔曼滤波框架下计算出AR模型系数，具体实施过程如下：

(2a)根据AR模型的定义，建立AR模型系数约束的表达式；

(2b)计算AR模型系数；

(2c)计算状态转移矩阵F_k|k-1；

(3)由步骤(2)得到F_k|k-1的值，利用基于AR模型的卡尔曼滤波来预测目标状态，目标状态包括均值和误差协方差；

(4)利用测量数据更新目标状态，并且计算增益矩阵、后验估计均值以及后验估计误差协方差；

(5)在线自适应地计算状态噪声协方差，输出目标位置的估计均值和估计误差协方差，并且令k增加1，返回进行步骤2。

本发明首先将AR模型套入到卡尔曼滤波算法中，通过最小均方误差准则计算出最优AR模型的系数；然后利用基于AR模型的卡尔曼滤波算法对目标状态进行估计，并且利用新息序列在线自适应地计算状态噪声的协方差，以更好地适应目标机动的大小。

与现有技术相比，本发明具有以下优点：

(1)本发明提出了一种用于目标导航的预测模型——AR模型，属于一种自适应的状态模型。在滤波过程中，此模型不仅能满足目标运动状态的多项式约束，还能利用额外的自由度滤除噪声；

(2)本发明结合基于AR模型的卡尔曼滤波算法，利用新息序列在线计算状态噪声的协方差矩阵，可以更加精确地估计目标的运动状态。

附图说明

图1是本发明的流程图。

图2是本发明计算状态噪声协方差原理图。

图3是本发明仿真实验在算法参数匹配情况下各模型跟踪效果图。

图4是本发明仿真实验在算法参数不匹配情况下各模型跟踪效果图。

图5是本发明现场测试实验车运动的轨迹。

图6是本发明现场测试中两种自适应算法在X方向的位置误差图。

图7是本发明现场测试中两种自适应算法在Y方向的位置误差图。

具体实施方式

下面结合附图和实施例详细说明本发明的实施方式。

如图1所示，本发明是一种应用于GPS导航的自适应卡尔曼滤波算法，包括如下步骤：

步骤1设置模型参数：状态变量的维数M，多项式的阶数N，且满足M≥N+2，状态噪声方差初始矩阵Q₀，并对目标状态的估计均值x_k-1|k-1和协方差P_k-1|k-1进行初始化，k表示离散时间的采样点；初始状态估计x_0|0＝[z₀ z_-1 … z_-M+1]^T，其中z_i(i＝-M+1,-M+2,…,-1,0)为滤波器起始之前各个时刻的目标位置观测量；

步骤2建立AR模型，套用卡尔曼滤波框架，并且计算出AR模型系数；

该步骤的具体实施过程如下：

(2a)建立AR模型，AR模型系数的约束为

Au_k＝b <1>

其中AR模型的系数u_k＝[h₁ h₂ … h_M]^T，h_i(i＝1，2，…，M)为此滤波器的系数，b＝[1 0 … 0]^T，A为(N+1)×M维Vandermonde矩阵

(2b)计算AR模型系数

根据最小均方误差准则，求得AR模型系数向量u_k的最优解计算表达如式<3>:

(2c)计算状态转移矩阵F_k|k-1，把(2b)得到的最优解代入F_k|k-1计算式中，F_k|k-1的表达如式<4>：

步骤3由步骤2得到F_k|k-1，利用基于AR模型的卡尔曼滤波来预测目标状态；

(3a)滤波器预测模型

目标在k+1时刻的状态向量为：

x_k+1＝F_k+1|kx_k+w_k <5>

其中x_k＝[x_k x_k-1 … x_k-M+1]^T，为目标k时刻之前M个采样点的位置值，其中F_k+1|k为状态转移矩阵，w_k是状态噪声，即w_k＝[w_k w_k-1 … w_k-M+1]^T，代表各时刻目标位置的随机波动，它是一个独立同分布的零均值高斯序列，即w_k～N(0,Q_k)，假定不同时刻目标的位置波动相互独立，则AR模型中的状态噪声协方差表示为：

Q_k＝E[w_k(w_k)^T]＝q_rΔt·I <6>

其中，q_r为目标位置的状态噪声强度，Δt为采样间隔，I为M维单位矩阵；

(3b)预测目标状态包括预测均值和预测误差:

x_k|k-1＝F_k|k-1·x_k-1|k-1 <7>

其中x_k|k-1为k-1时刻目标状态的预测均值，P_k|k-1为k-1时刻预测误差，F_k|k-1由式<4>给出；

步骤4，使用测量数据更新目标状态，并且计算增益矩阵、后验估计均值以及后验估计误差协方差；

(4a)滤波器观测模型

目标在k时刻的位置观测值可由

z_k＝Hx_k+v_k <9>

得到。其中观测矩阵H＝[1 0 … 0]_1×M，v_k是观测噪声，代表各时刻目标位置的观测误差，v_k也是一个白的高斯随机过程，与状态噪声w_k相互独立，即v_k～N(0,R_k)，其中R_k为观测噪声的协方差矩阵，由于观测向量z_k只有一个元素，因此观测噪声协方差矩阵R_k也只有一个元素，表示出来的状态模型和观测模型都是线性的，因此卡尔曼滤波算法能够提供最小均方误差意义下的最优状态估计；

(4b)更新目标状态

使用测量数据更新目标状态，包括增益矩阵、估计均值以及估计误差协方差的计算，表达式如下：

K_k＝P_k|k-1·H^T·[HP_k|k-1H^T+R_k]^-1 <10>

x_k|k＝x_k|k-1+K_k·[z_k-Hx_k|k-1] <11>

P_k|k＝[I-K_k·H]·P_k|k-1 <12>

其中，K_k为k时刻的增益矩阵，x_k|k为k时刻目标状态估计均值，P_k|k为k时刻的目标状态估计误差协方差，x_k|k由式<7>给出，P_k|k-1由式<8>给出；

步骤5，在线自适应地计算状态噪声协方差，输出x_k|k和P_k|k；

(5a)利用新息序列计算状态噪声协方差

如图2所示，利用新息序列计算实际的新息协方差矩阵

其中d_k＝z_k-Hx_k|k-1，W为滑窗长度。考虑到实际的采样时刻k小于滑窗长度W的情况，由式<14>给出

(2)将理论的新息协方差S_k用实际的新息协方差代替，并用式<15>在线计算状态噪声的协方差：

(5b)输出x_k|k和P_k|k，并且令k增加1，返回进行步骤2。

本发明的效果可以通过以下仿真结果和现场测试进一步说明：

1.实验场景：

(1)仿真实验

假定只能得到目标距离的观测值，观测噪声的方差R＝100m²，采样间隔Δt＝1s，总的观测时间为100s。3个AR模型系数的个数分别为M＝2，3，4，并且3个AR模型阶数均为N＝1。M＝2的AR模型在图中用“AR模型0”表示。由于目标跟踪误差与状态噪声强度的设置密切相关，图3和图4分别给出了状态噪声强度分别为q_r＝0(与实际场景匹配)和q_r＝0.1(与实际场景不匹配)时的目标位置估计的均方根误差。

(2)现场测试

本次测试采用两套双频GPS接收机，其中一个GPS接收器被设置为静态参考，另一个放置在实验车的顶部。图5是实验车运动的轨迹。这次测试主要用N＝1，M＝3的基于AR模型的自适应卡尔曼滤波器(AKF-AR)和基于匀速(Constant Velocity，CV)模型的自适应卡尔曼滤波器(AKF-CV)进行比较，二者其他参数设置相同。

2.实验结果分析：

图3和图4分别给出了在状态噪声强度q_r＝0和q_r＝1.0情况下AR模型和CV模型的均方根误差。M＝2的AR模型(即“AR model 0”)与CV模型的跟踪误差相同，说明了N＝1，M＝2的AR模型与CV模型都可以描述匀速状态。然后将AR模型系数的个数增加，在满足目标运动状态约束的前提下以最小均方误差准则对AR模型系数进行求解，可以获得比CV模型更好的估计性能。M＝3，4的AR模型的估计误差确实要远小于CV模型。这是因为M＝3，4的AR模型不但能满足目标的运动状态，而且可以利用额外的自由度滤除噪声，提高了估计精度。另外从图中可以看出，M＝4的AR模型的估计性能要好于M＝3的AR模型。这是由于AR模型系数个数越大，可以利用的先验信息越多，从而预测精度越准确。但是在预测机动目标当前的运动状态时，并不是先验信息越多越好。综合考虑滤波精度和运算量，AR模型系数的个数可选择为N+1≤M≤5。整体对比图3和图4可以看出，当状态噪声的强度与目标实际状态匹配时，AR模型与CV模型的跟踪性能相差不大。但是当参数设置与目标实际运动状态不匹配时，AR模型的优势则更为明显。由于AR模型可以调整自己的参数设置为最佳性能，因此以上实验都表明AR模型的性能优于CV模型。

图6和图7给出了当滑窗长度W＝20时，AKF-AR算法和AKF-CV算法分别在X和Y两个方向的位置误差。可以看出，AKF-AR(N＝1，M＝3)的位置误差比AKF-CV要小得多。这是因为AKF-AR一方面可以实时地在线计算状态噪声协方差；另一方面它可以根据状态噪声协方差的波动调整状态模型。该算法在处理机动目标运动的动态性能方面比传统的算法更有优势。另外，在不同滑窗长度的情况下的算法的估计精度是不同的。因此需要根据实际情况来选择合适的滑窗长度。

Claims

1.一种应用于GPS导航的自适应卡尔曼滤波算法，其特征在于，包括如下步骤：

(1)设置模型参数：状态变量的维数M，多项式的阶数N，且满足M≥N+2，状态噪声方差初始矩阵Q₀，并对目标状态的估计均值x_k-1|k-1和协方差P_k-1|k-1进行初始化，k表示离散时间的采样点；初始状态估计x_0|0＝[z₀ z_-1 … z_-M+1]^T，其中z_i；i＝-M+1,-M+2,…,-1,0；为滤波器起始之前各个时刻的目标位置观测量；

(2a)根据AR模型的定义，建立AR模型系数约束的表达式Au_k＝b，其中u_k为AR模型系数，u_k＝[h₁ h₂ … h_M]^T，h_i为滤波器系数，i＝1,2,…,M，b＝[1 0 … 0]^T，A为(N+1)×M维Vandermonde矩阵，

(2b)计算AR模型系数向量u_k的最优解计算式为

(2c)计算状态转移矩阵F_k|k-1，

(4)利用实时测量的数据更新目标状态，并且计算增益矩阵、后验估计均值以及后验估计误差协方差；

(5)在线自适应地计算状态噪声协方差，输出目标位置的估计均值和估计误差协方差，并且令k增加1，返回进行步骤2；

其中在线自适应地计算状态噪声协方差的过程如下：

5.1利用新息序列计算实际的新息协方差矩阵

其中d_k＝z_k-Hx_k|k-1，W为滑窗长度；H为观测矩阵，H＝[1 0 … 0]_1×M，x_k|k-1为k-1时刻目标状态的预测均值；

5.2将理论的新息协方差S_k用实际的新息协方差代替，并用公式在线计算状态噪声的协方差，其中K_k为k时刻的增益矩阵。

2.根据权利要求1所述应用于GPS导航的自适应卡尔曼滤波算法，其特征在于，所述步骤(3)中预测目标状态的过程如下：

3.1建立滤波器预测模型

目标在k+1时刻的状态向量为x_k＝F_k|k-1x_k-1+w_k；

其中x_k＝[x_k x_k-1 ... x_k-M+1]^T为目标k时刻之前M个采样点的位置值，w_k是状态噪声，w_k＝[w_k w_k-1 ... w_k-M+1]^T，代表各时刻目标位置的随机波动，它是一个独立同分布的零均值高斯序列，即w_k～N(0,Q_k)，假定不同时刻目标的位置波动相互独立，则AR模型中的Q_k表示为：Q_k＝E[w_k(w_k)^T]＝q_r△t·I；

其中，q_r为目标位置的状态噪声强度，△t为采样间隔，I为M维单位矩阵；

3.2预测目标状态包括预测均值和误差协方差：

x_k|k-1＝F_k|k-1·x_k-1|k-1；

其中P_k|k-1为k-1时刻预测误差。

3.根据权利要求2所述应用于GPS导航的自适应卡尔曼滤波算法，其特征在于，所述步骤(4)中利用测量数据更新目标状态，过程如下：

4.1建立滤波器观测模型

目标在k时刻的位置观测值由z_k＝Hx_k+v_k得到，其中v_k是观测噪声，代表各时刻目标位置的观测误差，v_k是一个白的高斯随机过程，与状态噪声w_k相互独立，即v_k～N(0,R_k)，其中R_k为观测噪声的协方差矩阵，由于观测向量z_k只有一个元素，因此观测噪声协方差矩阵R_k也只有一个元素，表示出来的状态模型和观测模型都是线性的，因此卡尔曼滤波算法能够提供最小均方误差意义下的最优状态估计；

4.2更新目标状态

利用观测信息对预测值进行更新，得到目标状态的估计均值和估计协方差；

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1，

x_k|k＝x_k|k-1+K_k(z_k-Hx_k|k-1)，

P_k|k＝(I-K_kH)P_k|k-1，

其中，x_k|k为k时刻目标状态估计均值，P_k|k为k时刻的目标状态估计误差协方差。

4.根据权利要求1所述应用于GPS导航的自适应卡尔曼滤波算法，其特征在于，实际的采样时刻k小于滑窗长度W，