一种基于强跟踪SDRE滤波的组合导航系统及导航方法

摘要

本发明公开了一种基于强跟踪SDRE滤波的组合导航系统及导航方法，其中，所述系统包括GPS接收机、惯性导航子系统、和滤波器，其中，所述滤波器包括空间模块、SDRE分解模块、离散化处理模块和强跟踪滤波模块，并且，所述强跟踪滤波模块又包括初始化模块、一步预测子模块、增益矩阵获得子模块、实际状态估计子模块和误差方差阵更新子模块。所述方法包括以下步骤：先进行SDRE分解，然后进行离散化处理，最后进行强跟踪滤波，得到组合导航数据，并进行数据输出。本发明所述的系统和方法引入了自适应渐消因子，能够进行准确滤波，得到精确的导航数据。

说明书

技术领域

本发明涉及导航领域，尤其涉及组合导航系统及导航方法，特别地，涉及一种基于强跟踪SDRE滤波的组合导航系统及导航方法。

背景技术

基于GPS技术和惯性导航的组合导航已被广泛应用于军民诸多领域，该技术对GPS定位数据和惯性导航定位数据进行估计、修正、融合，从而达到优势互补以提高载体导航定位精度的目的。在GPS和惯性导航组合导航中以导航参数作为估计对象，可直观描述导航参数的动态变化过程，准确反映真实状态的演变情况。

在组合导航系统中，实际系统总是存在不同程度的非线性。同时，实际系统还存在高斯或非高斯的随机噪声干扰。在这种情况下，对非线性系统进行有效甚至最优状态估计十分重要，需要采用非线性滤波技术。在面对非线性系统时，一般采用扩展卡尔曼滤波(EKF)进行处理。

但这种方法具有高阶截断误差，并且必须求解系统的Jacobian矩阵，同时还存在着强非线性系统下失去滤波稳定性的可能。滤波技术的基础是建立准确的模型，如果系统模型不精确，会影响滤波精度。

发明内容

为了克服上述问题，本发明人进行了锐意研究，使用GPS接收机、惯性导航(INS)子系统(简称惯导系统)组成组合导航系统，其次使用SDRE技术处理滤波模型，用残差序列处处正交的原理设计滤波器，设计出一种基于强跟踪SDRE滤波的组合导航系统及导航方法，这样，可以得到高精度的位置和速度信息，实现精确的组合导航，从而完成本发明。

本发明一方面提供了一种基于强跟踪SDRE滤波的组合导航系统，其中，所述系统包括GPS接收机1、惯性导航子系统2、和滤波器3，其中，所述滤波器3包括空间模块31、SDRE分解模块32、离散化处理模块33、和强跟踪滤波模块34，并且，所述强跟踪滤波模块34包括初始化模块340、一步预测子模块341、增益矩阵获得子模块342、实际状态估计子模块343和误差方差阵更新子模块344。

本发明另一方面提供了一种基于强跟踪SDRE滤波的导航方法，优选利用本发明第一方面所述系统进行，其中，所述方法包括以下步骤：

步骤1、利用SDRE分解模块32对空间模块31内的状态空间和量测空间进行状态相关分解，分别得到状态分解空间和量测分解空间；

步骤2、利用离散化处理模块33对SDRE分解模块32内的状态分解空间和量测分解空间进行离散化处理，分别得到状态离散空间和量测离散空间；

步骤3、利用强跟踪滤波模块34对离散化处理模块33内的状态离散空间和量测离散空间进行强跟踪滤波处理，得到组合导航数据，并进行数据输出。

附图说明

图1示出本发明所述组合导航系统的结构示意图；

图2示出本发明所述组合导航方法的流程图；

图3示出在实验例中飞行器所做的运动轨迹；

图4a示出利用实施例和对比例所述方法得到的在东向的位置误差；

图4b示出利用实施例和对比例所述方法得到的在北向的位置误差；

图4c示出利用实施例和对比例所述方法得到的在天向的位置误差；

图5a示出利用实施例和对比例所述方法得到的在东向的速度误差；

图5b示出利用实施例和对比例所述方法得到的在北向的速度误差；

图5c示出利用实施例和对比例所述方法得到的在天向的速度误差。

附图标记

1-GPS接收机；2-惯性导航子系统；21-加速度计；22-陀螺仪；3-滤波器；31-空间模块；32-SDRE分解模块；33-离散化处理模块；34-强跟踪滤波模块；340-初始化模块；341-一步预测子模块；342-增益矩阵获得子模块；343-实际状态估计子模块；344-误差方差阵更新子模块。

具体实施方式

下面通过附图对本发明进一步详细说明。通过这些说明，本发明的特点和优点将变得更为清楚明确。

本发明一方面提供了一种基于强跟踪SDRE滤波的组合导航系统，所述导航系统包括GPS接收机1、惯性导航子系统2和滤波器3。其中，所述GPS接收机1用于输出通过GPS技术得到的当前地理位置和当前速度信息；所述惯性导航子系统2包括加速度计21和陀螺仪22，用于检测并输出通过惯性导航技术得到的当前地理位置、当前速度和当前姿态角信息；所述滤波器3用于对GPS接收机1和惯性导航子系统2输出的数据进行数据处理，得到组合导航信息并进行输出。

根据本发明一种优选的实施方式，所述滤波器3包括空间模块31、SDRE分解模块32、离散化处理模块33和强跟踪滤波模块34。

在进一步优选的实施方式中，在所述空间模块31内集成有状态空间和量测空间。

在更进一步优选的实施方式中，所述状态空间和量测空间分别如式(31-1)和式(31-2)所示：

z＝h(x)+V 式(31-2)

其中，在式(31-1)中：

其中，φ_e、φ_n和φ_u分别为数学平台误差角，实时进行更新；v_e、v_n和v_u分别为载体的东向、北向、天向的速度；λ和h分别为载体所在的纬度，经度和高度；ε_e、ε_n和ε_u分别为陀螺仪常值漂移，Δ_e、Δ_n和Δ_u分别为加速度计常值漂移；

其中，和分别表示陀螺仪在东向、北向和天向的随机误差，和分别表示加速度计在东向、北向和天向的随机误差；其中，所述随机误差即为系统噪声；

G为系统噪声转移矩阵，其中，

其中，I表示单位矩阵。

根据本发明一种优选的实施方式，如式(31-2)所示的状态空间如下建立：以地理系(东、北、天)为导航解算的基本坐标系，考虑飞行高度h，并假设地球为椭球体，惯性导航(INS)状态方程如下：

(1)指北惯性导航系统姿态误差角方程：

其中，在式(31-1-1)～式(31-1-3)中，R_M是当地子午面内主曲率半径；R_N是卯酉面内主曲率半径；ω_ie是地球自转角速率；λ和h分别为惯导系统(INS)测得的载体所在的纬度、经度和高度；φ_e、φ_n和φ_e为数学平台误差角；v_e、v_n和v_u分别为惯导系统(INS)测得的载体的东向、北向和天向的速度；ε_e、ε_n和ε_u分别为陀螺仪在东向、北向和天向的常值漂移；δv_e为惯导系统(INS)所测的东向速度与GPS接收机所测的东向速度之差，δv_n为惯导系统(INS)所测的东向速度与GPS接收机所测的北向速度之差；为惯导系统(INS)所测的纬度与GPS接收机所测的纬度之差，δh为惯导系统(INS)所测的高度与GPS接收机所测的高度之差。其中，地球长半径R_e＝6378245，地球扁率e＝1/298.257。

(2)速度方程为：

其中，在式(31-1-4)～式(31-1-6)中，f_e、f_n和f_u分别为加速度计的测量值在东向、北向和天向三个方向的加速度分量；Δ_e、Δ_n和Δ_u为加速度计在东向、北向和天向的常值漂移。

(3)位置方程：

(4)惯性元件误差方程：假设惯性元件的误差是常值噪声，则其误差方程为：

在进一步优选的实施方式中，将上述(1)所述的指北惯性导航系统姿态误差角方程、上述(2)所述的速度方程、上述(3)所述的位置方程以及上述(4)所述的惯性元件误差方程联立起来，取15阶状态变量：即可得到如式(31-1)所示的状态空间，其中，f(x)为对应的式(31-1-1)～式(31-1-9)所示的9个基本导航参数方程和式(31-1-10)与式(31-1-11)所示的惯性元件误差方程。

根据本发明一种优选的实施方式，在式(31-2)中：

量测信息z＝[v_ge,v_gn,v_gu,p_ge,p_gn,p_gu]^T，其中，v_ge表示GPS接收机测得的载体在东向的速度，v_gn表示GPS接收机测得的载体在北向的速度，v_gu表示GPS接收机测得的载体在天向的速度，p_ge表示GPS接收机测得的载体在东向的位置，p_gn表示GPS接收机测得的载体在北向的位置，p_gu表示GPS接收机测得的载体在天向的位置；

V为量测噪声，V＝[V_vge,V_vgn,V_vgu,V_pge,V_pgn,V_pgu]^T，其中，V_vge表示GPS接收机测得的载体在东向速度的随机误差，V_vgn表示GPS接收机测得的载体在北向速度的随机误差，V_vgu表示GPS接收机测得的载体在天向速度的随机误差，P_pge表示GPS接收机测得的载体在东向位置的随机误差，P_pgn表示GPS接收机测得的载体在北向位置的随机误差，P_pgu表示GPS接收机测得的载体在天向位置的随机误差。

在进一步优选的实施方式中，h(x)是线性方程，可以写成h(x)＝Hx，其中，H为量测矩阵，其中：

根据本发明一种优选的实施方式，所述SDRE分解模块32用于对空间模块31内如式(31-1)和式(31-2)所示的状态空间和量测空间进行状态相关分解，分别得到状态分解空间和量测分解空间。

在进一步优选的实施方式中，所述状态分解空间和量测分解空间分别如式(32-1)和式(32-2)所示:

z＝Hx+V式(32-2)。

其中，SDRE是用状态相关分解方法将非线性状态空间(式(31-1))分解成状态与转移矩阵相乘的形式，即f(x)＝F(x)x。其中，式(31-2)所示的量测空间中，h(x)是线性方程，因此不需要SDRE分解，因此，SDRE分解是对式(31-1)所述状态空间进行SDRE分解，得到式(32-1)。在本发明中，式(32-2)虽然不是经式(31-2)SDRE分解得到，但是，为了统一表述，将式(32-2)表述为量测分解空间。

在本发明中，状态转移矩阵F(x)为：

其中，F_N是9维矩阵，其中的非零元素为：

F(4,2)＝-f_u>n

F(5,1)＝f_u>e

F(6,1)＝-f_n>e

F(9,6)＝1；

F_s为：

根据本发明一种优选的实施方式，所述离散化处理模块33用于对SDRE分解模块32内的状态分解空间和量测分解空间进行离散化处理，得到状态离散空间和量测离散空间。

在进一步优选的实施方式中，所述状态离散空间和量测离散空间分别如式(33-1)和式(33-2)所示：

x_k+1＝Φ_k+1/kx_k+Γ_kw_k>

z_k+1＝Θ_k+1x_k+1+V_k+1式(33-2)。

其中，Φ_k+1/k是离散后的状态转移矩阵，Г_k是离散后的噪声转移矩阵，Θ_k+1是离散后的量测矩阵，其中，离散化后：

根据泰勒展开，Φ_k+1/k＝I+f(x_k)Δt+f(x_k)Δt²/2！+f(x_k)Δt²/3！+…，

Γ_k＝GΔt+GΔt²/2！+GΔt²/3！+…。

在更进一步优选的实施方式中，系统噪声向量w_k和量测噪声向量V_k的方差阵分别为Q_k和R_k，并且w_k和V_k满足：

E[w_k]＝0，表示w_k的均值为0；

其中，Cov[w_k,w_j]表示w_k和w_j的协方差，用于衡量w_k和w_j的总体误差；

E[V_k]＝0，表示V_k的均值为0；

Cov[V_k,V_j]表示V_k和V_j的协方差，用于衡量V_k和V_j的总体误差；

Cov[w_k,V_j]表示w_k和V_j的协方差，用于衡量w_k和V_j的总体误差。

其中，w_k表示在k时刻的系统噪声向量，w_j表示j时刻的系统噪声向量，V_k表示k时刻的量测噪声向量，V_j表示j时刻的量测噪声向量，δ_kj为Kronecker-δ函数，表示不同时刻噪声相互独立。

根据本发明一种优选的实施方式，所述强跟踪滤波模块34用于对离散化处理模块33内的如式(33-1)所示的状态离散空间和如式(33-2)所示的量测离散空间进行强跟踪滤波处理，得到组合导航信息。

在进一步优选的实施方式中，强跟踪滤波模块34包括一步预测子模块341、增益矩阵获得子模块342、实际状态估计子模块343和误差方差阵更新子模块344。

根据本发明一种优选的实施方式，所述一步预测子模块用于进行状态的一步预测和误差方差阵的一步预测。

其中，所述状态的一步预测是指根据当前时刻(k时刻)的状态信息对下一时刻(k+1时刻)的状态信息进行预测，所述误差方差阵的一步预测是指根据当前时刻的误差方阵对下一时刻的误差方差阵进行预测。

在进一步优选的实施方式中，所述状态的一步预测如式(341-1)所示：

其中，在式(341-1)中，表示根据k时刻的状态预测k+1时刻的状态预测值，x_k表示k时刻的状态，Δt表示滤波周期。

在更进一步优选的实施方式中，所述误差方差阵的一步预测如式(341-2)所示：

其中，在式(341-2)中，P_k+1/k表示根据k时刻的误差方差阵预测得到的k+1时刻的误差方差阵预测值，β_k为自适应渐消因子，P_k表示k时刻的误差方差阵，Φ_k+1/k表示离散后的状态转移矩阵，表示Φ_k+1/k的转置矩阵，Q_k表示w_k的方差阵，即系统噪声方差阵，k表示当前时刻，k+1表示下一时刻，其中，k＝0,1,2,3,…，L，其中，L由滤波时间和滤波周期决定，L＝滤波时间/滤波周期。

在本发明中，为了增强滤波器对模型不确定的鲁棒性，在进行误差方差阵的一步预测时，引入自适应渐消因子β_k，提高了一步预测状态的不确定度。其主要是基于新息残差正交化原理，即其中，新息是实际量测值与一步预测的量测值之差如果一步预测值准确，那么，各时刻的新息是正交的，即新息是均值为0的高斯白噪声，一旦不准确，则不再正交。

根据本发明一种优选的实施方式，所述自适应渐消因子通过下式(a)～式(e)进行调整，(可参见：周东华，席裕庚张钟俊.非线性系统带次优渐消因子的扩展卡尔曼滤波[J].控制与决策,1990(05):1-6.)：

其中，在式(a)中，表示新息序列，表示的转置矩阵，ρ表示遗忘因子、为人为设定、且ρ＝0～1，V_k-1表示k-1时刻的量测噪声；

其中，在式(b)中，N_k没有实际意义，其用来表示表示的，其中，Θ_k+1表示离散后的量测矩阵，表示Θ_k+1的转置矩阵，Q_k表示w_k的方差阵，R_k+1表示量测噪声向量V_k+1的方差阵；

其中，在式(c)中，M_k没有实际意义，其用来表示Θ_k+1表示离散后的量测矩阵，表示Θ_k+1的转置矩阵，Φ_k+1/k表示离散后的状态转移矩阵，表示Φ_k+1/k的转置矩阵，P_k表示k时刻的误差方差阵；

其中，在式(d)中，tr(N_k)表示N_k的迹，tr(M_k)表示M_k的迹，所述迹即为矩阵的迹，即矩阵对角线上各元素的和；

其中，通过上式进行调整得到的自适应渐消因子能够提高滤波的准确性，并且得到准确的状态值。具体地，获得自适应渐消因子β_k的最终目的是在一步预测状态值与实际状态值值相差较大的情况下，增大滤波增益矩阵，从而加大对量测量的信任，得到更精确的状态估计值。

根据本发明一种优选的实施方式，所述增益矩阵获得子模块342用于获得k+1时刻的增益矩阵，如式(342-1)所示：

其中，在式(342-1)中，P_k+1/k表示根据k时刻的误差方差阵预测得到的k+1时刻的误差方差阵，Θ_k+1是离散后的量测矩阵，为Θ_k+1的转置矩阵，R_k+1为量测噪声向量V_k+1的方差阵。

根据本发明一种优选的实施方式，所述实际状态估计子模块343用于估计k+1时刻的实际状态值，即用于获得k+1时刻的实际状态估计值，具体如式(343-1)所示：

其中，表示根据k时刻的状态预测的k+1时刻的预测状态值，K_k+1表示k+1时刻的增益矩阵，z_k+1为k+1时刻的量测数据(根据GPS给出的k+1时刻的信息获得)，表示一步预测状态值的量测值。

根据本发明一种优选的实施方式，所述误差方差阵更新子模块344用于对k+1时刻的误差方差阵进行更新，其中，利用k+1时刻的增益矩阵K_k+1和根据k时刻的误差方差阵预测得到的k+1时刻的误差方差阵P_k+1/k获得k+1时刻的误差方差阵P_k+1。

在进一步优选的实施方式中，k+1时刻的误差方差阵的更新如式(344-1)进行：

P_k+1＝(I-K_k+1Θ_k+1)P_k+1/k式(344-1)

其中，为了令滤波器不断地运行下去，直至系统过程结束，必须进行下一时刻(k+1时刻)的误差方差阵的更新。

根据本发明一种优选的实施方式，本发明所述滤波器还包括初始化模块340，用于对状态和误差方差阵进行初始化，同时进行时间更新。

在进一步优选的实施方式中，所述初始化模块340赋予状态初始值x₀，即惯导系统提供的初始值。

在更进一步优选的实施方式中，所述初始化模块340赋予初始误差方差阵P₀，其表示惯导系统的不确定性。其中，根据误差统计值，预估初始误差方差阵，并且，为了使滤波更快到达准确位置，可以将P₀设置为误差统计值的10倍，其中所述误差统计值为统计出来的传感器的误差值。

本发明另一方面还提供了一种基于强跟踪SDRE滤波的组合导航方法。

根据本发明一种优选的实施方式，如图2所示，所述导航方法包括以下步骤：

步骤1、利用SDRE分解模块32对空间模块31内的状态空间和量测空间进行状态相关分解，分别得到状态分解空间和量测分解空间。

根据本发明一种优选的实施方式，在步骤1中，所述状态空间和量测空间分别如式(31-1)和式(31-2)所示：

z＝h(x)+V式(31-2)。

根据本发明一种优选的实施方式，在步骤1中，SDRE分解模块32进行状态相关分解后得到的状态分解空间和量测分解空间分别如式(32-1)和式(32-2)所示：

z＝Hx+V式(32-2)。

其中，SDRE分解是用状态相关分解方法将非线性状态空间(式(31-1))分解成状态与转移矩阵相乘的线性系统形式，即f(x)＝F(x)x。其中，在式(31-2)所示的量测空间中，h(x)是线性方程，因此不需要SDRE分解，因此，SDRE分解是对式(31-1)所述状态空间进行SDRE分解，得到式(32-1)。在本发明中，式(32-2)虽然不是经式(31-2)SDRE分解得到，但是，为了统一表述，将式(32-2)表述为量测分解空间。

根据本发明一种优选的实施方式，在步骤1之前进行步骤1’：

步骤1’、利用初始化模块340对状态和误差方差阵进行初始化并进行时间更新。

在进一步优选的实施方式中，初始化模块340对状态进行初始化，得到状态初始值x₀和初始误差方差阵P₀，即分别为惯导系统提供的初始值与惯导系统的不确定性。

步骤2、利用离散化处理模块33对SDRE分解模块32内的状态分解空间和量测分解空间进行离散化处理，分别得到状态离散空间和量测离散空间。

根据本发明一种优选的实施方式，经离散化处理模块33处理得到的状态离散空间和量测离散空间分别如式(33-1)和式(33-2)所示：

x_k+1＝Φ_k+1/kx_k+Γ_kw_k式(33-1)；

z_k+1＝Θ_k+1x_k+1+V_k+1式(33-2)。

其中，Φ_k+1/k是离散后的状态转移矩阵，Г_k是离散后的噪声转移矩阵，Θ_k+1是离散后的量测矩阵。

在进一步优选的实施方式中，系统噪声向量w_k和量测噪声向量V_k的方差阵分别为Q_k和R_k，并且w_k和V_k满足：

E[w_k]＝0，E[V_k]＝0，

其中，w_k表示在k时刻的系统噪声向量，w_j表示j时刻的系统噪声向量，V_k表示k时刻的量测噪声向量，V_j表示j时刻的量测噪声向量，δ_kj为Kronecker-δ函数，表示不同时刻噪声相互独立。

步骤3、利用强跟踪滤波模块34对离散化处理模块33内的状态离散空间和量测离散空间进行强跟踪滤波处理，得到组合导航数据，并进行数据输出。

根据本发明一种优选的实施方式，在步骤3中，所述组合导航数据包括：φ_e(k+1)、φ_n(k+1)、φ_e(k+1)、Δ_e(k+1)、Δ_n(k+1)、Δ_u(k+1)、v_e(k+1)、v_n(k+1)、v_u(k+1)、λ(k+1)和h(k+1)。

在进一步优选的实施方式中，如图2所示，所述步骤3包括以下子步骤：

步骤3-1、利用一步预测子模块341对下一时刻(k+1时刻)的状态和误差方阵进行一步预测，获得下一时刻的状态预测值和误差方差阵预测值，分别以和表示。

步骤3-2、利用步骤3-1获得的P_k+1/k，经增益矩阵获得子模块342处理，得到下一时刻的增益矩阵，以K_k+1表示；

步骤3-3、利用步骤3-1获得的和步骤3-2获得的K_k+1，经实际状态估计子模块343获得下一时刻的实际状态值，以x_k+1表示，即组合导航信息，并进行信息输出。

根据本发明一种优选的实施方式，在步骤3-1中，所述状态的一步预测是指根据当前时刻(k时刻)的状态信息对下一时刻(k+1时刻)的状态信息进行预测，其如式(341-1)进行：

其中，在式(341-1)中，表示根据k时刻的状态预测的k+1时刻的状态预测值，x_k表示k时刻的状态，Δt表示滤波周期。

在进一步优选的实施方式中，所述误差方差阵的一步预测是指根据当前时刻的误差方阵对下一时刻的误差方差阵进行预测，其如式(341-2)进行：

其中，在式(341-2)中，P_k+1/k表示根据k时刻的误差方差阵预测得到的k+1时刻的误差方差阵预测值，β_k为自适应渐消因子，P_k表示k时刻的误差方差阵，Φ_k+1/k表示离散后的状态转移矩阵，表示Φ_k+1/k的转置矩阵，Q_k表示w_k的方差阵，k表示当前时刻，k+1表示下一时刻，其中，k＝0,1,2,3,…，L，其中，L由滤波时间和滤波周期决定，L＝滤波时间/滤波周期。

根据本发明一种优选的实施方式，在步骤3-2中，利用步骤3-1获得的P_k+1/k经增益矩阵获得子模块342如式(342-1)所示的处理，得到k+1时刻的增益矩阵：

其中，在式(342-1)中，P_k+1/k表示根据k时刻的误差方差阵预测得到的k+1时刻的误差方差阵，Θ_k+1是离散后的量测矩阵，为Θ_k+1的转置矩阵，R_k+1为量测噪声向量V_k+1的方差阵。

根据本发明一种优选的实施方式，在步骤3-3中，利用步骤3-1获得的以及步骤3-2获得的P_k+1/k在实际状态估计子模块343内进行如式(343-1)所示处理，得到k+1时刻的实际状态估计值x_k+1。

其中，表示根据k时刻的状态预测的k+1时刻的预测状态，K_k+1表示k+1时刻的增益矩阵，z_k+1为k+1时刻的量测数据(根据GPS给出的k+1时刻的信息获得)，表示一步预测状态值的量测值。

根据本发明一种优选的实施方式，为了令滤波器不断地运行下去，直至系统过程结束，在进行步骤3-3的同时必须进行下一时刻(k+1时刻)的误差方差阵的更新，即与步骤3-3同时进行步骤3-4：

步骤3-4、利用步骤3-1获得的P_k+1/k以及步骤3-2获得的K_k+1经误差方差阵更新子模块344进行如式(344-1)所示进行处理：

P_k+1＝(I-K_k+1Θ_k+1)P_k+1/k式(344-1)

其中，所述误差方差阵更新子模块344用于对k+1时刻的误差方差阵进行更新，其中，利用k+1时刻的增益矩阵K_k+1和根据k时刻的误差方差阵预测得到的k+1时刻的误差方差阵P_k+1/k获得k+1时刻的误差方差阵P_k+1。

本发明的有益效果：

(1)本发明所述系统结构简单，运行速度快；

(2)在本发明所述系统中引入自适应渐消因子，经过系统处理可以得到高精度的状态值；

(3)本发明所述方法采用SDRE分解，无需进行线性化处理，也不用求解Jacobian矩阵，减少了处理量，对最终工程应用具有实际意义；

(4)本发明所述方法提高了对系统不确定情况下的鲁棒性以及对飞行状态的突变情况下的跟踪能力，改善了导航精度，提高了导航系统的可靠性；

(5)本发明所述方法简单易行，并且结果准确。

实施例

在本实施例中，设定飞行器做如图3所示的机动运动，飞行轨迹中含有爬升、下降、转弯和变速运动。惯导系统(INS)的采样周期为0.01s，GPS接收机的采样周期及滤波周期为1s，仿真模拟时间为1932s。

根据上述所述对空间模块31内的状态空间和量测空间依次进行SDRE分解、离散化处理，然后进行强滤波跟踪，具体过程如下：

(1)在零时刻，通过惯导系统测量得到x₀，同时经过处理得到初始误差方差阵P₀。

其中，根据误差统计值，预估初始误差方差阵，并且，为了使滤波更快到达准确位置，可以将P₀设置为误差统计值的10倍，其中所述误差统计值为统计出来的传感器的误差值。

(2)利用一步预测子模块341对第一时刻的状态和误差方阵进行一步预测，分别获得第一时刻的状态预测值和误差方差阵预测值P_1/0。

其中：

其中：x₀由步骤(1)得到；Δt为设定值，在本实施例中设为0.01s；f(x₀)＝F(x₀)x₀，其中，x₀由步骤(1)得到，F(x₀)是将零时刻的惯导系统输出的参数值输入状态转移矩阵F(x)中得到；

其中，β₀为自适应渐消因子，Φ_1/0＝I+F(x₀)T，其中，在F(x₀)内输入零时刻的惯导信息，Q₀为人为设置。

(3)利用步骤(2)获得的P_1/0，经增益矩阵获得子模块342处理，得到第一时刻的增益矩阵K₁。

其中，其中，P_1/0经上述(2)获得，Θ₁＝H，R₁为人为设置。

(4)利用步骤(2)获得的和步骤(3)获得的K₁，经实际状态估计子模块343获得下一时刻的实际状态值x₁。

其中，其中，由步骤(2)获得，K₁由步骤(3)获得，z₁表示在第一时刻的GPS检测数据，

(5)利用步骤(3)获得的K₁、步骤(2)获得的P_1/0经误差方差阵更新子模块344处理，得到第一时刻的误差方差阵P₁。

其中，P₁＝(I-K₁Θ₁)P_1/0，其中，步骤(5)的目的是为了令滤波器不断地运行下去，并且，步骤(5)与步骤(4)同时进行。

将获得的x₁与P₁，以及惯性导航通过惯导解算后的第一时刻的信息输入滤波器，进行数据更新，重复步骤(2)～(5)的过程。

对比例

与实施例不同于：采用SDRE滤波进行处理，而不是强跟踪SDRE滤波，其在步骤3中的自适应渐消因子β为1。

实验例

分别采用实施例所述方法以及对比例所述方法对飞行器进行仿真，同时，假设现有空间模型是精确空间模型，在500s～800s内保持滤波模型不变，飞行数据中引入状态突变量Δx＝[0_1×3,1,1,1,10/Re,10/Re,10,0_1×6]^T，使这段时间的真实空间为：

x_k+1＝f(x_k)+Δx

对比例和实施例与真实空间在东、北、天的位置与真实位置之间的位置误差结果分别如图4a～图4c所示，在东、北、天的速度与真实速度之间的速度误差结果如图5a～图5c所示。

其中：在图4a～图4c中可以看出，在500s～800s的时间段内，实施例在这一时间段内滤波后的位置误差明显小于对比例，保持在5m之内，实施例的滤波结果更接近于真实状态；在图5a～图5c中可以看出，在出现误差的时间段内，实施例的误差也均小于对比例，小于1m/s，再次证明实施例的滤波结果更接近于真实状态；

因此，在平稳飞行时间内，采用本发明所述方法，速度误差小于1m/s，位置误差小于5m。

以上结合了优选的实施方式对本发明进行了说明，不过这些实施方式仅是范例性的，仅起到说明性的作用。在此基础上，可以对本发明进行多种替换和改进，这些均落入本发明的保护范围内。