专利名称：一种基于观测噪声方差阵估计的自适应滤波方法
技术领域：
本发明涉及一种适用于组合导航系统信息融合的自适应滤波方法，可实时有效估计测量噪声的动态特性，避免出现滤波发散现象，用于提高组合导航系统定位精度，属于组合导航自适应滤波技术领域。
背景技术：
组合导航信息融合方法通常采用卡尔曼滤波方法，标准的卡尔曼滤波是建立在已知准确模型和噪声特性的最优估计。然而，噪声特性的获取一般都是基于验前统计信息，而任何统计信息都难免会失真，特别是难以精确描述当前观测噪声的动态特性。在观测噪声动态估计研究中，不同的研究者进行了大量研究，其方法主要有自适应更新状态噪声、观测噪声误差统计特性的Sage-husa方法；基于新息序列的自适应开窗逼近法(IAE)，它依据观测信息自适应调整状态协方差阵和观测协方差阵；此外，还有一种动态自适应抗差滤波算法，它能够自适应地分配观测信息和模型信息对状态估计的权重。若线性系统的模型方程为xk+1 = ok+1,kxk+wkZk = HkXk+Vk式中k为组合导航系统滤波解算时刻；Xk为k时刻组合导航系统的状态向量； Ok+1,k为k时刻到k+Ι时刻的状态转移矩阵；Wk为k时刻系统噪声向量；Zk为k时刻观测向量；Hk为k时刻观测矩阵；Vk为k时刻观测噪声向量。对于该线性系统而言，上述观测噪声特性统计方法均依赖于新息量，新息量的计算如下式Zkl^l =Zk-HkXkl^式中-.XklW为k时刻到k+Ι时刻的一步预测状态值；之吣为新息量。然而，如果一步状态预测值估计Λμ不准确，观测噪声特性的估计会出现偏差，从而影响滤波精度。
发明内容
本发明的目的是为了解决上述问题，提出一种基于观测噪声方差阵估计的自适应滤波方法，本发明从组合导航系统中不同测量系统的测量互补特性出发，在一定情况下，通过相对测量关系的构建，实现观测噪声的动态估计，在此基础上，以预设滤波精度为指标， 通过构造自适应因子对估计观测噪声进行适当的调节，最终实现高精度的自适应滤波。本发明的一种基于观测噪声方差阵估计的自适应滤波方法，包括以下几个步骤步骤一分别采集测量系统GPS和测量系统DR的测量数据信号，将其进行时间对准；步骤二 选择状态变量和观测变量，建立组合导航系统离散化卡尔曼滤波方程；步骤三设定滤波周期、初值及预期滤波精度，进行卡尔曼滤波时间更新和量测更新过程；
7
步骤四利用GPS和DR测量系统的测量互补特性，计算双测量系统单历元互差分结果，进行观测噪声的动态估计；步骤五设定观测异常阈值Rt，如果互差分估计的观测噪声Rl大于或者等于观测异常阈值Rt，则利用单历元径向、法向距离差的方法重新估计观测噪声；如果互差分估计的观测噪声Rl大于观测异常阈值Rt，则直接进行步骤六；步骤六根据估计的观测噪声更新观测噪声阵，在此基础上计算自适应因子并更新滤波增益阵，基于序贯处理方法进行量测更新过程；步骤七判断所有的测量数据已经处理完，如果还有待解算的数据，则返回步骤三；如果所有的测量数据已处理完，则结束自适应滤波过程。通过上述方法，利用双测量系统单历元互差分方法和单历元径向、法向距离差的方法进行噪声特性的估计，在此基础上从滤波精度要求出发，构造自适应因子调节滤波增益阵，避免某些特殊情况下单次噪声特性估计精度不高时造成的不良影响，提高组合系统输出精度和滤波稳定性。本发明的优点在于(1)本发明通过构造单历元双系统互差分序列和径向、法向距离差方法，避免了现有噪声估计方法中状态误差和历史数据的影响，实时有效地跟踪了测量系统的噪声变化特性；(2)本发明在满足预设精度的前提下，通过构造自适应因子对估计噪声方差阵进行适当调节，有效地提高了滤波精度，提高了滤波系统的精度和抗干扰性能；(3)本发明简单易行，计算量和存储量较小，具有广泛的工程实用价值。
图1是本发明的方法流程图；图2是本发明的径向、法向距离投影关系图(以二维运动为例)；图3为本发明实施例中机车定位系统运行的实际环境；图4为本发明实施例中机车由开阔地段至高楼附近再至开阔地段运行过程中GPS 的测量结果；图5a为本发明实施例中机车上GPS由正常定位至受到建筑物干扰再至恢复正常的过程中，本发明方法、标准卡尔曼滤波算法、Sage-husa自适应滤波算法及基于测量统计特性的MAKF自适应滤波算法间东向位置测量噪声R值的实现效果对比图；图5b为本发明实施例中机车上GPS由正常定位至受到建筑物干扰再至恢复正常的过程中，本发明方法、标准卡尔曼滤波算法、Sage-husa自适应滤波算法及基于测量统计特性的MAKF自适应滤波算法间北向位置测量噪声R值的实现效果对比图；图5c为本发明实施例中机车上GPS由正常定位至受到建筑物干扰再至恢复正常的过程中，本发明方法基于估计的东向位置测量噪声构造的东向位置自适应因子的实验结果；图5d为机车上GPS由正常定位至受到建筑物干扰再至恢复正常的过程中，本发明方法基于估计的北向位置测量噪声构造的北向位置自适应因子的实验结果；图6为机车上GPS由正常定位至受到建筑物干扰再至恢复正常的过程中，本发明
8方法、标准卡尔曼滤波算法、Sage-husa自适应滤波算法及基于测量统计特性的嫩に？自适 应滤波算法输出值与參考轨迹真值输出值间的实现效果对比图。图中
具体实施例方式下面将结合附图和实施例对本发明作进ー步的详细说明。本发明是ー种基于观测噪声方差阵估计的自适应滤波方法，流程如图1所示，包 括以下几个步骤步骤ー分别采集测量系统GPS和测量系统DR的测量数据信号，将其进行时间对 准；步骤ニ 选择状态变量和观测变量，建立组合导航系统离散化卡尔曼滤波方程；组合导航系统离散化卡尔曼滤波方程具体包括^离散化状态方程为X (k+l) = O (k+l，k) X (k) + r (k+l，k) W (k)其中，状态变量选取为ス=[へEx W V ^ B 5了冰为组合导航系统滤波解算
时刻外、艮分别为车辆的北向位置和东向位置，V为车辆航向，？为车辆速度，ゾ为航向角 速度，B为陀螺仪的零偏，S为里程仪比例因子；系统噪声阵为W = [W1 W2 W3 W4 W5 W6 W7] T，Wl、W2、W3、W4, W5, W6, W7分别为状态变量中各状态量的系统噪声分量，均为零均值白噪声。GPS/DR组合导航离散卡尔曼滤波状态方程为Ny (k+l) = Ny (k) +v (k) Tcos ( ￥ (k)) +W1Ex (k+l) = Ex (k) +v (k) Tsin ( ￥ (k)) +W2^(k + l) = ^/(k) + T^(k) + M'3v(k+l) = v(k)+w4^(k + l) = y/(k) + w5B(k+1) = B(k)+w6S(k+1) = S(k)+w7其中，了为滤波周期；k为组合导航系统滤波解算时刻；Ny(k)为k时刻状态向量的 北向位置分量。状态转移矩阵①(k+l, k)为
权利要求
1. 一种基于观测噪声方差阵估计的自适应滤波方法，其特征在于，包括以下几个步骤步骤一分别采集测量系统GPS和测量系统DR的测量数据信号，将其进行时间对准； 步骤二 选择状态变量和观测变量，建立组合导航系统离散化卡尔曼滤波方程； 步骤三设定滤波周期、初值及预期滤波精度，进行卡尔曼滤波时间更新和量测更新过程步骤四利用GPS和DR测量系统的测量互补特性，计算双测量系统单历元互差分结果， 进行观测噪声的动态估计；步骤五设定观测异常阈值Rt，如果互差分估计的观测噪声Rl大于或者等于观测异常阈值Rt，则利用单历元径向、法向距离差的方法重新估计观测噪声；如果互差分估计的观测噪声Rl大于观测异常阈值Rt，则直接进行步骤六；步骤六根据估计的观测噪声更新观测噪声阵，在此基础上计算自适应因子并更新滤波增益阵，基于序贯处理方法进行量测更新过程；步骤七判断所有的测量数据已经处理完，如果还有待解算的数据，则返回步骤三；如果所有的测量数据已处理完，则结束自适应滤波过程。
2.根据权利要求1所述的一种基于观测噪声方差阵估计的自适应滤波方法，其特征在于，所述的步骤二具体为 a、离散化状态方程为 X(k+1) = Φ (k+1, k) X (k) + Γ (k+1, k)ff(k)其中，状态变量选取为4 = [乂 Ex ψ ν ψ B sf，k为组合导航系统滤波解算时刻；&、艮分别为车辆的北向位置和东向位置，Ψ为车辆航向，V为车辆速度，为航向角速度，B为陀螺仪的零偏，S为里程仪比例因子；系统噪声阵为W = [W1 w2 W3 w4 w5 w6 w7]τ, W1, w2, w3> W4, W5, W6, W7分别为状态变量中各状态量的系统噪声分量，均为零均值白噪声； GPS/DR组合导航离散卡尔曼滤波状态方程为 Ny (k+1) = Ny (k) +ν (k) Tcos (ψ (k)) +W1 Ex (k+1) = Ex (k) +ν (k) Tsin ( Ψ (k)) +w2ψ ^ + ) = ψ(] ) + Ti//(k) + μ·3ν (k+1) = ν (k) +w4ψ(] +1) = Ψ(1 ) + W5B (k+1) = B (k) +W6S (k+1) = S (k) +w7其中，T为滤波周期；k为组合导航系统滤波解算时刻；Ny(k)为k时刻状态向量的北向位置分量；状态转移矩阵Φ (k+1, k)为"1 O -v{k)Tsmi}jy{k))O θ"01 vQc)T cosWky) Tsm^{k)) O O001OOT<l>{k + \k) =OOO1OOOOOO10OOOOOl系统噪声驱动阵Γ (k+1，k)为
3.根据权利要求1所述的一种基于观测噪声方差阵估计的自适应滤波方法，其特征在于，所述的步骤三具体为A、卡尔曼滤波的时间更新过程
4.根据权利要求1所述的一种基于观测噪声方差阵估计的自适应滤波方法，其特征在于，所述的步骤四具体为具体包括以下步骤(1)DR测量差分序列为ADRn(k) = Pod(k)*S*T*cos(w(k)) k = m.. ADRe(k) =Pod(k)*S*T*sin(i//(A:)) _ ''其中，k为当前滤波时刻；ADRN(k)为k时刻DR测量系统北向位置差分结果；ADRE(k) 为k时刻DR测量系统东向位置差分结果；P。d(k)为k-1 k时间段内里程仪测量的脉冲数； S为里程仪刻度因子；T为卡尔曼滤波周期；Ψ (k)为k时刻航向值，由上一时刻的卡尔曼滤波航向值加上k-Ι k时间段陀螺仪测量的航向角变化量得到；(2)GPS测量差分序列为AGPSn (k) = GPSn (k) - GPSn (k-l),,…Jc = \ 2 3...AGPSe(k) = GPSe{k)-GPSe{k-l) ‘‘其中，k为组合导航系统滤波解算时刻；GPSN(k)、GPSE(k)分别为k时刻GPS输出的北向位置、东向位置；AGPSnQO和Δ GPSe(k)分别为k时刻GPS输出的北向位置的差分结果及东向位置的差分结果；(3)GPS/DR双系统测量互差分序列为Cn (k) = AGPSn (k)-ADRn (k)众=123 … Ce (k) = AGPSe (k) - ADRe (k) _ ,,其中，k为组合导航系统滤波解算时刻；CN(k)、CE(k)分别为k时刻北向位置、东向位置的互差分结果；(4)双测量系统单历元互差分结果估计的测量系统GPS的观测噪声为C(k) = [CN(k)，CE(k)]R1(k) = {C(k)C(kf)/2其中，k为组合导航系统滤波解算时刻；C(k)为位置互差分向量；AW为k时刻基于双测量系统单历元互差分的估计结果。
5.根据权利要求1所述的一种基于观测噪声方差阵估计的自适应滤波方法，其特征在于，所述的步骤五具体为(1)如果互差分估计的观测噪声Rl不大于的设定的观测异常阈值Rt ；具体包括以下几个步骤1)径向距离、法向距离的投影的计算方法为dis = ^(GPSt-Xll)2 +(GPS^-Xll)2normal(k) = dis * sm(angle(k) -) radial {k) = dis * cos{angle (U)-X^1)其中，仏、Xll,之、分别为k-i时刻的自适应滤波东向位置值、北向位置值和航向值；dis为k时刻GPS测量值与k-Ι时刻卡尔曼滤波值的距离；啤、GP《分别为k 时刻GPS测量的东向位置和北向位置；angleGO为依据GPS测量位置计算所得的航向； normal (k)为k时刻法向距离的投影；radial (k)为k时刻径向距离的投影；2)依据法向、径向距离进行观测噪声估计的方法为
6.根据权利要求1所述的一种基于观测噪声方差阵估计的自适应滤波方法，其特征在于，所述的步骤六具体为具体为①基于观测噪声估计方差阵映射的观测误差水平，自适应因子αi构造方法如下‘w 具 ？其中，k为组合导航系统滤波解算时刻；i为估计观测向量的第i个分量；总为第i个观测分量的噪声估计值；ε为组合导航系统期望达到的滤波精度；②依据自适应因子调节滤波增益阵，采用序贯处理算法进行自适应卡尔曼滤波解算的方法如下
全文摘要
本发明公开了一种基于观测噪声方差阵估计的自适应滤波方法，该方法利用组合导航系统中不同测量系统的测量互补特性，构造互差分序列及径向、法向距离差序列对单次历元的测量噪声特性进行动态估计，并根据估计测量噪声映射的误差水平，以预设滤波精度为指标，构造自适应因子有效调节滤波增益阵，进行自适应卡尔曼滤波解算。本发明不仅实现了测量噪声特性的高动态估计，而且对滤波增益阵进行了有效更新，最终提高了组合导航系统的定位精度。
文档编号G01S19/49GK102508278SQ20111038519
公开日2012年6月20日 申请日期2011年11月28日 优先权日2011年11月28日
发明者周启帆, 张海, 李玉洁, 王嫣然, 苌永娜 申请人:北京航空航天大学