CN102508278A - 一种基于观测噪声方差阵估计的自适应滤波方法 - Google Patents

Abstract

本发明公开了一种基于观测噪声方差阵估计的自适应滤波方法，该方法利用组合导航系统中不同测量系统的测量互补特性，构造互差分序列及径向、法向距离差序列对单次历元的测量噪声特性进行动态估计，并根据估计测量噪声映射的误差水平，以预设滤波精度为指标，构造自适应因子有效调节滤波增益阵，进行自适应卡尔曼滤波解算。本发明不仅实现了测量噪声特性的高动态估计，而且对滤波增益阵进行了有效更新，最终提高了组合导航系统的定位精度。

Description

一种基于观测噪声方差阵估计的自适应滤波方法

技术领域

本发明涉及一种适用于组合导航系统信息融合的自适应滤波方法，可实时有效估计测量噪声的动态特性，避免出现滤波发散现象，用于提高组合导航系统定位精度，属于组合导航自适应滤波技术领域。

背景技术

组合导航信息融合方法通常采用卡尔曼滤波方法，标准的卡尔曼滤波是建立在已知准确模型和噪声特性的最优估计。然而，噪声特性的获取一般都是基于验前统计信息，而任何统计信息都难免会失真，特别是难以精确描述当前观测噪声的动态特性。在观测噪声动态估计研究中，不同的研究者进行了大量研究，其方法主要有：自适应更新状态噪声、观测噪声误差统计特性的Sage-husa方法；基于新息序列的自适应开窗逼近法(IAE)，它依据观测信息自适应调整状态协方差阵和观测协方差阵；此外，还有一种动态自适应抗差滤波算法，它能够自适应地分配观测信息和模型信息对状态估计的权重。

若线性系统的模型方程为：

X_k+1＝Φ_k+1，kX_k+W_k

Z_k＝H_kX_k+V_k

式中：k为组合导航系统滤波解算时刻；X_k为k时刻组合导航系统的状态向量；Φ_k+1，k为k时刻到k+1时刻的状态转移矩阵；W_k为k时刻系统噪声向量；Z_k为k时刻观测向量；H_k为k时刻观测矩阵；V_k为k时刻观测噪声向量。

对于该线性系统而言，上述观测噪声特性统计方法均依赖于新息量，新息量的计算如下式：

${\tilde{Z}}_{k/k-1}=Z_k-H_k{\hat{H}}_{k/k-1}$

式中：

为k时刻到k+1时刻的一步预测状态值；

为新息量。然而，如果一步状态预测值估计

不准确，观测噪声特性的估计会出现偏差，从而影响滤波精度。

发明内容

本发明的目的是为了解决上述问题，提出一种基于观测噪声方差阵估计的自适应滤波方法，本发明从组合导航系统中不同测量系统的测量互补特性出发，在一定情况下，通过相对测量关系的构建，实现观测噪声的动态估计，在此基础上，以预设滤波精度为指标，通过构造自适应因子对估计观测噪声进行适当的调节，最终实现高精度的自适应滤波。

本发明的一种基于观测噪声方差阵估计的自适应滤波方法，包括以下几个步骤：

步骤一：分别采集测量系统GPS和测量系统DR的测量数据信号，将其进行时间对准；

步骤二：选择状态变量和观测变量，建立组合导航系统离散化卡尔曼滤波方程；

步骤三：设定滤波周期、初值及预期滤波精度，进行卡尔曼滤波时间更新和量测更新过程；

步骤四：利用GPS和DR测量系统的测量互补特性，计算双测量系统单历元互差分结果，进行观测噪声的动态估计；

步骤五：设定观测异常阈值Rt，如果互差分估计的观测噪声R1大于或者等于观测异常阈值Rt，则利用单历元径向、法向距离差的方法重新估计观测噪声；如果互差分估计的观测噪声R1大于观测异常阈值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由正常定位至受到建筑物干扰再至恢复正常的过程中，本发明方法、标准卡尔曼滤波算法、Sage-husa自适应滤波算法及基于测量统计特性的MAKF自适应滤波算法输出值与参考轨迹真值输出值间的实现效果对比图。

图中：

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明是一种基于观测噪声方差阵估计的自适应滤波方法，流程如图1所示，包括以下几个步骤：

步骤一：分别采集测量系统GPS和测量系统DR的测量数据信号，将其进行时间对准；

步骤二：选择状态变量和观测变量，建立组合导航系统离散化卡尔曼滤波方程；

组合导航系统离散化卡尔曼滤波方程具体包括：

a、离散化状态方程为：

X(k+1)＝Φ(k+1，k)X(k)+Γ(k+1，k)W(k)

其中，状态变量选取为： $X={\left[\begin{array}{ccccccc}{N}_y& E_x& \mathrm{\ψ}& v& \stackrel{\·}{\mathrm{\ψ}}& B& S\end{array}\right]}^T,$ k为组合导航系统滤波解算时刻；N_y、E_x分别为车辆的北向位置和东向位置，ψ为车辆航向，v为车辆速度，

为航向角速度，B为陀螺仪的零偏，S为里程仪比例因子；系统噪声阵为：W＝[w₁ w₂ w₃ w₄ w₅ w₆ w₇]^T，w₁、w₂、w₃、w₄、w₅、w₆、w₇分别为状态变量中各状态量的系统噪声分量，均为零均值白噪声。

GPS/DR组合导航离散卡尔曼滤波状态方程为：

N_y(k+1)＝N_y(k)+v(k)Tcos(ψ(k))+w₁

E_x(k+1)＝E_x(k)+v(k)Tsin(ψ(k))+w₂

$\mathrm{\ψ}(k+1)=\mathrm{\ψ}\left(k\right)+T\stackrel{\·}{\mathrm{\ψ}}\left(k\right)+w_3$

v(k+1)＝v(k)+w₄

$\stackrel{\·}{\mathrm{\ψ}}(k+1)=\stackrel{\·}{\mathrm{\ψ}}\left(k\right)+w_5$

B(k+1)＝B(k)+w₆

S(k+1)＝S(k)+w₇

其中，T为滤波周期；k为组合导航系统滤波解算时刻；N_y(k)为k时刻状态向量的北向位置分量。

状态转移矩阵Φ(k+1，k)为：

$\mathrm{\Φ}(k+1,k)=\left[\begin{array}{cccccc}1& 0& -v\left(k\right)T\sin \left(\mathrm{\ψ}\left(k\right)\right)& T\cos \left(\mathrm{\ψ}\left(k\right)\right)& 0& 0\\ 0& 1& v\left(k\right)T\cos \left(\mathrm{\ψ}\left(k\right)\right)& T\sin \left(\mathrm{\ψ}\left(k\right)\right)& 0& 0\\ 0& 0& 1& 0& 0& T\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

系统噪声驱动阵Γ(k+1，k)为：

$\mathrm{\Γ}(k+1,k)=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

b、离散化观测方程为：

Z(k)＝H(k)X(k)+V(k)

其中，观测向量选取为： $Z={\left[\begin{array}{ccccc}{N}_{\mathrm{gps}}& E_{\mathrm{gps}}& {\mathrm{\ψ}}_{\mathrm{gps}}& v_{\mathrm{od}}& {\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gyro}}\end{array}\right]}^T,$ N_gps、E_gps分别为GPS测量的北向位置和东向位置，ψ_gps为GPS测量的航向角，v_od为里程仪测量的速度，为陀螺仪测量的角速度；观测噪声阵为：V＝[v₁ v₂ v₃ v₄ v₅]^T，v₁、v₂、v₃、v₄、v₅分别为观测向量中各观测量的观测噪声分量，均为零均值白噪声。

GPS/DR组合导航离散卡尔曼滤波观测方程为：

N_gps＝N_y(k)+v₁

E_gps＝E_x(k)+v₂

ψ_gps＝ψ(k)+v₃

v_od(k)＝P_od(k)S(k)+v₄

${\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gyro}}\left(k\right)=\stackrel{\·}{\mathrm{\ψ}}\left(k\right)+B+v_5$

其中，P_od(k)为里程仪测量的脉冲数；k为组合导航系统滤波解算时刻；N_y(k)为k时刻状态向量的北向位置分量。

观测矩阵H(k)为：

$H\left(k\right)=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& P_{\mathrm{od}}\left(k\right)\\ 0& 0& 0& 0& 1& 0& 1\end{array}\right]$

步骤三：设定滤波周期、初值及预期滤波精度，进行卡尔曼滤波时间更新和量测更新过程；

卡尔曼滤波时间更新和量测更新过程如下：

A、卡尔曼滤波的时间更新过程：

$\left\{\begin{array}{c}{\hat{X}}_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ P_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{{\mathrm{\Φ}}^T}_{k,k-1}+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T\end{array}\right.$

其中，k为组合导航系统滤波解算时刻；为状态X_k-1的卡尔曼滤波估计值；Φ_k，k-1为k-1到k时刻的状态转移矩阵；

为依据k-1时刻滤波值

对X_k的一步预测；Γ_k，k-1为系统噪声矩阵；P_k|k-1为一步预测均方误差矩阵；P_k-1为k-1时刻的估计均方误差阵；Q_k-1为k-1时刻系统噪声协方差矩阵。

B、卡尔曼滤波的量测更新过程：

$\left\{\begin{array}{c}{\hat{X}}_k={\hat{X}}_{k|k-1}+K_k(Z_k-H_k{\hat{X}}_{k|k-1})\\ K_k=P_{k|k-1}{H^T}_k{(H_k{P}_{k|k-1}{H^T}_k+R_k)}^{-1}\\ P_k=(I-K_k{H}_k)P_{k|k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K^T}_k\end{array}\right.$

其中，k为组合导航系统滤波解算时刻；

为k时刻的卡尔曼滤波估计值；K_k为k时刻的滤波增益矩阵；Z_k为k时刻的观测向量；

为依据k-1时刻滤波值

对X_k的一步预测；P_k|k-1为一步预测均方误差矩阵；H_k为k时刻的量测矩阵；R_k为k时刻的观测噪声协方差矩阵；P_k为k时刻的估计均方误差矩阵；I为单位矩阵。

步骤四：利用GPS和DR测量系统的测量互补特性，计算双测量系统单历元互差分结果，进行观测噪声的动态估计；

具体包括以下步骤：

(1)DR测量差分序列为：

$\left\{\begin{array}{c}{\mathrm{\ΔDR}}^N\left(k\right)=P_{\mathrm{od}}\left(k\right)*S*T*\cos \left(\mathrm{\ψ}\left(k\right)\right)\\ {\mathrm{\ΔDR}}^E\left(k\right)=P_{\mathrm{od}}\left(k\right)*S*T*\sin \left(\mathrm{\ψ}\left(k\right)\right)\end{array}\right.k=\mathrm{1,2,3}\·\·\·$

其中，k为当前滤波时刻；ΔDR^N(k)为k时刻DR测量系统北向位置差分结果；ΔDR^E(k)为k时刻DR测量系统东向位置差分结果；P_od(k)为k-1～k时间段内里程仪测量的脉冲数；S为里程仪刻度因子；T为卡尔曼滤波周期；ψ(k)为k时刻航向值，由上一时刻的卡尔曼滤波航向值加上k-1～k时间段陀螺仪测量的航向角变化量得到。

(2)GPS测量差分序列为：

$\left\{\begin{array}{c}{\mathrm{\ΔGPS}}^N\left(k\right)={\mathrm{GPS}}^N\left(k\right)-{\mathrm{GPS}}^N(k-1)\\ {\mathrm{\ΔGPS}}^E\left(k\right)={\mathrm{GPS}}^E\left(k\right)-{\mathrm{GPS}}^E(k-1)\end{array}\right.k=\mathrm{1,2,3}\·\·\·$

其中，k为组合导航系统滤波解算时刻；GPS^N(k)、GPS^E(k)分别为k时刻GPS输出的北向位置、东向位置；ΔGPS^N(k)和ΔGPS^E(k)分别为k时刻GPS输出的北向位置的差分结果及东向位置的差分结果。

(3)GPS/DR双系统测量互差分序列为：

$\left\{\begin{array}{c}{C}^N\left(k\right)={\mathrm{\ΔGPS}}^N\left(k\right)-{\mathrm{\ΔDR}}^N\left(k\right)\\ C^E\left(k\right)={\mathrm{\ΔGPS}}^E\left(k\right)-{\mathrm{\ΔDR}}^E\left(k\right)\end{array}\right.k=\mathrm{1,2,3}\·\·\·$

其中，k为组合导航系统滤波解算时刻；C^N(k)、C^E(k)分别为k时刻北向位置、东向位置的互差分结果。

(4)双测量系统单历元互差分结果估计的测量系统GPS的观测噪声为：

C(k)＝[C^N(k)，C^E(k)]

${\hat{R}}_1\left(k\right)=\left(C\left(k\right)C{\left(k\right)}^T\right)/2$

其中，k为组合导航系统滤波解算时刻；C(k)为位置互差分向量；为k时刻基于双测量系统单历元互差分的估计结果；

步骤五：设定观测异常阈值Rt，如果互差分估计的观测噪声R1大于或者等于观测异常阈值Rt，则利用单历元径向、法向距离差的方法重新估计观测噪声；如果互差分估计的观测噪声R1大于观测异常阈值Rt，则直接进行步骤六；

(1)如果互差分估计的观测噪声R1不大于的设定的观测异常阈值Rt；

由于互差分方法基于惯性系统的短期高精度，针对单次历元GPS的观测噪声进行估计，在某些特定情况下，例如GPS定位误差较大但相邻时刻测量位置距离较近的情况下，此方法估计的GPS观测噪声会小于实际的GPS观测噪声，为实现GPS观测噪声任意时刻的准确估计，如果互差分估计的观测噪声R1不大于的设定的观测异常阈值Rt，本发明提供了利用单次历元径向法向距离差的方法重新估计观测噪声，具体包括以下几个步骤：

1)径向距离、法向距离的投影的计算方法为：

$\mathrm{dis}=\sqrt{{({\mathrm{GPS}}_k^E-{\hat{X}}_{k-1}^E)}^2+{({\mathrm{GPS}}_k^N-{\hat{X}}_{k-1}^N)}^2}$

$\mathrm{normal}\left(k\right)=\mathrm{dis}*\sin (\mathrm{angle}\left(k\right)-{\hat{X}}_{k-1}^{\mathrm{\ψ}})$

$\mathrm{radial}\left(k\right)=\mathrm{dis}*\cos (\mathrm{angle}\left(k\right)-{\hat{X}}_{k-1}^{\mathrm{\ψ}})$

其中，分别为k-1时刻的自适应滤波东向位置值、北向位置值和航向值；dis为k时刻GPS测量值与k-1时刻卡尔曼滤波值的距离；

分别为k时刻GPS测量的东向位置和北向位置；angle(k)为依据GPS测量位置计算所得的航向；normal(k)为k时刻法向距离的投影；radial(k)为k时刻径向距离的投影；

2)依据法向、径向距离进行观测噪声估计的方法为：

${\hat{R}}_2=\max (\left(\mathrm{normal}\left(k\right)\mathrm{normal}{\left(k\right)}^T\right)/2,\left((\mathrm{riadal}\left(k\right)-\mathrm{\ΔDR}\left(k\right)){(\mathrm{riadal}\left(k\right)-\mathrm{\ΔDR}\left(k\right))}^T\right)/2)$

其中，ΔDR(k)＝P_od(k)*S*T，P_od(k)为k-1～k时间段内里程仪测量的脉冲数；S为里程仪刻度因子；T为卡尔曼滤波周期；max为求最大值的函数；normal(k)为k时刻法向距离的投影；radial(k)为k时刻径向距离的投影；

3)转入执行步骤六，且进入步骤六时的GPS测量噪声协方差估计值为：

${\hat{R}}_k={\hat{R}}_2$ ${\hat{R}}_1\≤\mathrm{Rt}$

(2)如果互差分估计的观测噪声R1大于观测异常阈值Rt，则直接执行步骤六，且进入步骤六时的GPS测量噪声协方差估计值为：

${\hat{R}}_k={\hat{R}}_1$ ${\hat{R}}_1>\mathrm{Rt}$

步骤六：根据估计的观测噪声更新观测噪声阵，在此基础上计算自适应因子并更新滤波增益阵，基于序贯处理方法进行量测更新过程。

具体为：

①基于观测噪声估计方差阵映射的观测误差水平，自适应因子α_i构造方法如下：

${\mathrm{\α}}_i=\left\{\begin{array}{cc}1,& \sqrt{{\hat{R}}_k^i}\≤\mathrm{c\ϵ}\\ \mathrm{c\ϵ}/\sqrt{{\hat{R}}_k^i},& \sqrt{{\hat{R}}_k^i}>\mathrm{c\ϵ}\end{array}\right.$

其中，k为组合导航系统滤波解算时刻；i为估计观测向量的第i个分量；

为第i个观测分量的噪声估计值；ε为组合导航系统期望达到的滤波精度。

②依据自适应因子调节滤波增益阵，采用序贯处理算法进行自适应卡尔曼滤波解算的方法如下：

$\left\{\begin{array}{c}{\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T\\ {\hat{X}}_k^0={\hat{X}}_{k/k-1},P_k^0=P_{k/k-1}\\ \left\{\begin{array}{c}\stackrel{\‾}{K_k^i}={\mathrm{\α}}_i{P}_k^{i-1}{H}_k^{\mathrm{iT}}{(H_k^i{P}_k^{i-1}{H}_k^{\mathrm{iT}}+{\hat{R}}_k^i)}^{-1}\\ {\hat{X}}_k^i={\hat{X}}_k^{i-1}+\stackrel{\‾}{K_k^i}(Z_k^i-H_k^i{\hat{X}}_k^{i-1})\\ P_k^i=(I-\stackrel{\‾}{K_k^i}{H}_k^i)P_k^{i-1},i=\mathrm{1,2},\·\·\·,m\end{array}\right.\\ {\hat{X}}_k={\hat{X}}_k^m,P_k=P_k^m\end{array}\right.$

其中，

为k-1时刻的卡尔曼滤波估计值；

为依据k-1时刻滤波值对X_k的一步预测；Φ_k，k-1为k-1时刻到k时刻的状态转移矩阵；Γ_k-1为系统噪声矩阵；P_k，k-1为一步预测均方误差矩阵；Q_k-1为k-1时刻系统噪声协方差矩阵；m为观测向量的维数；i为序贯处理算法的迭代次数；

为k时刻经自适应因子调节后的滤波增益矩阵的第i列分量；为k时刻第i次迭代中估计的状态向量；

为k时刻第i次迭代中估计的状态误差方差阵；

为观测噪声协方差矩阵第i个分量；

为k时刻的量测矩阵的第i列分量；

为k时刻的观测向量的第i个分量；I为单位矩阵；P_k为估计均方误差矩阵。

卡尔曼滤波解算的结果

中的各个分量即为运载体的北向位置、东向位置、航向、速度、航向角速率、陀螺仪零偏及里程仪刻度因子的估计值，由于组合导航系统的自适应卡尔曼滤波的定位精度通常高于单测量系统的测量精度，某些情况下可作为判断GPS是否异常的基准信息。

步骤七：判断所有的测量数据已经处理完，如果还有待解算的数据，则返回步骤三；如果所有的测量数据已处理完，则结束自适应滤波过程。

通过上述方法，基于GPS/DR两测量系统的测量互补特性，构造互差分序列和径向法向距离差序列，可以消去真实值的相对变化量，获得含GPS测量噪声信息的有效信息，实现了GPS测量噪声协方差的实时测量统计，在此基础上构造自适应因子调整滤波增益阵，最终提高了组合导航系统的输出精度和滤波稳定性。

在动态系统中真实值难以准确获取的情况下，不能依据测量值、估计值获得噪声信息。在组合导航系统中，对同一物理量通常存在两种不同特性的测量结果，本发明通过构造互差分序列对单次历元的测量噪声特性进行动态估计，并根据估计测量噪声映射的误差水平，以预设滤波精度为指标，构造自适应因子有效调节滤波增益阵，有效提高了组合导航系统的定位精度，特别适用于定位环境复杂、测量系统易受干扰情况下运载体的高精度定位。

实施例：

本发明应用于邯郸钢厂的机车GPS/DR定位系统中，如图3所示，图3a中机车运行在较为开阔的地段，图3b、图3c、图3d中机车上的GPS分别受到周围建筑物、皮带机和管道遮挡的干扰，在机车行驶过程中，GPS测量系统因高楼、皮带机、管道的遮挡出现很大的定位误差，若车机车通过厂房下的铁轨，GPS信号会完全中断。机车工作现场中，路况比较复杂，相邻铁轨之间距离在5m左右，各种障碍物较多，由于作业要求，机车运行速度较低，最终导致机车定位过程中GPS的测量噪声一直处于高动态的变化之中。

本实施例中机车的初始位置为东经114.451120°，北纬36.598090°(为实现高精度定位，将经纬度坐标转换为WGS-84直角坐标系)，初始速度为2.7344m/s，初始航向角为322.0755°，陀螺仪零偏为2.635199203187°，里程仪刻度因子为0.0163725648970。GPS采用差分定位，在无外界环境干扰时，定位精度在2m以内；当受到干扰的情况下，视干扰程度的不同其误差在几米至几十米范围内。为了考察GPS/DR组合导航系统在GPS测量环境改变时抗干扰能力的大小，选取的测试环境为：机车由开阔地段到高楼附近再至开阔地段运行(即GPS由正常定位至受到高楼干扰再恢复正常定位)，如图4a、4b所示，图4a为GPS测量的东向位置、北向位置与参考轨迹真值的效果对比图；图4b为GPS输出的航向和速度的测量结果。

本实施例中设定滤波周期为1s，设定预期滤波精度为5m，本实施例中ε为组合导航系统期望达到的滤波精度，取为5m；c为常数，通常取为0.2～0.8，取为0.2；应用本发明的方法，依据上述步骤，本实施例实现结果如图5a～5d中实线、图6中短虚线线所示。图5a～5b中：短虚线为标准卡尔曼滤波算法仿真结果，点划线为Sage-husa自适应滤波算法仿真结果，点虚线为基于测量统计特性的MAKF自适应滤波算法仿真结果。图6中，实线为参考轨迹真值，带加号点虚线为标准卡尔曼算法仿真结果，短虚线为Sage-husa自适应滤波算法仿真结果，点划线为基于测量统计特性的MAKF自适应滤波算法仿真结果。

i、由图5a、图5b可看出：标准卡尔曼滤波算法的东、北向位置R始终为预设经验值(10m2)，不能进行自适应调节；其它自适应算法可对R进行自适应调节：175～198s时间段内，GPS的噪声特性实际上已经发生了较大变化，但是Sage-husa算法、MAKF算法均未做出响应；200～230s时间段内，在GPS的定位误差较大的情况下，虽然Sage-husa算法、MAKF算法对GPS的噪声特性进行了自适应估计，但估计动态性能较差；231～245s时间段内，GPS已恢复正常定位，而Sage-husa算法、MAKF算法对GPS观测噪声的估计仍严重偏高；整个时间段内，本发明自适应滤波算法快速、及时地跟踪了GPS噪声特性的变化；

ii、由图5c、图5d可看出：本发明自适应算法中，当GPS东、北向位置的测量噪声较大时，对应的自适应因子亦随之变小，从而减小滤波增益阵中相关项的大小，最终降低当前滤波估计值对观测信息的利用程度；

iii、标准卡尔曼滤波、Sage-husa自适应算法、MAKF自适应算法及本发明自适应算法的定位效果如图6所示。GPS受到较大干扰的情况下：标准卡尔曼滤波采用验前统计值，无法做出调整；改进的sage-husa算法在对R更新时耦合了状态估计误差的影响，因而估计结果不可靠；MAKF算法对R的估计建立在窗口内各历元的统计结果的基础上，因而会受到历史数据平滑作用的影响，削弱了对观测噪声估计的动态性能，最终影响了定位精度；本发明算法能够自适应地跟踪GPS测量噪声的变化，输出结果光滑连续，在滤波精度和抗干扰性能上，具有明显的优越性。

Claims (6)

1.一种基于观测噪声方差阵估计的自适应滤波方法，其特征在于，包括以下几个步骤：

步骤一：分别采集测量系统GPS和测量系统DR的测量数据信号，将其进行时间对准；

步骤二：选择状态变量和观测变量，建立组合导航系统离散化卡尔曼滤波方程；

步骤三：设定滤波周期、初值及预期滤波精度，进行卡尔曼滤波时间更新和量测更新过程；

步骤四：利用GPS和DR测量系统的测量互补特性，计算双测量系统单历元互差分结果，进行观测噪声的动态估计；

步骤五：设定观测异常阈值Rt，如果互差分估计的观测噪声R1大于或者等于观测异常阈值Rt，则利用单历元径向、法向距离差的方法重新估计观测噪声；如果互差分估计的观测噪声R1大于观测异常阈值Rt，则直接进行步骤六；

步骤六：根据估计的观测噪声更新观测噪声阵，在此基础上计算自适应因子并更新滤波增益阵，基于序贯处理方法进行量测更新过程；

步骤七：判断所有的测量数据已经处理完，如果还有待解算的数据，则返回步骤三；如果所有的测量数据已处理完，则结束自适应滤波过程。

2.根据权利要求1所述的一种基于观测噪声方差阵估计的自适应滤波方法，其特征在于，所述的步骤二具体为：

a、离散化状态方程为：

X(k+1)＝Φ(k+1，k)X(k)+Γ(k+1，k)W(k)

其中，状态变量选取为：k为组合导航系统滤波解算时刻；N_y、E_x分别为车辆的北向位置和东向位置，ψ为车辆航向，v为车辆速度， 

为航向角速度，B为陀螺仪的零偏，S为里程仪比例因子；系统噪声阵为：W＝[w₁ w₂ w₃ w₄ w₅ w₆ w₇]^T，w₁、w₂、w₃、w₄、w₅、w₆、w₇分别为状态变量中各状态量的系统噪声分量，均为零均值白噪声；

GPS/DR组合导航离散卡尔曼滤波状态方程为：

N_y(k+1)＝N_y(k)+v(k)Tcos(ψ(k))+w₁

E_x(k+1)＝E_x(k)+v(k)Tsin(ψ(k))+w₂

v(k+1)＝v(k)+w₄

B(k+1)＝B(k)+w₆

S(k+1)＝S(k)+w₇

其中，T为滤波周期；k为组合导航系统滤波解算时刻；N_y(k)为k时刻状态向量的北向位置分量； 状态转移矩阵Φ(k+1，k)为：

系统噪声驱动阵Γ(k+1，k)为：

b、离散化观测方程为：

Z(k)＝H(k)X(k)+V(k)

其中，观测向量选取为：

N_gps、E_gps分别为GPS测量的北向位置和东向位置，ψ_gps为GPS测量的航向角，v_od为里程仪测量的速度， 为陀螺仪测量的角速度；观测噪声阵为：V＝[v₁ v₂ v₃ v₄ v₅]^T，v₁、v₂、v₃、v₄、v₅分别为观测向量中各观测量的观测噪声分量，均为零均值白噪声；

GPS/DR组合导航离散卡尔曼滤波观测方程为：

N_gps＝N_y(k)+v₁

E_gps＝E_x(k)+v₂

ψ_gps＝ψ(k)+v₃

v_od(k)＝P_od(k)S(k)+v₄

其中，p_od(k)为里程仪测量的脉冲数；k为组合导航系统滤波解算时刻；N_y(k)为k时刻状态向量的北向位置分量；

观测矩阵H(k)为：

。

3.根据权利要求1所述的一种基于观测噪声方差阵估计的自适应滤波方法，其特征在于，所述的步骤三具体为：

A、卡尔曼滤波的时间更新过程： 

其中，k为组合导航系统滤波解算时刻； 

为状态X_k-1的卡尔曼滤波估计值；Φ_k，k-1为k-1到k时刻的状态转移矩阵； 

为依据k-1时刻滤波值 

对X_k的一步预测；Γ_k，k-1为系统噪声矩阵；P_k|k-1为一步预测均方误差矩阵；P_k-1为k-1时刻的估计均方误差阵；Q_k-1为k-1时刻系统噪声协方差矩阵；

B、卡尔曼滤波的量测更新过程：

其中，k为组合导航系统滤波解算时刻； 

为k时刻的卡尔曼滤波估计值；K_k为k时刻的滤波增益矩阵；Z_k为k时刻的观测向量； 

为依据k-1时刻滤波值 

对X_k的一步预测；P_k|k-1为一步预测均方误差矩阵；H_k为k时刻的量测矩阵；R_k为k时刻的观测噪声协方差矩阵；P_k为k时刻的估计均方误差矩阵；I为单位矩阵。

4.根据权利要求1所述的一种基于观测噪声方差阵估计的自适应滤波方法，其特征在于，所述的步骤四具体为：

具体包括以下步骤：

(1)DR测量差分序列为：

其中，k为当前滤波时刻；ΔDR^N(k)为k时刻DR测量系统北向位置差分结果；ΔDR^E(k)为k时刻DR测量系统东向位置差分结果；P_od(k)为k-1～k时间段内里程仪测量的脉冲数；S为里程仪刻度因子；T为卡尔曼滤波周期；ψ(k)为k时刻航向值，由上一时刻的卡尔曼滤波航向值加上k-1～k时间段陀螺仪测量的航向角变化量得到；

(2)GPS测量差分序列为：

其中，k为组合导航系统滤波解算时刻；GPS^N(k)、GPS^E(k)分别为k时刻GPS输出的北向位置、东向位置；ΔGPS^N(k)和ΔGPS^E(k)分别为k时刻GPS输出的北向位置的差分结果及东向位置的差分结果；

(3)GPS/DR双系统测量互差分序列为：

其中，k为组合导航系统滤波解算时刻；C^N(k)、C^E(k)分别为k时刻北向位置、东向位置的互差分结果； 

(4)双测量系统单历元互差分结果估计的测量系统GPS的观测噪声为：

C(k)＝[C^N(k)，C^E(k)]

其中，k为组合导航系统滤波解算时刻；C(k)为位置互差分向量； 

为k时刻基于双测量系统单历元互差分的估计结果。

5.根据权利要求1所述的一种基于观测噪声方差阵估计的自适应滤波方法，其特征在于，所述的步骤五具体为：

(1)如果互差分估计的观测噪声R1不大于的设定的观测异常阈值Rt；

具体包括以下几个步骤：

1)径向距离、法向距离的投影的计算方法为：

其中， 

分别为k-1时刻的自适应滤波东向位置值、北向位置值和航向值；dis为k时刻GPS测量值与k-1时刻卡尔曼滤波值的距离； 

分别为k时刻GPS测量的东向位置和北向位置；angle(k)为依据GPS测量位置计算所得的航向；normal(k)为k时刻法向距离的投影；radial(k)为k时刻径向距离的投影；

2)依据法向、径向距离进行观测噪声估计的方法为：

其中，ΔDR(k)＝P_od(k)*S*T，P_od(k)为k-1～k时间段内里程仪测量的脉冲数；S为里程仪刻度因子；T为卡尔曼滤波周期；max为求最大值的函数；normal(k)为k时刻法向距离的投影；radial(k)为k时刻径向距离的投影；

3)转入执行步骤六，且进入步骤六时的GPS测量噪声协方差估计值为：

(2)如果互差分估计的观测噪声R 1大于观测异常阈值Rt，则直接执行步骤六，且进入步骤六时的GPS测量噪声协方差估计值为：

。

6.根据权利要求1所述的一种基于观测噪声方差阵估计的自适应滤波方法，其特征在于，所述的步骤六具体为：具体为：

①基于观测噪声估计方差阵映射的观测误差水平，自适应因子α_i构造方法如下：

其中，k为组合导航系统滤波解算时刻；i为估计观测向量的第i个分量； 

为第i个观测分量的噪声估计值；ε为组合导航系统期望达到的滤波精度；

②依据自适应因子调节滤波增益阵，采用序贯处理算法进行自适应卡尔曼滤波解算的方法如下：

其中， 

为k-1时刻的卡尔曼滤波估计值； 

为依据k-1时刻滤波值 

对X_k的一步预测；Φ_k，k-1为k-1时刻到k时刻的状态转移矩阵；Γ_k-1为系统噪声矩阵；P_k，k-1为一步预测均方误差矩阵；Q_k-1为k-1时刻系统噪声协方差矩阵；m为观测向量的维数；i为序贯处理算法的迭代次数； 为k时刻经自适应因子调节后的滤波增益矩阵的第i列分量； 

为k时刻第i次迭代中估计的状态向量； 

为k时刻第i次迭代中估计的状态误差方差阵； 为观测噪声协方差矩阵第i个分量； 

为k时刻的量测矩阵的第i列分量； 

为k时刻的观测向量的第i个分量；I为单位矩阵；P_k为估计均方误差矩阵；

卡尔曼滤波解算的结果 

中的各个分量即为运载体的北向位置、东向位置、航向、速度、航向角速率、陀螺仪零偏及里程仪刻度因子的估计值。 

Images