一种应用于gps导航的自适应卡尔曼滤波算法
【技术领域】
[0001 ]本发明属于GPS导航技术领域，特别涉及一种应用于GPS导航的自适应卡尔曼滤波 算法，可用于GPS导航系统中的数据处理。
【背景技术】
[0002] 随着精准农业的迅猛发展，以先进科学技术为主要支撑的智能农业成为农业生产 中的中坚力量，在大幅度减轻农民工作强度和伤害的同时，也大大地提高了工作效益和劳 动生产率。然而，要实现农业设备的精准作业，主要依赖于其设备对农田位置的精确感知， GPS(Global positioning System)作为农田位置感知的首选方式，在现代农业生产中得到 了极其广泛的应用。精准农业要求GPS有更高的跟踪精度、更准的目标识别能力、更强的抗 干扰能力、更快的反应速度。因此，GPS导航需要以智能、稳健、高效的工作模式来应对这些 挑战。
[0003] GPS是一种无线电导航定位系统，它利用导航卫星进行测时和测距，确定用户在空 间特定坐标系中的位置。但是在GPS导航过程中包含许多误差，使定位精度受到严重的影 响，并且利用传统的方法很难消除。而GPS自适应跟踪滤波是消除GPS定位跟踪误差的重要 方法，即利用特定的滤波方法消除导航过程中的各种误差，从而达到提高GPS导航跟踪精度 的目的。近些年来，卡尔曼滤波理论被普遍应用于动态定位数据处理中，尤其在GPS实时定 位跟踪方面。卡尔曼滤波技术是20世纪60年代产生的一种估计技术，是一种线性最小方差 滤波估计，它只需要通过上一时刻状态向量的最优估计和当前时刻的观测量就可以得到现 在时刻状态向量的最优估计值。
[0004] 从卡尔曼滤波算法诞生到现在，GPS导航定位的数据处理主要围绕多模型的自适 应处理方法和基于新息序列的自适应处理方法等两方面展开研究。目前，在国际刊物上发 表的较为有效的方法是X.Rong Li等学者采用图论的思想，提出了变结构的多模型 (Variable Structrure Multiple Model，VSMM)跟踪算法。该算法首先要建立一个模型库， 其中包含目标可能出现的各种状态模型，利用模型的先验概率和转移概率来计算每一个滤 波器的交互估计，且多个模型间的交互比较灵活。但是，此算法的性能在很大程度上依赖于 所使用的模型集，即为了提高估计性能就需要用更多的模型来匹配目标的各种运动状态。
[0005] 另外，现代农业要求使用高精度、低成本的定位跟踪算法。对于精准农业生产中的 机动目标来说，它们的方位、距离和径向速度有可能发生不同程度的变化，或快或慢。目前 的单模型机动目标跟踪算法不能做到自适应的跟踪，而多模型机动目标跟踪算法在跟踪弱 机动目标时，会有很多额外的模型不仅会大大增加系统的运算量，还会降低跟踪器的性能， 造成了资源的浪费。因此，这些算法很难直接应用在精准农业的GPS跟踪系统中。
【发明内容】
[0006] 为了克服上述现有技术的缺点，本发明的目的在于提供一种应用于GPS导航的自 适应卡尔曼滤波算法，基于六1?(4111:〇代〖^88；^6,41〇模型，通过设计滤波跟踪算法，实现6?3 系统中对运动目标的自适应系统。
[0007] 为了实现上述目的，本发明采用的技术方案是：
[0008] 一种应用于GPS导航的自适应卡尔曼滤波算法，包括如下步骤：
[0009] (1)设置模型参数:状态变量的维数M，多项式的阶数N，且满足Μ 2 N+2，状态噪声方 差初始矩阵Q〇，并对目标状态的估计均值xk-i | k-dP协方差Ph |k-i进行初始化，k表示离散时 间的采样点；初始状态估计XQ|Q=[Z() z-1…z-Μ+ι]τ，其中Zi(i = -M+l，-M+2，'"，-l，0)为滤 波器起始之前各个时刻的目标位置观测量；
[0010] (2)建立AR模型，在卡尔曼滤波框架下计算出AR模型系数，具体实施过程如下：
[0011] (2a)根据AR模型的定义，建立AR模型系数约束的表达式；
[0012] (2b)计算AR模型系数；
[0013] (2c)计算状态转移矩阵Fk|k-1;
[0014] (3)由步骤(2)得到Fki^的值，利用基于AR模型的卡尔曼滤波来预测目标状态，目 标状态包括均值和误差协方差；
[0015] (4)利用测量数据更新目标状态，并且计算增益矩阵、后验估计均值以及后验估计 误差协方差；
[0016] (5)在线自适应地计算状态噪声协方差，输出目标位置的估计均值和估计误差协 方差，并且令k增加1，返回进行步骤2。
[0017] 本发明首先将AR模型套入到卡尔曼滤波算法中，通过最小均方误差准则计算出最 优AR模型的系数;然后利用基于AR模型的卡尔曼滤波算法对目标状态进行估计，并且利用 新息序列在线自适应地计算状态噪声的协方差，以更好地适应目标机动的大小。
[0018] 与现有技术相比，本发明具有以下优点：
[0019] (1)本发明提出了一种用于目标导航的预测模型一一AR模型，属于一种自适应的 状态模型。在滤波过程中，此模型不仅能满足目标运动状态的多项式约束，还能利用额外的 自由度滤除噪声；
[0020] (2)本发明结合基于AR模型的卡尔曼滤波算法，利用新息序列在线计算状态噪声 的协方差矩阵，可以更加精确地估计目标的运动状态。
【附图说明】
[0021] 图1是本发明的流程图。
[0022] 图2是本发明计算状态噪声协方差原理图。
[0023] 图3是本发明仿真实验在算法参数匹配情况下各模型跟踪效果图。
[0024] 图4是本发明仿真实验在算法参数不匹配情况下各模型跟踪效果图。
[0025] 图5是本发明现场测试实验车运动的轨迹。
[0026] 图6是本发明现场测试中两种自适应算法在X方向的位置误差图。
[0027] 图7是本发明现场测试中两种自适应算法在Y方向的位置误差图。
【具体实施方式】
[0028] 下面结合附图和实施例详细说明本发明的实施方式。
[0029] 如图1所示，本发明是一种应用于GPS导航的自适应卡尔曼滤波算法，包括如下步 骤：
[0030] 步骤1设置模型参数:状态变量的维数M，多项式的阶数N，且满足M2 N+2,状态噪声 方差初始矩阵Q〇，并对目标状态的估计均值χη |k-dP协方差Pk-i |k-i进行初始化，k表示离散 时间的采样点；初始状态估计Χ(φ=[ζο z-1…z-Μ+ι]τ，其中Zi(i=-M+l，-M+2,…，-1，0)为 滤波器起始之前各个时刻的目标位置观测量；
[0031] 步骤2建立AR模型，套用卡尔曼滤波框架，并且计算出AR模型系数；
[0032]该步骤的具体实施过程如下：
[0033] (2a)建立AR模型，AR模型系数的约束为
[0034] Auk = b <1>
[0035]其中AR模型的系数Uk=[hi h2…1?1]1'，1^(丨=1，2，'"，]\〇为此滤波器的系数沁=
[1 0 …0]T，A为（N+1)XM维Vandermonde矩阵
[0036]
[0037] (2b)计算AR模型系数
[0038] 根据最小均方误差准则，求得AR模型系数向量uk的最优解《!，计算表达如式〈3>:
[0039]
[0040] (2c)计算状态转移矩阵Fk | k-i，把(2b)得到的最优解《:代入Fk | k-i计算式中，Fk | k-i的 表达如式〈4>:
[0041]
[0042] 步骤3由步骤2得到Fkiw，利用基于AR模型的卡尔曼滤波来预测目标状态；
[0043] (3a)滤波器预测模型
[0044] 目标在k+Ι时刻的状态向量为：
[0045] xk+i=Fk+i|kXk+Wk <5>
[0046] 其中Xk=[Xk Xk-!…xk-Μ+1]τ，为目标k时刻之前Μ个采样点的位置值，其中F k+1|k为 状态转移矩阵，Wk是状态噪声，即Wk=[Wk Wk-1…Wk-M+1]T，代表各时刻目标位置的随机波 动，它是一个独立同分布的零均值高斯序列，即w k~N(0，Qk)，假定不同时刻目标的位置波动 相互独立，则AR模型中的状态噪声协方差表示为：
[0047] Qk = E[wk(wk)T] = qr A t · I <6>
[0048] 其中，qr为目标位置的状态噪声强度，△ t为采样间隔，I为M维单位矩阵；
[0049] (3b)预测目标状态包括预测均值和预测误差：
[0050] xk|k-i = Fk|k-1 · xk-i|k-1 <7>
[0051]
[0052] 其中xk|k-1为k-1时刻目标状态的预测均值，Pk|k-Ak-l时刻预测误差，F k|k-i由式〈4 >给出；
[0053] 步骤4,使用测量数据更新目标状态，并且计算增益矩阵、后验估计均值以及后验 估计误差协方差；
[0054] (4a)滤波器观测模型
[0055]目标在k时刻的位置观测值可由
[0056] Zk = Hxk+vk 〈9>得到。其中观测矩阵H=[l 0…0]1XM，Vk是观测噪声，代表各时 刻目标位置的观测误差，vk也是一个白的高斯随机过程，与状态噪声wk相互独立，即vk~N (0，Rk)，其中Rk为观测噪声的协方差矩阵，由于观测向量^只有一个元素，因此观测噪声协 方差矩阵Rk也只有一个元素，表示出来的状态模型和观测模型都是线性的，因此卡尔曼滤 波算法能够提供最小均方误差意义下的最优状态估计；
[0057] (4b)更新目标状态
[0058] 使用测量数据更新目标状态，包括增益矩阵、估计