CN111947681B - 一种gnss与惯导组合导航位置输出的滤波校正方法 - Google Patents

Abstract

本发明属于卫星导航定位技术领域，具体提供了一种GNSS与惯导组合导航位置输出的滤波校正方法，先建立GNSS/INS组合导航系统的误差状态方程；然后建立GNSS/INS组合导航系统的量测方程，系统采用松组合的方式，利用GNSS与INS提供的位置之差作为卡尔曼滤波的量测信息；最后采用卡尔曼滤波方法对系统状态进行估计，并根据滤波估计结果，对INS输出的位置进行校正，得到最终的卡尔曼滤波结果。采用了反馈校正，避免了输出校正方式下的滤波器状态值会越来越大的情况，提高了滤波效果；交替采用反馈校正以及输出校正对系统的位置进行校正，不但高了组合导航系统的可靠性，而且也提高了卡尔曼滤波估计的精度。

Description

一种GNSS与惯导组合导航位置输出的滤波校正方法

技术领域

本发明属于卫星导航定位技术领域，具体涉及一种GNSS与惯导组合导航位置输出的滤波校正方法。

背景技术

铁路作为我国国民经济的大动脉、国家重要基础设施、大众化交通工具，具有运能大、效率高、运距长、能耗低等诸多优点，非常适合我国人口众多、幅员辽阔的基本国情，在我国经济社会发展中发挥着重要的作用。列车运行控制系统作为铁路运输的“神经中枢”，是保障铁路行车安全、提高运输效率的核心，需要对列车的速度、位置等信息进行实时、准确的掌握。提高列车运行安全水平、降低运营和维护成本、满足线路运量现在已经成为了列车控制系统的一个研究热点。

随着GPS、GLONASS、Galileo和我国自主研发的北斗卫星导航系统的逐步发展和完善，卫星导航定位技术的应用领域也在不断扩展，利用卫星定位系统技术快速、准确地提供列车位置信息，不仅可以减少轨旁设备，降低建设和运行成本，减少铁路维修周期，还能提高列车定位精度，缩短列车行驶间隔时间，提高线路运输效率。对于基于差分定位技术的卫星定位系统，其输出位置信息的频率不高于CORS站播发差分信息的频率(如1HZ/秒)，即卫星定位系统输出位置的频率受CORS站播发信息的频率限制。然而，对于时速较高的列车(如达到600km/小时的磁悬浮列车)，其定位系统须具有更高的位置输出频率(至少100HZ)，才能满足列车实时定位的需求。此外，因为列车在实际运行环境中不仅会遇到隧道、桥梁、山坡、城市建筑物等遮挡卫星信号等问题，还会受到电磁波干扰、阻塞，出现卫星信号接受困难或丢失、错误等情况，降低列车定位的可靠性和精确性。因此，仅仅依靠GNSS(GlobalNavigation Satellite System，全球卫星导航系统)定位技术难以实现列车的连续定位。一些研究提出增加INS(Inertial Navigation System)传感器进行多源信息融合处理，弥补GNSS定位的局限性，扬长避短，形成有效列车组合定位功能整体。但是也存在缺陷。

GNSS/INS组合导航技术与卡尔曼滤波器构成了列车组合导航定位系统。卡尔曼滤波器对传感器输出的状态信息进行递推计算，系统利用计算结果对状态信息进行实时校正，最终输出列车的位置及其他信息。利用卡尔曼滤波器校正状态信息的方式有两种，分别为输出校正和反馈校正。输出校正就是用导航参数误差的估值去校正系统输出的导航参数，得到组合导航系统的导航参数估值，是目前列车组合导航系统主要采用的校正方式。然而，由于未校正系统导航参数的误差会随时间而增大，因而输出校正方式下的滤波器状态值会越来越大。这使得方程线性化等近似计算误差不断增大，从而滤波效果变差。反馈校正是将惯导系统导航参数误差的估值反馈到惯导系统内，对误差状态进行校正。与输出校正相比，反馈校正的卡尔曼滤波器所估计的状态是经过校正的导航参数误差，在反馈校正后，惯导系统的输出就是组合系统的输出，误差始终保持为小量，克服了输出校正的缺点，更真实地反映理论系统误差的动态过程。当惯导系统精度要求较高，且连续工作时间又长时，可采用反馈校正。然而该校正方式的缺点是工程实现没有输出校正简单，且滤波器故障直接污染惯导系统输出，使系统可靠性降低。

发明内容

本发明的目的是克服现有技术中列车卫星惯导组合导航定位技精度及可靠性低的问题。

为此，本发明提供了一种GNSS与惯导组合导航位置输出的滤波校正方法，包括一下步骤：

首先，由INS输出导航参数，GNSS系统输出列车位置，将INS与GNSS系统输出位置之差作为观测量，进行卡尔曼滤波估计，根据估计结果对INS输出的位置进行反馈校正；

其次，将反馈校正后的INS输出的位置与GNSS输出的位置再次求差，将结果作为观测值再次进行卡尔曼滤波估计，根据结果对反馈校正后的INS输出的位置再次进行输出校正；

最后获得的INS位置为系统位置的最终结果。

优选地，具体包括以下步骤：

S1：列车启动时，GNSS/INS组合导航系统开始工作，并向滤波系统输入初始的列车位置信息

系统估计协方差阵P₀、系统噪声方差阵Q₀量测信息的方差阵R₀以及惯导系统的导航参数；

S2：根据S1中的信息，构建卡尔曼滤波估计所需的矩阵，包括离散状态方程X_k＝A_k,k-1X_k-1+B_k,k-1W_k-1中的A_k,k-1＝I+F_k,k-1T_s，B_k,k-1＝[I+F_k,k-1T_s/2]×G_k,k-1×T_s，一步预测误差方差阵

滤波增益矩阵：

估计协方差阵P_k＝[I-K_kH_k]×P_k,k-1；

S3：利用t_k时刻INS与GNSS系统输出的位置

计算t_k时刻的量测值

利用S2中的滤波增益矩阵K_k，根据公式

计算卡尔曼滤波反馈校正的估计结果，根据估计结果对INS的位置进行校正：

获得经过反馈校正后的INS系统位置；

S4：根据GNSS位置以及校正后的INS位置获得量测值

根据公式

计算卡尔曼滤波输出校正的估计结果；

利用估计结果对反馈校正后的INS位置进行校正：

最终得到t_k时刻卡尔曼滤波估计校正后的系统位置。

优选地，所述导航参数包括列车的位置、速度、姿态角、姿态四元数以及列车在地理坐标系下的比力。

优选地，在步骤S1之前还包括：

建立GNSS/INS组合导航系统的误差状态方程

上式中X(t)的初始值为0，W(t)为系统噪声矩阵，G(t)为噪声驱动矩阵，F(t)为状态转移矩阵。

优选地，在步骤S1之前还包括：

建立GNSS/INS组合导航系统的量测方程，

Z(t)＝H(t)X(t)+V(t) (4)

上式中，量测值

分别为纬度、经度、高度之差，下标I和G分别表示INS以及GNSS系统；

V(t)为量测信号的噪声。

优选地，步骤S3具体包括：

交替采用卡尔曼滤波的输出校正以及反馈校正方式对系统校正，具体处理流程如下：

A：根据t_k-1历元经卡尔曼滤波输出校正后的系统位置

以及姿态四元数，更新误差状态方程

中的矩阵F_k,k-1与G_k,k-1；

将连续的误差状态方程公式(1)转化为离散状态方程：X_k＝A_k,k-1X_k-1+B_k,k-1W_k-1，其中，A_k,k-1＝I+F_k,k-1T_s，B_k,k-1＝[I+F_k,k-1T_s/2]×G_k,k-1×T_s，T_s为滤波的周期；

构建卡尔曼滤波过程必须的矩阵，包括：一步预测误差方差阵

其中Q_k-1为系统噪声的方差矩阵；P_k＝[I-K_kH_k]×P_k,k-1为估计协方差阵；滤波增益矩阵：

其中R_k为量测值的方差阵，根据t_k时刻INS与GNSS输出的系统位置之差

获得滤波估计结果

其中上标0表示由卡尔曼滤波校正之前INS和GNSS输出的状态，根据滤波估计结果，对INS输出的位置进行校正，

其中

分别为

的第7、8以及9个元素，即系统位置误差的估计结果；

B：利用A获得的A_k,k-1，B_k,k-1，P_k,k-1，P_k以及K_k矩阵。根据[L'_k λ'_k h'_k]_I ^T与t_k时刻GNSS输出的系统位置之差

获得滤波估计结果

其中

为前一历元卡尔曼滤波输出校正的估计结果。根据滤波估计结果，对INS输出的位置进行校正，得到最终的卡尔曼滤波结果：

优选地，状态转移矩阵为：

F(1,5)＝1/(R_M+h)，F(2,1)＝-(ω_iesinL+V_E/(R_N+h)×tgL)，F(2,3)＝-V_N/(R_M+h)，

F(2,4)＝1/(R_N+h)，F(3,1)＝ω_iecosL+V_E/(R_N+h)，F(2,7)＝-ω_iesinL，

F(3,2)＝V_N/(R_M+h)，F(3,7)＝ω_iecosL+V_E/(R_N+h)×sec²L，F(3,4)＝tgL/(R_N+h)，

F(4,4)＝(V_N/(R_M+h)×tgL-V_U/(R_M+h))，F(4,2)＝-f_U，F(4,3)＝f_U，

F(4,5)＝2ω_iesinL+V_E/(R_N+h)×tgL，F(4,6)＝-(2ω_iecosL+V_E/(R_N+h))，F(5,1)＝f_U，

F(5,3)＝-f_U，F(4,7)＝2ω_iecosLV_N-V_EV_N/sec²L+2ω_iesinLV_U，F(5,5)＝-V_U/(R_M+h)，

F(5,6)＝-V_N/(R_M+h)，F(5,4)＝-2(ω_iesinL+V_E/(R_N+h)×tgL)，F(6,1)＝-f_N，

F(6,2)＝-f_E，F(6,4)＝-2(ω_iecosL-V_E/(R_E+h))，F(6,5)＝-2V_N/(R_M+h)，

F(6,7)＝-2V_Eω_iesinL，F(7,5)＝1/(R_M+h)，F(8,4)＝secL/(R_N+L)，F(9,6)＝1，

其中ω_ie为地球自转角速度，[f_E f_N f_U]为东北天方向的比力，R_M＝R_e(1-2f+3fsin²L)，R_N＝R_e(1+fsin²L)，f＝1/298.257，

R_e＝6378137m，[V_E V_N V_U]为系统在东北天方向的速度，

其中

q＝[q(1)q(2)q(3)q(4)]为姿态四元数，F_M＝diag{000-1/Tg(1)-1/Tg(2)-1/Tg(3)-1/Ta(1)-1/Ta(2)-1/Ta(3)}，其中Tg＝300×ones(3,1)为陀螺仪误差漂移相关时间，Ta＝1000×ones(3,1)为加速度计误差漂移相关时间。

优选地，系统噪声W＝[ω_gx,ω_gy,ω_gz,ω_rx,ω_rx,ω_rx,ω_ax,ω_ax,ω_ax]^T，其中ω_g为陀螺误差漂移白噪声，ω_r为陀螺误差漂移马尔柯夫过程，ω_a为加速度计误差马尔柯夫过程。

优选地，所述惯导系统的坐标系为东北天坐标系，系统状态变量为

其中φ＝[φ_x φ_y φ_z]为平台失准角，δv＝[δv_x δv_y δv_z]为速度误差，δp＝[δL δλ δh]为位置误差；ε_b＝[ε_bx ε_by ε_bz]为陀螺漂移随机常数，ε_r＝[ε_rxε_ry ε_rz]为陀螺漂移一阶马尔柯夫过程，

为加速度计零位误差，下标x，y，z分别表示东、北、天三个轴向，L、λ和h分别表示载体的纬度、经度以及高度。

本发明的有益效果：本发明提供的这种GNSS与惯导组合导航位置输出的滤波校正方法，先建立GNSS/INS组合导航系统的误差状态方程；然后建立GNSS/INS组合导航系统的量测方程，系统采用松组合的方式，利用GNSS与INS提供的位置之差作为卡尔曼滤波的量测信息；最后采用卡尔曼滤波方法对系统状态进行估计，并根据滤波估计结果，对INS输出的位置进行校正，得到最终的卡尔曼滤波结果。采用了反馈校正，避免了输出校正方式下的滤波器状态值会越来越大的情况，提高了滤波效果；交替采用反馈校正以及输出校正对系统的位置进行校正，不但高了组合导航系统的可靠性，而且也提高了卡尔曼滤波估计的精度。

以下将结合附图对本发明做进一步详细说明。

附图说明

图1是GNSS与惯导组合导航位置输出的滤波校正方法的原理示意图；

图2是GNSS与惯导组合导航位置输出的滤波校正方法的具体处理流程图；

图3是GNSS与惯导组合导航位置输出的滤波校正方法的卡尔曼滤波估计校正前后INS系统输出的列车位置误差图；

图4是GNSS与惯导组合导航位置输出的滤波校正方法的卡尔曼滤波估计校正后INS系统输出的列车位置误差图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

在本发明的描述中，需要理解的是，术语“中心”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征；在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。

本发明实施例是针对列车卫星惯导组合导航定位技术中的滤波器单独采用输出校正或反馈校正存在的问题，本发明提出同时采用输出校正以及反馈校正技术对组合导航输出的位置进行校正。实现这两种技术的优势互补，提高组合导航定位系统的精度以及可靠性。

具体提供了一种GNSS与惯导组合导航位置输出的滤波校正方法，如图1所示，包括以下步骤：

首先，由INS输出导航参数，GNSS系统输出列车位置，将INS与GNSS系统输出位置之差作为观测量，进行卡尔曼滤波估计，根据估计结果对INS输出的位置进行反馈校正；

其次，将反馈校正后的INS输出的位置与GNSS输出的位置再次求差，将结果作为观测值再次进行卡尔曼滤波估计，根据结果对反馈校正后的INS输出的位置再次进行输出校正；

最后获得的INS位置为系统位置的最终结果。

其主要解决的关键技术问题有二：第一，两种校正方式的混合使用方式。第二，利用一种校正方式获得的位置信息如何被另一种校正方式利用，提高位置的精度。

本实施例是以某路段列车INS与GNSS组合系统的采集的一个半小时的数据为例，采用混合校正实时获得列车的运行中的位置。图2为该方法的流程图，具体步骤如下：

S1：列车启动时，GNSS/INS组合导航系统开始工作，并向滤波系统输入初始的列车位置信息

系统估计协方差阵P₀、系统噪声方差阵Q₀量测信息的方差阵R₀以及惯导系统的导航参数(包括列车位置，速度，姿态角，姿态四元数，列车地理坐标系下的比力)。

S2：根据S1中的信息，构建卡尔曼滤波估计所需的矩阵，如离散状态方程X_k＝A_{k,k- 1}X_k-1+B_k,k-1W_k-1中的A_k,k-1＝I+F_k,k-1T_s，B_k,k-1＝[I+F_k,k-1T_s/2]×G_k,k-1×T_s，一步预测误差方差阵

滤波增益矩阵：

估计协方差阵P_k＝[I-K_kH_k]×P_k,k-1；

S3：利用t_k时刻INS与GNSS系统输出的位置

计算t_k时刻的量测值

利用S2中的滤波增益矩阵K_k，根据公式

计算卡尔曼滤波反馈校正的估计结果，根据估计结果对INS的位置进行校正：

获得经过反馈校正后的INS系统位置；

S4：根据GNSS位置以及校正后的INS位置获得量测值

根据公式

计算卡尔曼滤波输出校正的估计结果；

利用估计结果对反馈校正后的INS位置进行校正：

最终得到t_k时刻卡尔曼滤波估计校正后的系统位置。

由于GNSS位置误差较小，以GNSS系统输出的位置为真值，比较滤波校正前后INS的位置误差变化，具体结果如图3、图4所示。图3为INS卡尔曼滤波估计校正前(虚)后(实)位置误差。图4为INS卡尔曼滤波估计校正后位置误差。可以看出，滤波之前，INS输出的位置误差随时间的增加而变大，采用本发明的方法后，三方向位置误差变化平稳，水平方向误差处于10厘米以内，垂直方向的误差处于20厘米以内。这说明采用本方法有效提高了滤波的精度以及系统的可靠性。

本发明的有益效果：本发明提供的这种GNSS与惯导组合导航位置输出的滤波校正方法，先建立GNSS/INS组合导航系统的误差状态方程；然后建立GNSS/INS组合导航系统的量测方程，系统采用松组合的方式，利用GNSS与INS提供的位置之差作为卡尔曼滤波的量测信息；最后采用卡尔曼滤波方法对系统状态进行估计，并根据滤波估计结果，对INS输出的位置进行校正，得到最终的卡尔曼滤波结果。采用了反馈校正，避免了输出校正方式下的滤波器状态值会越来越大的情况，提高了滤波效果；交替采用反馈校正以及输出校正对系统的位置进行校正，不但高了组合导航系统的可靠性，而且也提高了卡尔曼滤波估计的精度。

以上例举仅仅是对本发明的举例说明，并不构成对本发明的保护范围的限制，凡是与本发明相同或相似的设计均属于本发明的保护范围之内。

Claims (6)

1.一种GNSS与惯导组合导航位置输出的滤波校正方法，其特征在于，包括以下步骤：

首先，由INS输出导航参数，GNSS系统输出列车位置，将INS与GNSS系统输出位置之差作为观测量，进行卡尔曼滤波估计，根据估计结果对INS输出的位置进行反馈校正；

其次，将反馈校正后的INS输出的位置与GNSS输出的位置再次求差，将结果作为观测值再次进行卡尔曼滤波估计，根据结果对反馈校正后的INS输出的位置再次进行输出校正；

最后获得的INS位置为系统位置的最终结果；

具体包括以下步骤：

S0：建立GNSS/INS组合导航系统的误差状态方程

上式中X(t)的初始值为0，W(t)为系统噪声矩阵，G(t)为噪声驱动矩阵，F(t)为状态转移矩阵；

S1：列车启动时，GNSS/INS组合导航系统开始工作，并向滤波系统输入初始的列车位置信息

、 系统估计协方差阵P₀、系统噪声方差阵Q₀、 量测信息的方差阵R₀以及惯导系统的导航参数；

S2：根据S1中的信息，构建卡尔曼滤波估计所需的矩阵，包括离散状态方程X_k＝A_{k,k- 1}X_k-1+B_k,k-1W_k-1中的A_k,k-1＝I+F_k,k-1T_s，

一步预测误差方差阵

滤波增益矩阵：

估计协方差阵P_k＝[I-K_kH_k]×P_k,k-1；

S3：利用t_k时刻INS与GNSS系统输出的位置

计算t_k时刻的量测值

利用S2中的滤波增益矩阵K_k，根据公式

计算卡尔曼滤波反馈校正的估计结果，根据估计结果对INS的位置进行校正：

获得经过反馈校正后的INS系统位置；

S4：根据GNSS位置以及校正后的INS位置获得量测值

根据公式

计算卡尔曼滤波输出校正的估计结果；

利用估计结果对反馈校正后的INS位置进行校正：

最终得到t_k时刻卡尔曼滤波估计校正后的系统位置；

其中，状态转移矩阵为：

F(1,5)＝1/(R_M+h)，F(2,1)＝-(ω_iesinL+V_E/(R_N+h)×tgL)，F(2,3)＝-V_N/(R_M+h)，F(2,4)＝1/(R_N+h)，F(3,1)＝ω_iecosL+V_E/(R_N+h)，F(2,7)＝-ω_iesinL，F(3,2)＝V_N/(R_M+h)，F(3,7)＝ω_iecosL+V_E/(R_N+h)×sec²L，F(3,4)＝tgL/(R_N+h)，F(4,4)＝(V_N/(R_M+h)×tgL-V_U/(R_M+h))，F(4,2)＝-f_U，F(4,3)＝f_U，F(4,5)＝2ω_iesinL+V_E/(R_N+h)×tgL，F(4,6)＝-(2ω_iecosL+V_E/(R_N+h))，F(5,1)＝f_U，F(5,3)＝-f_U，F(4,7)＝2ω_iecosLV_N-V_EV_N/sec²L+2ω_iesinLV_U，F(5,5)＝-V_U/(R_M+h)，F(5,6)＝-V_N/(R_M+h)，F(5,4)＝-2(ω_iesinL+V_E/(R_N+h)×tgL)，F(6,1)＝-f_N，F(6,2)＝-f_E，F(6,4)＝-2(ω_iecosL-V_E/(R_E+h))，F(6,5)＝-2V_N/(R_M+h)，F(6,7)＝-2V_Eω_iesinL，F(7,5)＝1/(R_M+h)，F(8,4)＝secL/(R_N+L)，F(9,6)＝1，

其中ω_ie为地球自转角速度，[f_E f_N f_U]为东北天方向的比力，R_M＝R_e(1-2f+3f sin²L)，R_N＝R_e(1+f sin²L)，f＝1/298.257，R_e＝6378137m，[V_E V_N V_U]为系统在东北天方向的速度，其中

q＝[q(1)q(2) q(3) q(4)]为姿态四元数，F_M＝diag{0 0 0 -1/Tg(1) -1/Tg(2) -1/Tg(3) -1/Ta(1) -1/Ta(2) -1/Ta(3)}，其中Tg＝300×ones(3,1)为陀螺仪误差漂移相关时间，Ta＝1000×ones(3,1)为加速度计误差漂移相关时间。

2.根据权利要求1所述的GNSS与惯导组合导航位置输出的滤波校正方法，其特征在于：所述导航参数包括列车的位置、速度、姿态角、姿态四元数以及列车在地理坐标系下的比力。

3.根据权利要求1所述的GNSS与惯导组合导航位置输出的滤波校正方法，其特征在于，在步骤S1之前还包括：

建立GNSS/INS组合导航系统的量测方程，

Z(t)＝H(t)X(t)+V(t) (4)

上式中，量测值

分别为纬度、经度、高度之差，下标I和G分别表示INS以及GNSS系统；

V(t)为量测信号的噪声。

4.根据权利要求1所述的GNSS与惯导组合导航位置输出的滤波校正方法，其特征在于，步骤S3具体包括：

交替采用卡尔曼滤波的输出校正以及反馈校正方式对系统校正，具体处理流程如下：

A：根据t_k-1历元经卡尔曼滤波输出校正后的系统位置[L_k-1 λ_k-1 h_k-1]^T以及姿态四元数，更新误差状态方程

中的矩阵F_k,k-1与G_k,k-1；

将连续的误差状态方程公式(1)转化为离散状态方程：X_k＝A_k,k-1X_k-1+B_k,k-1W_k-1，其中，A_k,k-1＝I+F_k,k-1T_s，B_k,k-1＝[I+F_k,k-1T_s/2]×G_k,k-1×T_s，T_s为滤波的周期；

构建卡尔曼滤波过程必须的矩阵，包括：一步预测误差方差阵

其中Q_k-1为系统噪声的方差矩阵；P_k＝[I-K_kH_k]×P_k,k-1为估计协方差阵；滤波增益矩阵：

其中R_k为量测值的方差阵，根据t_k时刻INS与GNSS输出的系统位置之差

获得滤波估计结果

其中上标0表示由卡尔曼滤波校正之前INS和GNSS输出的状态，根据滤波估计结果，对INS输出的位置进行校正，

其中

分别为

的第7、8以及9个元素，即系统位置误差的估计结果；

B：利用A获得的A_k,k-1，B_k,k-1，P_k,k-1，P_k以及K_k矩阵， 根据[L'_k λ'_k h'_k]_I ^T与t_k时刻GNSS输出的系统位置之差

获得滤波估计结果

其中

为前一历元卡尔曼滤波输出校正的估计结果， 根据滤波估计结果，对INS输出的位置进行校正，得到最终的卡尔曼滤波结果：

5.根据权利要求1所述的GNSS与惯导组合导航位置输出的滤波校正方法，其特征在于：系统噪声W＝[ω_gx,ω_gy,ω_gz,ω_rx,ω_rx,ω_rx,ω_ax,ω_ax,ω_ax]^T，其中ω_g为陀螺误差漂移白噪声，ω_r为陀螺误差漂移马尔柯夫过程，ω_a为加速度计误差马尔柯夫过程。

6.根据权利要求1所述的GNSS与惯导组合导航位置输出的滤波校正方法，其特征在于：所述惯导系统的坐标系为东北天坐标系，系统状态变量为

其中φ＝[φ_x φ_y φ_z]为平台失准角，δv＝[δv_x δv_y δv_z]为速度误差，δp＝[δL δλ δh]为位置误差；ε_b＝[ε_bx ε_by ε_bz]为陀螺漂移随机常数，ε_r＝[ε_rx ε_ry ε_rz]为陀螺漂移一阶马尔柯夫过程，

为加速度计零位误差，下标x，y，z分别表示东、北、天三个轴向，L、λ和h分别表示载体的纬度、经度以及高度。

Images