均值以及估计误差协方差的计 算，表达式如下：
[0059] Kk = Pk|k-1 · Ητ · [HPk|k-iHT+Rk]-1 <10>
[0060] Xk|k = xk|k-i+Kk · [Zk_Hxk|k-1] <11>
[0061] Pk|k=[I-Kk · H] · Pk|k-i <12>
[0062] 其中，Kk为k时刻的增益矩阵，xk|k为k时刻目标状态估计均值，P k|k为k时刻的目标 状态估计误差协方差，xk|k由式〈7>给出，PkiH由式〈8>给出；
[0063]步骤5，在线自适应地计算状态噪声协方差，输出xk | k和Pk | k;
[0064] (5a)利用新息序列计算状态噪声协方差
[0065] 如图2所示，利用新息序列计算实际的新息协方差矩阵
[0066]
[0067]其中dkzzk-Hxklk-^W为滑窗长度。考虑到实际的采样时刻k小于滑窗长度W的情 况，尾由式<H>给出 [0068]
[0069] (2)将理论的新息协方差Sk用实际的新息协方差夂代替，并用式〈15>在线计算状 态噪声的协方差：
[0070]
[0071 ] (5b)输出xk I k和Pk I k，并且令k增加 1，返回进行步骤2。
[0072]本发明的效果可以通过以下仿真结果和现场测试进一步说明：
[0073] 1.实验场景：
[0074] (1)仿真实验
[0075] 假定只能得到目标距离的观测值，观测噪声的方差R= 100m2,采样间隔At = Is， 总的观测时间为100s。3个AR模型系数的个数分别为M = 2，3，4，并且3个AR模型阶数均为N = 11 = 2的AR模型在图中用"AR模型0"表示。由于目标跟踪误差与状态噪声强度的设置密切 相关，图3和图4分别给出了状态噪声强度分别为q r = 0(与实际场景匹配)和qr = 0.1(与实际 场景不匹配)时的目标位置估计的均方根误差。
[0076] (2)现场测试
[0077]本次测试采用两套双频GPS接收机，其中一个GPS接收器被设置为静态参考，另一 个放置在实验车的顶部。图5是实验车运动的轨迹。这次测试主要用N=1，M=3的基于AR模 型的自适应卡尔曼滤波器(AKF-AR)和基于勾速(Constant Velocity，CV)模型的自适应卡 尔曼滤波器(AKF-CV)进行比较，二者其他参数设置相同。
[0078] 2.实验结果分析：
[0079]图3和图4分别给出了在状态噪声强度qr = 0和qr=1.0情况下AR模型和CV模型的均 方根误差。M= 2的AR模型（8卩"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模型。
[0080] 图6和图7给出了当滑窗长度W = 20时，AKF-AR算法和AKF-CV算法分别在X和Y两个 方向的位置误差。可以看出，AKF-AR(N=1，M = 3)的位置误差比AKF-CV要小得多。这是因为 AKF-AR-方面可以实时地在线计算状态噪声协方差;另一方面它可以根据状态噪声协方差 的波动调整状态模型。该算法在处理机动目标运动的动态性能方面比传统的算法更有优 势。另外，在不同滑窗长度的情况下的算法的估计精度是不同的。因此需要根据实际情况来 选择合适的滑窗长度。
【主权项】
1. 一种应用于GI^导航的自适应卡尔曼滤波算法，其特征在于，包括如下步骤： (1) 设置模型参数:状态变量的维数M，多项式的阶数N，且满足M>N+2,状态噪声方差初 始矩阵Qo,并对目标状态的估计均值xk-i|k-i和协方差Pk-i|k-i进行初始化，k表示离散时间的 义样点；初始状态估计x〇|〇=[zo Z-1…ζ-Μ+ι]τ，其中zi(i=-M+l,-M+2,…，-1,0)为滤波器 起始之前各个时刻的目标位置观测量； (2) 建立A財莫型，在卡尔曼滤波框架下计算出A財莫型系数，具体实施过程如下： (2a)根据A財莫型的定义，建立A財莫型系数约束的表达式； (2b)计算A財莫型系数； (2c)计算状态转移矩阵Fk|k-i; (3) 由步骤(2)得到Fk|k-i的值，利用基于AR模型的卡尔曼滤波来预测目标状态，目标状 态包括均值和误差协方差； (4) 利用实时测量的数据更新目标状态，并且计算增益矩阵、后验估计均值W及后验估 计误差协方差； (5) 在线自适应地计算状态噪声协方差，输出目标位置的估计均值和估计误差协方差， 并且令k增加1，返回进行步骤2。2. 根据权利要求1所述应用于GPS导航的自适应卡尔曼滤波算法，其特征在于，所述步 骤(2)中： AR模型系数约束的表达式为Auk = b 其中uk为AR模型的系数，uk=[hi h2…hM]T，hi为滤波器系数，i = l，2,…，M，b=[l 0 …〇]τ，Α为(N+1) XM维Vandermonde矩阵，求解得AR模型系数向量Uk的最优解％，.计算式为苗=坂(乂町*_1乂'）V，间接计算 Fk|k-i; 其中3. 根据权利要求1所述应用于GPS导航的自适应卡尔曼滤波算法，其特征在于，所述步 骤(3)中预测目标状态的过程如下： 3.1建立滤波器预测模型 目标在k+1时刻的状态向量为xk = Fk I k-ixk-i+Wc; 其中xk=[xkxk-i ... xk-M+i]T为目标k时刻之前Μ个采样点的位置值，wk是状态噪声，wk = [WkWk-l ... Wk-M+1]T，代表各时刻目标位置的随机波动，它是一个独立同分布的零均值 高斯序列，即Wk~N(0，Qk)，假定不同时刻目标的位置波动相互独立，则AR模型中的Qk表示 为：Qk=E[wk(wk)T] =屯·Δ? · I; 其中，qr为目标位置的状态噪声强度，Δ t为采样间隔，I为Μ维单位矩阵； 3.2预测目标状态包括预测均值和误差协方差： xk|k-i = Fk|k-i · xk-i|k-i；其中xkM为k-1时刻目标状态的预测均值，PkM为k-1时刻预测误差。4. 根据权利要求1所述应用于GPS导航的自适应卡尔曼滤波算法，其特征在于，所述步 骤(4)中利用测量数据更新目标状态，过程如下： 4.1建立滤波器观测模型 目标在k时刻的位置观测值由zk =化k+vk得到，其中观测矩阵H=[l 0…0]ixM，vk是观 测噪声，代表各时刻目标位置的观测误差，Vk是一个白的高斯随机过程，与状态噪声Wk相互 独立，即vk~N(0,化），其中Rk为观测噪声的协方差矩阵，由于观测向量zk只有一个元素，因 此观测噪声协方差矩阵化也只有一个元素，表示出来的状态模型和观测模型都是线性的， 因此卡尔曼滤波算法能够提供最小均方误差意义下的最优状态估计； 4.2更新目标状态 利用观测信息对预测值进行更新，得到目标状态的估计均值和估计协方差； Kk = Pk|k-iHT(HPk|k-iHT+Rk)-i， Xk I k = Xk I k-1+Kk (Zk-Hxk I k-i)， Pk|k= (I-KkH)Pk|k-i， 其中，Kk为k时刻的增益矩阵，Xkik为k时刻目标状态估计均值，Pkik为k时刻的目标状态估 计误差协方差。5. 根据权利要求1所述应用于GPS导航的自适应卡尔曼滤波算法，其特征在于，所述步 骤(5)中在线自适应地计算状态噪声协方差，过程如下： 5.1利用新息序列计算实际的新息协方差矩阵其中dk = zk-Hxk I k-i，W为滑窗长度； 5.2将理论的新息协方差Sk用实际的新息协方差衣代替，并用公式a =ii冷在线计 算状态噪声的协方差。6. 根据权利要求5所述应用于GPS导航的自适应卡尔曼滤波算法，其特征在于，实际的 采样时刻k小于滑窗长度W:
【专利摘要】本发明公开了一种应用于GPS导航的自适应卡尔曼滤波算法，主要解决现有的自适应卡尔曼滤波算法无法随着滤波过程自适应调节的问题，其过程是：(1)设置算法参数，并对目标状态进行初始化；(2)建立AR模型，套用卡尔曼滤波框架计算出AR模型的系数；(3)利用基于AR模型的卡尔曼滤波来预测目标状态，包括预测均值和误差协方差；(4)使用测量数据更新目标状态,计算增益矩阵、后验估计均值以及后验估计误差协方差；(5)在线自适应地计算状态噪声协方差，输出目标位置的状态值和协方差，令k增加1,返回步骤2；本发明与现有自适应卡尔曼滤波算法相比，能够实现更加精确的目标状态估计，可用于实际的GPS导航系统。
【IPC分类】G01S19/42
【公开号】CN105549049
【申请号】CN201510883751
【发明人】靳标, 郭交, 王胜兰, 苏宝峰, 朱学卫
【申请人】西北农林科技大学
【公开日】2016年5月4日
【申请日】2015年12月4日