CN101788679B - 一种基于新息正交的sins/gps自适应野值检测与实时补偿方法 - Google Patents

Abstract

一种基于新息正交的SINS/GPS自适应野值检测与实时补偿方法，研究了SINS/GPS(捷联惯性/卫星)组合导航系统的GPS接收数据野值检测与实时补偿问题。本发明在分析卡尔曼滤波(Kalman Filtering)新息正交性的基础上，将野值检测归结为评价函数的确定和阈值的选取问题，给出了检验野值存在的逻辑判断关系，在此基础上进一步给出了野值自适应补偿的修正卡尔曼滤波算法。该发明已经应用于某车载SINS/GPS组合测量系统，通过对车载SINS/GPS组合测量系统实验数据的处理证明其有效抑制了GPS接收数据野值对滤波精度的影响。

Description

一种基于新息正交的SINS/GPS自适应野值检测与实时补偿方法

技术领域

本发明涉及一种动态测量数据野值检测与实时补偿方法，用于实现SINS/GPS组合导航系统GPS接收数据野值的辨识与剔除，提高系统的导航精度和可靠性。

背景技术

捷联惯性导航(SINS)用于为航空遥感、智能交通、无人机、船舶等运载体提供精确的位置、姿态运动信息，具有全自主、运动信息全面、短时高精度的优点，但误差随时间积累，长航时运行条件下将导致导航精度严重下降。卫星导航(GPS)可提供全天时、全天候、全球范围的高精度定位导航信息，且误差不随时间积累，但难以提供高精度姿态信息。SINS/GPS综合了捷联惯性导航和卫星导航的优势，能够连续、实时地获取载体的位置、姿态、速度、加速度以及角速度等全面的空间运动参数，且误差不随时间积累。

实际SINS/GPS组合导航系统应用环境日趋复杂，GPS接收机存在各种噪声和干扰，接收数据野值的出现往往不可避免。GPS接收数据野值的存在已经成为制约SINS/GPS组合导航系统实现高精度、高可靠性导航的瓶颈问题，相关领域学者已经针对动态测量数据野值的剔除方法展开了研究。柳海峰等在其发表在《电机与控制学报》上的论文“Kalman滤波新息正交性抗野值法研究”中根据卡尔曼滤波新息是否满足正交性进行测量数据野值的判断，利用活化函数对含有野值的测量数据进行加权限制，实现测量数据野值的剔除。祝转民等在其发表在《系统工程与电子技术》上的论文“动态测量数据野值的辨识与剔除”中利用新息构造新息统计量，在求解新息数理统计特性的基础上建立野值判断函数实现测量数据中野值的判断，进而通过调整滤波增益矩阵实现测量数据野值的剔除；王光鼎等在其发表在《电子信息与工程学报》上的论文“一种基于卡尔曼滤波处理的北斗卫星无源组合导航自适应野值剔除方法”中也提出了类似的野值剔除方法。

综观上述学者提出的动态测量数据野值辨识与剔除方法，均需要已知实际测量输出数据的数理统计特性，因此难于在线实施。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种基于新息正交的SINS/GPS自适应野值检测与实时补偿方法，该方法仅需要在线计算无野值出现时理想测量输出

的数理统计特性，易于实现。

本发明的技术解决方案为：一种基于新息正交的SINS/GPS自适应野值检测与实时补偿方法，步骤如下：

(1)野值检测

针对线性离散系统描述的SINS/GPS组合导航系统，根据卡尔曼滤波新息正交性，无野值出现时理想测量输出

符合如下正态分布函数：

其中，

表示

的第i个分量，和

分别表示

的均值和方差，

是

的第i的分量，

是

的第i个对角元素。

和

根据卡尔曼滤波理论可得，计算公式为：

根据假设检验理论建立实际测量输出y(k)的野值判断评价函数J_i(k)和阀值J_th，i(k)为：

从而可基于如下逻辑关系判断实际测量输出y(k)中是否存在野值：

(i)如果J_i(k)≤J_th，i(k)时，y_i(k)中无野值出现；

(ii)如果J_i(k)＞J_th，i(k)时，y_i(k)中有野值出现；

其中，y_i(k)为y(k)的第i个分量，α为给定加权系数。这里α可以根据对野值误检率的要求确定。若野值误检率要求是1％之内，取α≥2.58；若野值误检率要求是5％之内，取α≥1.96。

(2)实时补偿

在对实际测量输出y(k)进行野值辨识的基础上，根据卡尔曼滤波理论提出如下的野值自适应补偿的修正的卡尔曼滤波算法：

$\left\{\begin{array}{c}\hat{x}\left(k\right|k-1)=F(k-1)\hat{x}(k-1)\\ \hat{x}\left(k\right)=\hat{x}\left(k\right|k-1)+L\left(k\right)(y_m\left(k\right)-H\left(k\right)\hat{x}\left(k\right|k-1))\\ \hat{x}\left(0\right)={\hat{x}}_0\end{array}\right.$

其中y_m(k)是检测到野值后补偿得到的输出。用y_m，i(k)表示y_m(k)的第i个分量，y_m，i(k)由如下的野值补偿准则确定：

(i)当J_i(k)≤J_th，i(k)时，y_i(k)中无野值出现，y_m，i(k)＝y_i(k)；

(ii)当J_i(k)＞J_th，i(k)时，y_i(k)中有野值出现，y_m，i(k)由下式计算：

其中，F(k-1)、H(k)、L(k)分别为状态转移矩阵、量测矩阵和滤波增益矩阵，

分别为状态一步预测和估计，

为状态初始值，J_i(k)、J_th，i(k)分别为实际测量输出y(k)的野值判断评价函数和阀值，y_i(k)为y(k)的第i个分量，

分别为理想测量输出

的第i个分量

的均值和均方差，α＞0为给定加权系数，sign[θ]表示符号函数，其中

sign[θ]定义如下：

(3)将步骤(2)得到野值自适应补偿的修正的卡尔曼滤波算法，作用于SINS/GPS组合导航系统，实现SINS/GPS组合导航系统GPS接收数据野值的辨识与剔除。

本发明的原理是：线性离散系统描述的SINS/GPS组合导航系统理想情况下其卡尔曼滤波新息满足正交性，测量输出符合正态分布，但当GPS接受数据中有野值出现时，测量输出不再满足正态分布；据此建立野值判断评价函数及阀值，定性分析测量输出中是否存在野值；在测量输出野值定性分析的基础上进一步给出野值补偿算法，利用修正的卡尔曼滤波算法实现野值的自适应实时补偿。

本发明与现有技术相比的优点在于：现有技术在进行野值辨识及剔除时需计算实际测量输出及其新息的数理统计特性，难以在线实现；本发明避开了实际测量输出及其新息的数理统计特性的计算，以无野值存在的理想测量输出及其新息的数理统计特性为依据，建立了实际测量输出的野值辨识评价函数和野值补偿自适应卡尔曼滤波算法，实现了测量数据野值的在线检测与实时补偿。

附图说明

图1为本发明的测量输出野值检测与实时补偿流程图；

图2为DGPS、标准KF和修正KF的纬度比较；

图3为DGPS、标准KF和修正KF的经度比较；

图4为DGPS、标准KF和修正KF的高度比较I；

图5为DGPS、标准KF和修正KF的高度比较II。

具体实施方式

本发明是一种基于新息正交的SINS/GPS自适应野值检测与实时补偿方法，研究了SINS/GPS(捷联惯性/卫星)组合导航系统的GPS接收数据野值检测与实时补偿问题。本发明在分析卡尔曼滤波(Kalman Filtering)新息正交性的基础上，将野值检测归结为评价函数的确定和阈值的选取问题，给出了检验野值存在的逻辑判断关系，在此基础上进一步给出了野值自适应补偿的修正卡尔曼滤波算法。该发明已经应用于某车载SINS/GPS组合测量系统，通过对车载SINS/GPS组合测量系统实验数据的处理证明其有效抑制了GPS接收数据野值对滤波精度的影响。

如图1所示，本发明的具体实施步骤如下：

(1)由动调陀螺、加速度计及DGPS构成的SINS/GPS组合导航系统，其线性离散状态方程为：

$\left\{\begin{array}{c}x(k+1)=F\left(k\right)x\left(k\right)+G\left(k\right)w\left(k\right)\\ y\left(k\right)=H\left(k\right)x\left(k\right)+v\left(k\right)\end{array}\right.$

系统转移矩阵为：

$F\left(k\right)=\left[\begin{array}{cc}{F}_{9\×9}^N\left(k\right)& F_{9\×6}^S\left(k\right)\\ 0_{6\×9}& 0_{6\×6}\end{array}\right]$

其中， $F_{9\×6}^S\left(k\right)=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{3\×3}& 0_{3\×3}\end{array}\right],$ F_9×9 ^N(k)中非零元素 $F_{12}^N={\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_N+h},$

$F_{13}^N=-{\mathrm{\ω}}_{\mathrm{ie}}\cos L-\frac{V_E}{R_N+h},$ $F_{15}^N=-\frac{1}{R_M+h},$ $F_{19}^N=\frac{V_N}{{(R_M+h)}^2},$

$F_{21}^N={-\mathrm{\ω}}_{\mathrm{ie}}\sin L-\frac{V_E\tan L}{R_N+h},$ $F_{23}^N=-\frac{V_N}{R_M+h},$ $F_{24}^N=\frac{1}{R_N+h},$ $F_{27}^N=-{\mathrm{\ω}}_{\mathrm{ie}}\sin L,$

$F_{29}^N=\frac{{-V}_E}{{(R_N+h)}^2},$ $F_{31}^N={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h},$ $F_{32}^N=\frac{V_N}{R_M+h},$ $F_{34}^N=\frac{\tan L}{R_N+h},$

$F_{37}^N={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E{\sec }^{2}L}{R_N+h},$ $F_{39}^N=-\frac{V_E\tan L}{{(R_N+h)}^2},$ $F_{42}^N=-f_U,$ $F_{43}^N=f_N,$

$F_{44}^N=\frac{V_N\tan L-V_U}{R_N+h},$ $F_{45}^N=2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_N+h},$ $F_{46}^N=-2{\mathrm{\ω}}_{\mathrm{ie}}\cos L-\frac{V_E}{R_N+h},$

$F_{47}^N=2{\mathrm{\ω}}_{\mathrm{ie}}(V_N\cos L+V_U\sin L)+\frac{V_E{V}_N{\sec }^{2}L}{R_N+h},$ $F_{49}^N=\frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_N+h)}^2},$ $F_{51}^N=f_U,$

$F_{53}^N=-f_E,$ $F_{55}^N=\frac{{-V}_U}{R_M+h},$ $F_{56}^N=\frac{{-V}_N}{R_M+h},$ $F_{57}^N=-V_E({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E{\sec }^{2}L}{R_N+h}),$

$F_{59}^N=\frac{V_N{V}_U+V_E^2\tan L}{{(R_N+h)}^2},$ $F_{61}^N=-f_N,$ $F_{62}^N=f_E,$ $F_{64}^N=2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}),$

$F_{65}^N=\frac{{2V}_N}{R_M+h},$ $F_{67}^N=-2{\mathrm{\ω}}_{\mathrm{ie}}{V}_E\sin L,$ $F_{69}^N=-\frac{V_E^2+V_N^2}{{(R_N+h)}^2},$ $F_{75}^N=\frac{1}{R_M+h},$

$F_{79}^N=\frac{{-V}_N}{{(R_M+h)}^2},$ $F_{84}^N=\frac{\sec L}{R_N+h},$ $F_{87}^N=\frac{V_E\sec L\tan L}{R_N+h},$ $F_{89}^N=\frac{{-V}_E\sec L}{{(R_N+h)}^2},$ $F_{96}^N=1,$

F_ij ^N为F_9×9 ^N(k)的第i行第j列元素，V_E、V_N和V_U分别为载体东向、北向和天向速度，f_E、f_N和f_U分别为载体东向、北向和天向加速度计的测量值，ω_ie为地球自转角速度，L为当地地理纬度值，C_b ⁿ为载体相对导航坐标系的姿态变换矩阵，R_M和R_N分别为地球沿子午圈和卯酉圈的主曲率半径，h为载体到地球表面的高度值。

系统量测矩阵为：

H＝[H_v H_P]^T

其中，H_v＝[0_3×3 diag(1，1，1) 0_3×9]，

H_p＝[0_3×6 diag(R_M+h，(R_N+h)cosL，1) 0_3×6]，R_M和R_N分别为地球沿子午圈和卯酉圈的主曲率半径，h为载体到地球表面的高度值，L为当地地理纬度值。

系统噪声矩阵为：

$G\left(k\right)=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{9\×3}& 0_{9\×3}\end{array}\right]$

其中，C_b ⁿ为载体相对导航坐标系的姿态变换矩阵。

状态变量为：

其中，

和分别为载体东向、北向和天向姿态误差，δV_E(k)、δV_N(k)和δV_U(k)分别为载体东向、北向和天向速度误差，δL(k)、δλ(k)和δH(k)分别为纬度、经度和高度误差，ε_x(k)、ε_y(k)和ε_z(k)分别为东向、北向和天向陀螺常值漂移，

和

分别为东向、北向和天向加速度计常值偏置。

实际测量输出向量为：

y(k)＝[δV′_E(k) δV′_N(k) δV′_U(k) δL′(k) δλ′(k) δH′(k)]^T

其中，δV′_E(k)、δV′_N(k)、δV′_U(k)、δL′(k)、δλ′(k)、δH′(k)分别为捷联解算与GPS输出的东向、北向、天向速度和位置之差。

系统噪声向量为：

$\mathrm{\ω}\left(k\right)={\left[\begin{array}{cccccc}{\mathrm{\ω}}_{{\mathrm{\ϵ}}_x}\left(k\right)& {\mathrm{\ω}}_{{\mathrm{\ϵ}}_y}\left(k\right)& {\mathrm{\ω}}_{{\mathrm{\ϵ}}_z}\left(k\right)& {\mathrm{\ω}}_{{\▿}_x}\left(k\right)& {\mathrm{\ω}}_{{\▿}_y}\left(k\right)& {\mathrm{\ω}}_{{\▿}_z}\left(k\right)\end{array}\right]}^T$

其协方差阵为：

Q＝diag[(0.1°/h)²，(0.1°/h)²，(0.1°/h)²，(0.0001g)²，(0.0001g)²，(0.0001g)²]

其中，g为重力加速度。

量测噪声向量为：

$v\left(k\right)={\left[\begin{array}{cccccc}{v}_{{\mathrm{\δV}}_E^{\′}}\left(k\right)& v_{{\mathrm{\δV}}_N^{\′}}\left(k\right)& v_{{\mathrm{\δV}}_U^{\′}}\left(k\right)& v_{{\mathrm{\δL}}^{\′}}\left(k\right)& v_{{\mathrm{\δ\λ}}^{\′}}\left(k\right)& v_{{\mathrm{\δH}}^{\′}}\end{array}\left(k\right)\right]}^T$

其协方差阵为：

R＝diag[(0.01m/s)²，(0.01m/s)²，(0.01m/s)²，(0.05m)²，(0.05m)²，(0.05m)²]

(2)令{y(0)，y(1)，…，y(k)}表示测量数据序列，

和

分别表示一步状态预测与估计，并定义如下新息向量：

$\left\{\begin{array}{c}\tilde{x}\left(k\right)=x\left(k\right)-\hat{x}\left(k\right|k-1)\\ e\left(k\right)=y\left(k\right)-H\left(k\right)\hat{x}\left(k\right|k-1)\end{array}\right.$

其中，H(k)为系统量测矩阵，x(k)为系统状态。

记一步状态估计方差阵

则应用卡尔曼滤波可得如下线性最小方差估计：

其中，

为状态估计，初始值F(k-1)为系统状态转移矩阵，增益矩阵L(k)由下式计算：

$\left\{\begin{array}{c}L\left(k\right)=P\left(k\right|k-1)H^T\left(k\right){(H\left(k\right)P\left(k\right|k-1)H^T\left(k\right)+R\left(k\right))}^{-1}\\ P\left(k\right|k-1)=F\left(k\right)P(k-1)F^T\left(k\right)+G\left(k\right)Q\left(k\right)G^T\left(k\right)\\ P\left(k\right)=(I-L\left(k\right)H\left(k\right))P\left(k\right|k-1){(I-L\left(k\right)H\left(k\right))}^T+L\left(k\right)R\left(k\right)L^T\left(k\right)\\ P\left(0\right)=P_0\end{array}\right.$

其中，P(k)为状态估计方差阵，P(k)的初始值为：

P₀＝diag[(0.1°)²，(0.1°)²，(0.5°)²，(0.3m/s)²，(0.3m/s)²，(0.3m/s)²，

        (10m)²，(10m)²，(15m)²，(0.1°/h)²，(0.1°/h)²，(0.1°/h)²，

        (0.0001g)²，(0.0001g)²，(0.0001g)²]

由上述分析可进一步求得：

$\left\{\begin{array}{c}e\left(k\right)=H\left(k\right)\tilde{x}\left(k\right)+v\left(k\right)\\ y\left(k\right)=e\left(k\right)+H\left(k\right)\hat{x}\left(k\right|k-1)\end{array}\right.$

假设在无野值出现的情况下，理想测量输出为

则e(k)和

满足正交条件，因此

的数学期望和协方差阵满足下式：

令无野值时的理想测量输出

即

表示

的第i个分量，则有：

其中

为

的第i个对角线元素。很显然在无野值出现情况下有：

当测量输出中出现野值时，实际测量输出为

其中

为无野值出现时的理想测量输出，d_i(k)表示有界未知输入，此时e(k)和y(k)的正交性不再满足。

根据以上分析野值判断的评价函数J_i(k)和阈值J_th，i(k)可选择如下：

从而可基于如下逻辑关系检测实际测量输出y_i(k)是否存在野值：

(i)如果J_i(k)≤J_th，i(k)时，y_i(k)中无野值出现；

(ii)如果J_i(k)＞J_th，i(k)时，y_i(k)中有野值出现；

其中，增加α意味着提高阈值J_th，i(k)，这将会降低野值的误检率(false detectionrate，FDR)，但是也会同时导致较高的野值漏检率(missed detection rate，MDR)，所以一般选取α为一适当的正数，本实施例中取α＝2.58。

(3)在野值辨识的基础上，为有效抑制野值对滤波性能的影响，本发明提出了如下野值自适应补偿的修正卡尔曼滤波算法：

$\left\{\begin{array}{c}\hat{x}\left(k\right|k-1)=F(k-1)\hat{x}(k-1)\\ \hat{x}\left(k\right)=\hat{x}\left(k\right|k-1)+L\left(k\right)(y_m\left(k\right)-H\left(k\right)\hat{x}\left(k\right|k-1))\\ \hat{x}\left(0\right)={\hat{x}}_0\end{array}\right.$

其中y_m(k)是检测到野值后补偿得到的输出。用y_m，i(k)表示y_m(k)的第i个分量，y_m，i(k)由如下的野值补偿准则确定：

(i)当J_i(k)≤J_th，i(k)时，y_i(k)中无野值出现，y_m，i(k)＝y_i(k)；

(ii)当J_i(k)＞J_th，i(k)时，y_i(k)中有野值出现，y_m，i(k)由下式计算：

其中，F(k-1)、H(k)、L(k)分别为状态转移矩阵、量测矩阵和滤波增益矩阵，

分别为状态一步预测和估计，

为状态初始值，J_i(k)、J_th，i(k)分别为实际测量输出y(k)的野值判断评价函数和阀值，y_i(k)为y(k)的第i个分量，

分别为理想测量输出的第i个分量

的均值和均方差，α＞0为给定加权系数，sign[θ]表示符号函数，其中

sign[θ]定义如下：

图2至图5为本实施例的实验结果，从中可以看出，利用本发明提出的方法能够有效检测到野值的存在并对其进行抑制，得到了较理想的纬度、经度和高度估计值。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (1)

1.一种基于新息正交的SINS/GPS自适应野值检测与实时补偿方法，其特征在于步骤如下：

（1）野值检测

针对线性离散系统描述的SINS/GPS组合导航系统，根据卡尔曼滤波新息正交性，确定无野值出现时理想测量输出

符合正态分布函数：

其中，N(E[y_i(k)],分别表示均值为

方差为的正态分布函数，

表示

的第i个分量，

和

的均值和方差，

是

的第i个分量，

是

的第i个对角元素，

和

根据卡尔曼滤波理论得，计算公式如下：

其中P(k|k-1)为一步状态估计方差阵，R(k)为量测噪声向量协方差阵；

根据假设检验原理建立实际测量输出y(k)的野值判断评价函数J_i(k)和阀值J_th,i(k)为：

从而基于如下逻辑关系判断实际测量输出y(k)中是否存在野值：

（i）如果J_i(k)≤J_th,i(k)时，y_i(k)中无野值出现；

(ii)如果J_i(k)＞J_th,i(k)时，y_i(k)中有野值出现；

其中，y_i(k)为y(k)的第i个分量；α为给定加权系数，根据对野值误检率的要求确定，若野值误检率要求是1%之内，取α≥2.58；若野值误检率要求是5%之内，取α≥1.96；

（2）实时补偿

在对实际测量输出y(k)进行野值检测的基础上，根据卡尔曼滤波理论提出如下的野值自适应补偿的修正的卡尔曼滤波算法：

$\left\{\begin{array}{c}\hat{x}\left(k\right|k-1)=F(k-1)\hat{x}(k-1)\\ \hat{x}\left(k\right)=\hat{x}\left(k\right|k-1)+L\left(k\right)(y_m\left(k\right)-H\left(k\right)\hat{x}\left(k\right|k-1))\\ \hat{x}\left(0\right)={\hat{x}}_0\end{array}\right.$

其中y_m(k)是检测到野值后补偿得到的输出，用y_m,i(k)表示y_m(k)的第i个分量，y_m,i(k)由如下的野值补偿准则确定：

(i)当J_i(k)≤J_th,i(k)时，y_i(k)中无野值出现，y_m,i(k)＝y_i(k)；

(ii)当J_i(k)＞J_th,i(k)时，y_i(k)中有野值出现，y_m,i(k)由下式计算：

其中，F(k-1)、H(k)、L(k)分别为状态转移矩阵、量测矩阵和滤波增益矩阵，

分别为状态一步预测和估计，为状态初始值，J_i(k)、J_th,i(k)分别为实际测量输出y(k)的野值判断评价函数和阀值，y_i(k)为y(k)的第i个分量，分别为理想测量输出

的第i个分量

的均值和均方差，sign[θ]表示符号函数，其中

sign[θ]定义如下：

（3）将步骤（2）得到野值自适应补偿的修正的卡尔曼滤波算法，作用于SINS/GPS组合导航系统，实现SINS/GPS组合导航系统GPS接收数据野值的辨识与剔除。

Images