CN102538788A - 一种基于状态估计和预测的低成本阻尼导航方法 - Google Patents

Abstract

本发明属于导航领域，具体涉及一种基于状态估计和预测的低成本阻尼导航方法。本方法利用卫星信息和惯导信息之差作为观测量，通过卡尔曼滤波对各项惯性误差进行估计，以速度误差作为控制量修正惯性导航的速度误差和失调角；并根据惯性误差估计结果对惯性输出信息进行修正。本发明提供了一种简单、廉价、有效的阻尼导航方法，提高低成本惯性/卫星组合导航系统精度，特别是姿态精度，以适应低成本高精度导航系统的要求。

Description

一种基于状态估计和预测的低成本阻尼导航方法

技术领域

本发明属于导航领域，具体涉及一种惯性/卫星组合导航系统基于状态估计和预测的阻尼方法，特别是一种低成本MEMS/GPS组合导航的误差阻尼方法。 

背景技术

在惯性技术高速发展的基础上，低成本微机械惯性测量单元逐渐被广泛应用于惯性测量领域。但其精度较差，零偏稳定性和随机漂移比较大，无法进行惯性自主导航，即使使用GPS等辅助导航手段进行组合导航也难于克服非线性系统误差估计精度差的缺点。 

发明内容

本发明的目的为提高低成本惯性/卫星组合导航系统精度，提供一种基于状态估计和预测的低成本阻尼导航方法。 

本发明所采用的技术方案是： 

一种基于状态估计和预测的低成本阻尼导航方法，利用卫星信息和惯导信息之差作为观测量，通过卡尔曼滤波对各项惯性误差进行估计，以速度误差作为控制量修正惯性导航的速度误差和失调角；并根据惯性误差估计结果对惯性输出信息进行修正。 

如上所述的一种基于状态估计和预测的低成本阻尼导航方法，其中：当卫星导航系统正常定位时，惯性导航系统以卫星导航信息与惯导导航信息作为量测输入，使用卡尔曼滤波方法估计惯性导航系统各项惯性误差；当卫星导航出现异常时，不再进行量测计算，使用卡尔曼滤波方法预测惯性导航系统各项惯性误差。 

如上所述的一种基于状态估计和预测的低成本阻尼导航方法，其中：在 指北导航坐标系下，选取位置误差 

Δλ，速度误差ΔV_N、ΔV_E，失调角Φ_N、Φ_E、Φ_U，陀螺漂移ε_z、ε_y、ε_z，加速度计零偏 

作为系统状态变量。 

如上所述的一种基于状态估计和预测的低成本阻尼导航方法，其中：根据系统状态变量建立的阻尼导航惯导基本误差方程为： 

$\mathrm{\Δ}{\stackrel{\·}{v}}_N=a_U{\mathrm{\Φ}}_E-a_E{\mathrm{\Φ}}_{\mathrm{up}}+T_{11}{\▿}_x+T_{13}{\▿}_z+U_N^1$

$\mathrm{\Δ}{\stackrel{\·}{V}}_E=-a_U{\mathrm{\Φ}}_N+a_N{\mathrm{\Φ}}_{\mathrm{up}}+T_{31}{\▿}_x+T_{33}{\▿}_z+U_E^1$

${\stackrel{\·}{\mathrm{\Φ}}}_N=\frac{1}{R_N+h}\mathrm{\Δ}{V}_E-T_{11}{\mathrm{\ϵ}}_x-T_{12}{\mathrm{\ϵ}}_y-T_{13}{\mathrm{\ϵ}}_z+U_N^2$

${\stackrel{\·}{\mathrm{\Φ}}}_E=\frac{1}{R_M+h}\mathrm{\Δ}{V}_N-T_{31}{\mathrm{\ϵ}}_x-T_{32}{\mathrm{\ϵ}}_y-T_{33}{\mathrm{\ϵ}}_z+U_E^2$

${\stackrel{\·}{\mathrm{\ϵ}}}_x=-\frac{1}{{\mathrm{\τ}}_x}{\mathrm{\ϵ}}_x+{\mathrm{\ω}}_{\mathrm{xr}}$

${\stackrel{\·}{\mathrm{\ϵ}}}_y=-\frac{1}{{\mathrm{\τ}}_y}{\mathrm{\ϵ}}_y+{\mathrm{\ω}}_{\mathrm{yr}}$

${\stackrel{\·}{\mathrm{\ϵ}}}_z=-\frac{1}{{\mathrm{\τ}}_z}{\mathrm{\ϵ}}_z+{\mathrm{\ω}}_{\mathrm{zr}}$

${\stackrel{\·}{\▿}}_x={\▿}_{\mathrm{xr}}$

${\stackrel{\·}{\▿}}_z={\▿}_{\mathrm{zr}}$

式中： 

V_E为导航坐标系北向速度值，单位：米/秒； 

为导航得到的当地纬度值，单位：弧度； 

h为海拔高度，单位：米； 

R_M、R_N分别为地球子午圈和卯酉圈地球半径，单位：米； 

τ_x、τ_y、τ_z为陀螺一阶马尔可夫过程的时间常数； 

ω_xr、ω_yr、ω_zr为陀螺零均值高斯白噪声； 

为加速度计零均值白噪声； 

T_ij(i＝1～3，j＝1～3)为捷联姿态矩阵 

相应各元素， $C_b^n=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right];$

分别为惯性导航的北向和东向速度误差修正量； 

分别为惯性导航的北向和东向失调角修正量； 

为 

的导数。 

如上所述的一种基于状态估计和预测的低成本阻尼导航方法，其中：所述修正量 

计算如下： 

$U_N^1=-C_1\mathrm{\Δ}{\hat{V}}_N$

$U_E^1=-C_1\mathrm{\Δ}{\hat{V}}_E$

$U_N^2=C_2\mathrm{\Δ}{\hat{V}}_E$

$U_E^2={-C}_2\mathrm{\Δ}{\hat{V}}_N$

其中，C₁、C₂为误差修正阻尼系数，取值范围为0～1； 

分别为惯性导航的北向和东向速度误差估计结果。 

如上所述的一种基于状态估计和预测的低成本阻尼导航方法，其中：所述卡尔曼滤波过程如下： 

取系统噪声向量： 

$W={\left[\begin{array}{ccccc}{\▿}_{\mathrm{xr}}& {\▿}_{\mathrm{zr}}& {\mathrm{\ϵ}}_{\mathrm{xr}}& {\mathrm{\ϵ}}_{\mathrm{yr}}& {\mathrm{\ϵ}}_{\mathrm{zr}}\end{array}\right]}^T$

其中， 

ε_xr，ε_yr，ε_zr分别为 

ε_x，ε_y，ε_z的噪声； 

取惯导系统和卫星定位系统的位置、速度差为观测向量： 

其中， 

λ、V_E、V_N表示由惯导得到的纬度、经度、东向速度、北向速度， 

λ_GPS、V_E-GPS、V_N-GPS表示由GPS得到的纬度、经度、东向速度、北向速度； 

惯导系统误差状态方程表示为： 

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+\mathrm{LU}\left(t\right)+\mathrm{DW}\left(t\right)$

其中，A(t)为系统参数矩阵，D为系统噪声矩阵，L为控制量系数矩阵， U(t)为修正量， $U\left(t\right)={\left[\begin{array}{cc}\mathrm{\Δ}{\hat{V}}_N& \mathrm{\Δ}{\hat{V}}_E\end{array}\right]}^T;$

量测方程为： 

Y＝HX(t)+η(t) 

其中，H为量测参数阵，η(t)为量测噪声向量； 

将状态方程和量测方程离散化为： 

X_k＝Φ_k，k-1X_k-1+LU_k-1+DW_k-1

Y_k＝HX_k+η_k

${\mathrm{\Φ}}_{k,k-1}=I+A_{k}T+\frac{1}{2}{A}_k^2{T}^2$

其中，k表示卡尔曼滤波拍数，Φ_k，k-1为状态转移矩阵，T为滤波周期，I为单位矩阵，A_k为系统参数矩阵，U_k-1为修正量，W_k-1为系统噪声，Y_k为观测量。 

如上所述的一种基于状态估计和预测的低成本阻尼导航方法，其中：当卫星导航系统正常定位时，卡尔曼滤波器滤波模型为： 

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+L{U}_{k-1}$

${\hat{X}}_k={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k,k-1}+K_k(Y_k-H{\hat{X}}_{k,k-1})$

K_k＝P_k，k-1H^T(HP_k，k-1H^T+R_K)^-1

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+D{Q}_{k-1}{D}^T$

$P_k=(I-K_{k}H)P_{k,k-1}{(I-K_{k}H)}^T+K_k{R}_k{K}_k^T$

当卫星导航出现异常时，卡尔曼滤波器预测模型为： 

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+{\mathrm{LU}}_{k-1}$

${\hat{X}}_k={\hat{X}}_{k,k-1}$

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{DQ}}_{k-1}{D}^T$

P_k＝P_k-1

其中， 

为状态一步预测值，Φ_k，k-1为状态转移矩阵，K_k为滤波器增益阵，L为控制量系数矩阵，U_k-1为修正量，D为系统噪声矩阵，P_k，k-1为滤波器预测误差方差阵，P_k为滤波器的协方差阵，R_k为滤波器量测噪声方差阵，H为量测参数阵，Q_k为滤波器系统噪声方差阵， 

为状态估计量，上标T表 示转置。 

本发明的有益效果是： 

1.本发明提供了一种简单、廉价、有效的阻尼导航方法，提高低成本惯性/卫星组合导航系统精度，特别是姿态精度，以适应低成本高精度导航系统的要求。解决了低成本、低精度惯性测量单元导航误差大引起的非线性系统误差估计精度差的问题。 

2.本发明通过卡尔曼滤波对各项惯性误差进行估计，以速度误差作为控制量通过校正网络阻尼惯性导航的舒拉回路，实时修正惯性导航的速度误差和失调角，将其控制在较小的范围内。实现低成本惯性/卫星组合导航系统的高精度，提高动态性能和长时间工作稳定性。 

具体实施方式

下面结合实施例对本发明提供的一种基于状态估计和预测的低成本阻尼导航方法进行介绍： 

一种基于状态估计和预测的低成本阻尼导航方法，利用卫星信息和惯导信息之差作为观测量，通过卡尔曼滤波对各项惯性误差进行估计，以速度误差作为控制量通过校正网络阻尼惯性导航的舒拉回路，将惯性导航的速度误差和失调角控制在较小的范围内；并根据惯性误差估计结果对惯性输出信息进行修正，以获得高精度的位置、速度和姿态信息。具体包括如下步骤： 

(1)选取多项惯性误差作为系统状态变量建立卡尔曼滤波模型。在指北导航坐标系下，选取位置误差 

Δλ，速度误差ΔV_N、ΔV_E，失调角Φ_N、Φ_E、Φ_U，陀螺漂移ε_z、ε_y、ε_z，加速度计零偏 

作为系统状态变量，即： 

其中： 

为纬度误差，单位：弧度； 

Δλ为经度误差，单位：弧度； 

ΔV_N为北向速度误差，单位：米/秒； 

ΔV_E为东向速度误差，单位：米/秒； 

Φ_N为北向失调角，单位：弧度； 

Φ_E为东向失调角，单位：弧度； 

Φ_U为天向失调角，单位：弧度； 

ε_x为处理成一阶马尔可夫过程的载体系X向陀螺漂移误差，单位：弧度/秒； 

ε_y为处理成一阶马尔可夫过程的载体系Y向陀螺漂移误差，单位：弧度/秒； 

ε_z为处理成一阶马尔可夫过程的载体系Z向陀螺漂移误差，单位：弧度/秒； 

为载体系X向加速度计零偏误差，单位：米/秒²； 

为载体系Z向加速度计零偏误差，单位：米/秒²。 

阻尼导航惯导基本误差方程： 

$\mathrm{\Δ}{\stackrel{\·}{V}}_N=a_U{\mathrm{\Φ}}_E-a_E{\mathrm{\Φ}}_{\mathrm{up}}+T_{11}{\▿}_x+T_{13}{\▿}_z+U_N^1$

$\mathrm{\Δ}{\stackrel{\·}{V}}_E=-a_U{\mathrm{\Φ}}_N+a_N{\mathrm{\Φ}}_{\mathrm{up}}+T_{13}{\▿}_x+T_{33}{\▿}_z+U_E^1$

${\stackrel{\·}{\mathrm{\Φ}}}_N=\frac{1}{R_N+h}\mathrm{\Δ}{V}_E-T_{11}{\mathrm{\ϵ}}_x-T_{12}{\mathrm{\ϵ}}_y-T_{13}{\mathrm{\ϵ}}_z+U_N^2$

${\stackrel{\·}{\mathrm{\Φ}}}_E=-\frac{1}{R_M+h}\mathrm{\Δ}{V}_N-T_{31}{\mathrm{\ϵ}}_x-T_{32}{\mathrm{\ϵ}}_y-T_{33}{\mathrm{\ϵ}}_z+U_E^2---\left(2\right)$

${\stackrel{\·}{\mathrm{\ϵ}}}_x=-\frac{1}{{\mathrm{\τ}}_x}{\mathrm{\ϵ}}_x+{\mathrm{\ω}}_{\mathrm{xr}}$

${\stackrel{\·}{\mathrm{\ϵ}}}_y=-\frac{1}{{\mathrm{\τ}}_y}{\mathrm{\ϵ}}_y+{\mathrm{\ω}}_{\mathrm{yr}}$

${\stackrel{\·}{\mathrm{\ϵ}}}_z=-\frac{1}{{\mathrm{\τ}}_z}{\mathrm{\ϵ}}_z+{\mathrm{\ω}}_{\mathrm{zr}}$

${\stackrel{\·}{\▿}}_x={\▿}_{\mathrm{xr}}$

${\stackrel{\·}{\▿}}_z={\▿}_{\mathrm{zr}}$

式中： 

V_E为导航坐标系北向速度值，单位：米/秒； 

为导航得到的当地纬度值，单位：弧度； 

h为海拔高度，单位：米； 

R_M、R_N分别为地球子午圈和卯酉圈地球半径，单位：米； 

τ_x、τ_y、τ_z为陀螺一阶马尔可夫过程的时间常数； 

ω_xr、ω_yr、ω_zr为陀螺零均值高斯白噪声； 

为加速度计零均值白噪声； 

T_ij(i＝1～3，j＝1～3)为捷联姿态矩阵 

相应各元素， $C_b^n=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right];$

分别为惯性导航的北向和东向速度误差修正量； 

分别为惯性导航的北向和东向失调角修正量； 

为 的导数。 

(2)利用惯性导航的北向和东向速度误差估计修正卡尔曼滤波模型中速度误差与失调角。 

式(2)中修正量 

计算如下： 

$U_N^1=-C_1\mathrm{\Δ}{\hat{V}}_N$

$U_E^1=-C_1\mathrm{\Δ}{\hat{V}}_E---\left(3\right)$

$U_N^2=C_2\mathrm{\Δ}{\hat{V}}_E$

$U_E^2={-C}_2\mathrm{\Δ}{\hat{V}}_N$

其中，C₁、C₂为误差修正阻尼系数，取值范围为0～1； 

分别为惯性导航的北向和东向速度误差估计结果。 

(3)求解不同情况下的卡尔曼滤波模型。 

取系统噪声向量： 

$W={\left[\begin{array}{ccccc}{\▿}_{\mathrm{xr}}& {\▿}_{\mathrm{zr}}& {\mathrm{\ϵ}}_{\mathrm{xr}}& {\mathrm{\ϵ}}_{\mathrm{yr}}& {\mathrm{\ϵ}}_{\mathrm{zr}}\end{array}\right]}^T---\left(4\right)$

其中， 

ε_xr，ε_yr，ε_zr分别为 

ε_x，ε_y，ε_z的噪声。 

取惯导系统和卫星定位系统的位置、速度差为观测向量： 

其中， 

λ、V_E、V_N表示由惯导得到的纬度、经度、东向速度、北向速度， 

λ_GPS、V_E-GPS、V_N-GPS表示由GPS得到的纬度、经度、东向速度、北向速度。 

惯导系统误差状态方程可表示为： 

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+\mathrm{LU}\left(t\right)+\mathrm{DW}\left(t\right)---\left(6\right)$

式中：A(t)为12×12维系统参数矩阵，根据式(2)计算；D为12×12维系统噪声矩阵；L为12×2维控制量系数矩阵；U(t)为修正量， $U\left(t\right)={\left[\begin{array}{cc}\mathrm{\Δ}{\hat{V}}_N& \mathrm{\Δ}{\hat{V}}_E\end{array}\right]}^T.$

量测方程为： 

Y＝HX(t)+η(t)                                 (7) 

式中H为量测参数阵，η(t)为量测噪声向量。 

将状态方程和量测方程离散化为： 

X_k＝Φ_k，k-1X_k-1+LU_k-1+DW_k-1

Y_k＝HX_k+η_k                                    (8) 

${\mathrm{\Φ}}_{k,k-1}=I+A_{k}T+\frac{1}{2}{A}_k^2{T}^2$

式中k表示卡尔曼滤波拍数，Φ_k，k-1为状态转移矩阵，T为滤波周期，I为单位矩阵，A_k为系统参数矩阵，U_k-1为修正量，W_k-1为系统噪声，Y_k为观测量。 

当基准信息有效时，卡尔曼滤波器滤波模型为： 

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+L{U}_{k-1}$

${\hat{X}}_k={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k,k-1}+K_k(Y_k-H{\hat{X}}_{k,k-1})$

K_k＝P_k，k-1H^T(HP_k，k-1H^T+R_k)^-1

(9) 

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+D{Q}_{k-1}{D}^T$

$P_k=(I-K_{k}H)P_{k,k-1}{(I-K_{k}H)}^T+K_k{R}_k{K}_k^T$

当基准信息无效时，卡尔曼滤波器预测模型为： 

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+{\mathrm{LU}}_{k-1}$

${\hat{X}}_k={\hat{X}}_{k,k-1}---\left(10\right)$

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{DQ}}_{k-1}{D}^T$

P_k＝P_k-1

式(9)(10)中： 

为状态一步预测值，K_k为滤波器增益阵，P_k，k-1为滤波器预测误差方差阵，P_k为滤波器的协方差阵，R_k为滤波器量测噪声方差阵，Q_k为滤波器系统噪声方差阵， 

为状态估计量，上标T表示转置。 

当判断卫星导航系统正常定位时，采用惯性导航系统和卫星定位位置、速度信息，使用式(5)计算观测量，使用式(9)所述的卡尔曼滤波算法估计惯性导航系统位置、速度、失调角等误差。当卫星导航出现失锁或异常等无效时，使用式(10)卡尔曼滤波器预测模型预测惯性导航系统位置、速度、失调角等误差。根据估计或预测速度误差，通过式(3)实时反馈修正惯性导航速度和失调角误差，保证长时间工作各项误差仍保持小量，满足系统线性化条件。根据估计或预测的位置误差、速度误差和失调角误差修正惯性导航的位置、速度和姿态角，误差修正后便得到了阻尼导航系统高精度的组合位置、速度和姿态信息。 

试验时，采用MEMS/GPS组合导航系统，陀螺漂移零偏重复性和稳定性均优于30°/h，选择阻尼系数C₁＝0.1414、C₂＝0.001，建立惯导误差模型，进行阻尼式导航。根据卫星状态判断卫星是否正常，当卫星导航系统正常定位时，惯性导航系统以卫星导航信息与惯导导航信息作为量测输入，使用卡尔曼滤波算法估计惯性导航系统位置、速度、水平失调角等误差；当卫星导航 出现异常时，不再进行量测计算，使用卡尔曼滤波算法预测惯性导航系统位置、速度、水平失调角等误差。根据估计或预测的惯导系统速度误差信息，通过阻尼系数对惯导速度、失调角误差进行补偿。 

MEMS/GPS组合导航系统在静态条件下进入阻尼式导航状态后，采用卡尔曼滤波算法估计惯导的误差信息，通过阻尼系数进行速度误差和失调角的修正，1min后阻尼式导航算法收敛，实时修正的惯性导航纬度误差为46.53m，经度误差为-75.68m，北速误差为0.101m/s，东速误差为-0.105m/s。卡尔曼滤波估计结果为： 

根据估计结果，组合纬度误差为-0.73m，组合经度误差为-2.16m，组合北速误差为0.003m/s，组合东速误差为-0.004m/s，有效提高了组合导航精度。 

导航至1953s卫星开始失锁，失锁97s卫星重新定位，在卫星失锁过程中，采用速度误差的预测值修正惯导系统的速度和姿态角，失锁97s内组合纬度误差为-58.77m，组合经度误差为-33.73m，组合北速误差为-1.10m/s，组合东速误差为-0.58m/s，有效提高了卫星失锁过程中的组合导航精度。 

Claims (7)

1.一种基于状态估计和预测的低成本阻尼导航方法，利用卫星信息和惯导信息之差作为观测量，通过卡尔曼滤波对各项惯性误差进行估计，以速度误差作为控制量修正惯性导航的速度误差和失调角；并根据惯性误差估计结果对惯性输出信息进行修正。

2.根据权利要求1所述的一种基于状态估计和预测的低成本阻尼导航方法，其特征在于：当卫星导航系统正常定位时，惯性导航系统以卫星导航信息与惯导导航信息作为量测输入，使用卡尔曼滤波方法估计惯性导航系统各项惯性误差；当卫星导航出现异常时，不再进行量测计算，使用卡尔曼滤波方法预测惯性导航系统各项惯性误差。

3.根据权利要求1或2所述的一种基于状态估计和预测的低成本阻尼导航方法，其特征在于：在指北导航坐标系下，选取位置误差 

Δλ，速度误差ΔV_N、ΔV_E，失调角Φ_N、Φ_E、Φ_U，陀螺漂移ε_z、ε_y、ε_z，加速度计零偏 

作为系统状态变量。

4.根据权利要求3所述的一种基于状态估计和预测的低成本阻尼导航方法，其特征在于：根据系统状态变量建立的阻尼导航惯导基本误差方程为：

式中：

V_E为导航坐标系北向速度值，单位：米/秒；

为导航得到的当地纬度值，单位：弧度；

h为海拔高度，单位：米；

R_M、R_N分别为地球子午圈和卯酉圈地球半径，单位：米；

τ_x、τ_y、τ_z为陀螺一阶马尔可夫过程的时间常数；

ω_xr、ω_yr、ω_zr为陀螺零均值高斯白噪声；

为加速度计零均值白噪声；

T_ij(i＝1～3，j＝1～3)为捷联姿态矩阵 

相应各元素，

分别为惯性导航的北向和东向速度误差修正量；

分别为惯性导航的北向和东向失调角修正量； 

为 

的导数。

5.根据权利要求4所述的一种基于状态估计和预测的低成本阻尼导航方法，其特征在于：所述修正量 

计算如下：

其中，C₁、C₂为误差修正阻尼系数，取值范围为0～1；

分别为惯性导航的北向和东向速度误差估计结果。

6.根据权利要求5所述的一种基于状态估计和预测的低成本阻尼导航方法，其特征在于：所述卡尔曼滤波过程如下：

取系统噪声向量：

其中， 

ε_xr，ε_yr，ε_zr分别为 

ε_x，ε_y，ε_z的噪声；

取惯导系统和卫星定位系统的位置、速度差为观测向量：

其中， 

λ、V_E、V_N表示由惯导得到的纬度、经度、东向速度、北向速度， λ_GPS、V_E-GPS、V_N-GPS表示由GPS得到的纬度、经度、东向速度、北向速度；

惯导系统误差状态方程表示为：

其中，A(t)为12×12维系统参数矩阵，D为12×12维系统噪声矩阵，L为12×2维控制量系数矩阵，U(t)为修正量，

量测方程为：

Y＝HX(t)+η(t)

其中，H为量测参数阵，η(t)为量测噪声向量；

将状态方程和量测方程离散化为：

X_k＝Φ_k，k-1X_k-1+LU_k-1+DW_k-1

Y_k＝HX_k+η_k

其中，k表示卡尔曼滤波拍数，Φ_k，k-1为状态转移矩阵，T为滤波周期，I为单位矩阵，A_k为系统参数矩阵，U_k-1为修正量，W_k-1为系统噪声，Y_k为观测量。

7.根据权利要求6所述的一种基于状态估计和预测的低成本阻尼导航方法，其特征在于：当卫星导航系统正常定位时，卡尔曼滤波器滤波模型为：

K_k＝P_k，k-1H^T(HP_k，k-1H^T+R_k)^-1

当卫星导航出现异常时，卡尔曼滤波器预测模型为：

P_k＝P_k-1

其中， 

为状态一步预测值，Φ_k，k-1为状态转移矩阵，K_k为滤波器增益阵，L为控制量系数矩阵，U_k-1为修正量，D为系统噪声矩阵，P_k，k-1为滤波器预测误差方差阵，P_k为滤波器的协方差阵，R_k为滤波器量测噪声方差阵，H为量测参数阵，Q_k为滤波器系统噪声方差阵， 

为状态估 计量，上标T表示转置。