一体化无陀螺捷联惯导系统与GPS系统组合导航方法

摘要

本发明公开了一种一体化无陀螺捷联惯导系统与GPS系统组合导航方法，属于组合导航技术领域。本发明的主要特征为：首先选取六维加速度传感器作为惯性导航的惯性元件，并进行捷联解算，使其构成一体化无陀螺捷联惯导系统；其次是将该捷联惯导系统作为主要子系统，实现与GPS系统相结合的组合导航方法，选取组合导航系统的状态量，建立组合导航系统的状态方程和量测方程，采用卡尔曼滤波器完成信息融合，得到系统的最优估计，以此校正惯导系统输出的导航参数值，最终得到载体导航参数的最优解。本发明基于一体化无陀螺捷联惯导系统与GPS系统的组合导航方法，能提高单个导航系统的精度，有效提升导航系统的综合性能。

说明书

技术领域

本发明涉及组合导航技术领域，尤其涉及一种一体化无陀螺捷联惯导系统(gyro free  strapdown inertial navigation system,GFSINS)与GPS系统的组合导航方法。

背景技术

随着飞机、导弹、潜艇及机器人技术的现代化发展，军事和民用领域对导航新技术的需 求越来越迫切，对导航精度的要求越来越高，对导航系统其他各项性能，如自主性、稳定性、 抗干扰能力等的要求也越来越全面。以现有导航技术来看，单一类型的导航系统几乎不可能 同时满足以上要求，因而近年来组合导航技术得到了飞速的发展。组合导航是指两种或两种 以上导航技术的结合，组合后的系统称为组合导航系统。目前应用于组合导航系统的导航类 型主要有惯性导航、卫星导航、天文导航等。根据不同的导航需求，会将上述的单类型导航 按各自的优缺点进行不同方式的组合。

惯性导航系统(inertial navigation system,INS)利用自身的惯性敏感元件测量航行 体相对惯性空间的运动参数，在给定的运动初始条件下，由计算机解算出航行体的位置、姿 态、速度等参数，从而引导航行体完成预定的航行任务。惯性导航最主要的惯性敏感元件是 加速度计和陀螺仪，利用这两种惯性元件与其它控制元件组成测量系统完成导航参数的测量。 它不依赖于任何外界信息，不受天然或人为的干扰，具有很好的隐蔽性，是一种完全自主式 的导航系统。无陀螺捷联惯导系统由于摒弃了结构复杂，维护困难的陀螺，依靠加速度计的 空间位置组合即可代替陀螺测量载体的角运动参数，因而在承载惯性导航固有优势的基础上， 更具有结构简单、成本低廉、使用方便，可靠性高等特征，继而成为惯性导航研究的新热点。 随着科学技术的进步和工艺水平的提高，捷联惯导系统的各项性能已有了大幅提升，但其固 有的结构特点使其在实际的工程应用过程中也遇到一些新问题。例如，由于惯性敏感器直接 与运载体固连，运载体的角运动将直接传递给惯性敏感器而引起动态误差；需进行大量的坐 标变换计算及动态误差补偿计算；对信号处理系统的容量、速度和精度的要求较高等；但其 最重要的缺点是误差随时间累积，这是惯导的通病，也是惯导系统目前只能应用于短程导航 的关键原因，因此需要外部辅助定位的方法对其进行修正。

现行的外部辅助导航技术中，卫星导航系统，尤其是全球定位系统成为最理想的辅助定 位方法。全球定位系统(inerglobal position system,GPS)是美国国防部研制的第二代卫 星导航系统，它以空间卫星为基础，能为海、陆、空各种载体提供全天候、高精度的三维位 置、速度信息，并且还可以在载体上安装多个天线来测量载体的姿态信息。与惯性导航相比， 它的优点是技术趋于成熟，精度高且成本低，缺点是自主性差和抗干扰能力弱。这与惯导系 统在性能上正好形成优势互补，因而近年来，基于惯性导航和GPS组成的组合导航系统成为 了国内外组合导航研究的主流。

组合后的导航系统不仅可以同时发挥两个导航子系统的优势，在定位精度、性能、和可 靠性等方面都要优于单独的导航子系统。由于惯导系统能够提供比较多的导航参数和全姿态 信息参数，且不受外界干扰、隐蔽性和连续性好，因此一般在以惯性导航和GPS结合的组合 导航系统中，惯导系统多作为主要子系统，高精度的GPS导航信息则作为外部输入，在运动 过程中不断修正惯导系统。在组合导航系统的信息融合算法上，目前使用最为广泛的是卡尔 曼滤波算法。两个子系统将各自的导航参数信息输入到滤波器中，系统选取合适的状态变量， 建立组合系统的状态空间模型，推导滤波迭代方程，通过前一时刻的估计值和新时刻的观测 值来对状态变量进行估计，从而得到最优解，来校正单个子系统的参数误差。

发明内容

本发明所要解决的技术问题是针对现有基于惯导系统与GPS系统的组合导航系统存在计 算复杂、解算效率低的不足，提出一种基于一体化无陀螺捷联惯导系统和GPS系统的组合导 航方法，其结构简单、计算效率高、系统稳定性好。

本发明为解决上述技术问题采用以下技术方案：

一体化无陀螺捷联惯导系统与GPS系统组合导航方法，包含以下步骤：

步骤1)，将六维加速度传感器引入捷联惯导系统中充当惯性敏感元件，根据六维加速度 传感器输出的六维加速度进行相应的捷联解算，获得载体的导航位置、速度和姿态参数；

步骤2)，采用GPS系统对载体的运动进行跟踪，获取载体的位置、速度和姿态导航信息；

步骤3)，将步骤1)和步骤2)分别获取的导航参数值对应相减后得到的差值输入组合 导航滤波器；

步骤4)，组合导航滤波器对输入的差值进行相应的滤波处理，得到捷联惯导系统导航参 数的最优误差；

步骤5)，将步骤1)得到的各导航参数值与步骤4)得到的最优误差对应相减，得到载 体此时的最优位置、速度和姿态，并输出。

作为本发明一体化无陀螺捷联惯导系统与GPS系统组合导航方法进一步的优化方案，步 骤1)中所述根据六维加速度传感器输出的六维加速度进行相应的捷联解算的详细步骤如下：

步骤1.1)，获取六维加速度传感器输出的以下参数：外壳{S}相对于相对惯性系{O}的线 加速度^Oa_S、角加速度^Oα_S、角速度^Oω_S、^Oω_S在外壳系{S}中的投影^S(^Oω_S)以及姿态矩阵

步骤1.2)，根据以下公式将步骤1.1)中得到的参数转换为以载体系{B}为运动主体的参 量：

${}^{I}a_N={}^{I}a_B={}_O{}^{I}R({}^{O}a_S-{}^O\alpha _S\times {}_S{}^{O}R{}^{B}r_S-{}^O\omega _S\times ({}^O\omega _S\times {}_S{}^{O}R^{B}r_S))$

^B(^Iω_B)＝^B(^Oω_B)＝^S(^Oω_S)

其中，^Ia_N，^Ia_B分别表示导航系{N}和载体系{B}相对于绝对惯性系{I}的加速度，为 绝对惯性系{I}和相对惯性系{O}之间的方位矩阵，^Br_S为传感器的安装位置矢量，^B(^Iω_B)表 示载体系{B}相对于绝对惯性系{I}的角速度在载体系{B}中的投影，^B(^Oω_B)表示载体系{B}相 对于相对惯性系{O}的角速度在载体系{B}中的投影；

步骤1.3)，根据以下公式推导捷联惯导系统的惯导基本方程，并根据步骤1.2)中得到 的各项参数值求解载体的导航加速度：

${}_I{}^{N}R={}_E{}^{N}R_I^{E}R$

${}^N\stackrel{·}{V}={}_I{}^{N}R{}^{I}a_B-(2{}_I{}^{N}R{}^I\omega _E+{}_E{}^{N}R{}^E\omega _N)\times {}^{N}V-{}_I{}^{N}R{}^I\omega _E\times ({}_I{}^{N}R{}^I\omega _E\times {}_E{}^{N}R{}^{E}L_N)$

其中，为导航系{N}相对于绝对惯性系{I}的方位矩阵，为导航的位置矩阵，为 地球的自转矩阵，^NV为导航速度，^Iω_E为地球的自转角速度，^Eω_N为载体的位置角速率，^EL_N为地球系{E}到导航系{N}的位置矢量；

步骤1.4)，利用步骤1.3)中得到的载体导航加速度，通过数值积分运算分别求解出载 体的导航速度和位置；

步骤1.5)，根据以下导航的姿态方程求解出姿态矩阵的各元素值：

${}_B{}^{N}R={}_E{}^{N}R_I^E{R}_O^I{R}_S^{O}R$

其中，为载体的姿态矩阵；

步骤1.6)，求解姿态矩阵表达式，并将姿态矩阵表达式和步骤1.5)中的元素值相互对 应，解算出载体的导航姿态角。

作为本发明一体化无陀螺捷联惯导系统与GPS系统组合导航方法进一步的优化方案，步 骤4)中所述组合导航滤波器对输入的差值进行相应的滤波处理的详细步骤如下：

步骤2.1)，建立捷联惯导系统的误差模型，推导导航位置、速度和姿态的误差微分方程， 其中，位置误差方程为：

$\left(\begin{array}{c}\delta \stackrel{·}{\lambda }=\frac{\sec \phi }{R+h}{\mathrm{\delta V}}_E+\frac{V_E\tan \phi \sec \phi }{R+h}\mathrm{\delta \phi }-\frac{V_E\sec \phi }{{(R+h)}^2}\mathrm{\delta h}\\ \delta \stackrel{·}{\phi }=\frac{1}{R+h}{\mathrm{\delta V}}_N-\frac{V_N}{{(R+h)}^2}\mathrm{\delta h}\\ \delta \stackrel{·}{h}={\mathrm{\delta V}}_U\end{array}\right)$

式中，λ，φ，h为载体所处地球表面的经度、纬度、高度，V_E，V_N，V_U为载体沿东、 北、天向的速度，R为地球半径；

速度误差方程为：

${\delta }^N\stackrel{·}{V}={}_I{}^{N}R{\delta }^I{a}_B+{}_I{}^{N}R\beta \times {}^{I}a_B-(2_I^N{R}^I{\omega }_E+{}_E{}^{N}R^E{\omega }_N)\times {\delta }^{N}V-({2\delta }^N\left({}^I\omega _E\right)+{\delta }^N\left({}^E\omega _N\right))\times {}^{N}V$

式中，β为实际导航系{N}偏离理想导航系(地理坐标系{G})的误差角矢量；

姿态误差方程为：

$\stackrel{·}{ϵ}=-{}_B{}^{N}R{\delta }^B\left({}^I\omega _B\right)+{\delta }^N\left({}^I\omega _N\right)+ϵ\times {}^N\left({}^I\omega _N\right)$

式中，ε为姿态误差角矢量，^N(^Iω_N)为导航系{N}相对于绝对惯性系{I}的角速率在导航 系{N}中的投影；

步骤2.2)，选取捷联惯导系统与GPS系统组合导航系统的状态变量X为9维、状态噪声 变量W为6维、量测变量Z为9维、量测噪声变量V为9维，具体参量如下：

W＝[δ^Ia_Bx δ^Ia_By δ^Ia_Bz δ^S(^Oω_Sx) δ^S(^Oω_Sy) δ^S(^Oω_Sz)]^T

其中，θ，γ，分别为载体的俯仰角、横滚角和航向角，N_E，N_N，N_U分别为GPS测 量得到的载体的位置沿东、北、天方向的距离误差；

步骤2.3)，建立捷联惯导系统与GPS系统组成的组合导航系统的状态空间模型和量测空 间模型，根据步骤2.1)中的误差方程和步骤2.2)中的状态变量，分别求解模型中的状态矩 阵、系统噪声矩阵、量测矩阵和量测噪声矩阵，得到组合导航系统的状态方程和量测方程；

步骤2.4)，对步骤2.3)中的状态方程和量测方程进行离散化处理，得到组合导航系统 的时间更新方程和量测更新方程；

步骤2.5)，将输入的差值代入到步骤2.4)的更新方程中，迭代求解出组合导航系统的 最优估计，即捷联惯导系统导航参数的最优误差，并输出。

作为本发明一体化无陀螺捷联惯导系统与GPS系统组合导航方法进一步的优化方案，步 骤4)中所述组合导航滤波器采用卡尔曼滤波器。

作为本发明一体化无陀螺捷联惯导系统与GPS系统组合导航方法进一步的优化方案，步 骤1)中所述六维加速度传感器采用8-UPS型并联式六维加速度传感器。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

一体化无陀螺捷联惯导系统与现有的捷联惯导系统相比，兼具结构简单，成本低，解算 效率和精度高，物理模型误差敏感性低等优势。特别地，由于其惯性敏感元件——并联式六 维加速度传感器独特的解耦算法，一体化无陀螺捷联惯导系统较之目前的无陀螺捷联惯导系 统而言，对载体角运动信息的测量精度明显提高；较之目前的有陀螺式惯导系统而言，其成 本低，维护方便，系统可靠性高，因而在导航系统整体性能的兼顾性方面，具有突出的优势。

具备以上优势的一体化无陀螺捷联惯导系统与GPS系统结合后形成的组合导航系统，与 现有的INS/GPS组合导航系统相比，在计算效率和解算精度方面都有了较大提升；此外，由 于组合导航系统的状态量只有9维，状态噪声量只有6维，组合导航的计算量也得以进一步 减少，因而在保证系统实时性和导航精度最优性的基础上，可以尽可能地提高GPS的更新频 率和组合导航系统的滤波频率，从而进一步提高组合导航系统的精度。

附图说明

图1是本发明的一体化无陀螺捷联惯导系统捷联解算流程图；

图2是本发明的一体化无陀螺捷联惯导系统与GPS系统的组合导航系统结构示意图；

图3是本发明的一体化无陀螺捷联惯导系统与GPS系统的组合导航方法流程图。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

组合导航系统要求每个导航子系统都具备完整良好的性能，本发明是基于惯导系统和GPS 系统的组合导航系统，其中，惯性导航是由文献《Influences analysis of configurations  on the performance of parallel type six-axis accelerometers》中的8-UPS型并联式六 维加速度传感器演变而来，GPS为通用的卫星信号接收机，在对惯导和GPS组合导航系统进 行信息融合处理时，选择卡尔曼滤波器进行。

一体化无陀螺捷联惯导系统与GPS系统组合导航方法，主要包含以下步骤：

步骤1)，如图1所示，将六维加速度传感器引入捷联惯导系统中充当惯性敏感元件，根 据六维加速度传感器输出的六维加速度进行相应的捷联解算，获得载体的导航位置、速度和 姿态参数；

步骤2)，采用GPS系统对载体的运动进行跟踪，获取载体的位置、速度和姿态导航信息；

步骤3)，如图2所示，将步骤1)和步骤2)分别获取的导航参数值对应相减后得到的 差值输入组合导航滤波器；

步骤4)，组合导航滤波器对输入的差值进行相应的滤波处理，得到捷联惯导系统导航参 数的最优误差；

步骤5)，将步骤1)得到的各导航参数值与步骤4)得到的最优误差对应相减，得到载 体此时的最优位置、速度和姿态，并输出。

步骤6)，如图3所示，当GPS处于未更新时段内，组合导航系统参考步骤1)进行；惯 导系统利用传感器测量得到的六维加速度，通过捷联解算得到载体的位置、速度、姿态等导 航信息，并直接作为此刻组合导航系统的参数输出值。当GPS处于更新时段内，组合导航系 统参考步骤1)-步骤5)重复进行，利用GPS的测量值对惯导的输出参数进行辅助修正，直 到滤波结束。

作为本发明一体化无陀螺捷联惯导系统与GPS系统组合导航方法进一步的优化方案，步 骤1)中所述根据六维加速度传感器输出的六维加速度进行相应的捷联解算的详细步骤如下：

步骤1.1)，选择8-UPS型并联式六维加速度传感器作为惯性导航的惯性敏感元件，参考 文献《Influences analysis of configurations on the performance of parallel type  six-axis accelerometers》中的8-UPS型并联式六维加速度传感器的解耦算法，结合传感器 的解耦原理和惯性导航的解算原理，建立如下常用坐标系：

绝对惯性系{I}：恒定不变坐标系，原点o在地球质心，ox_I指向赤道和本初子午线的交 点，oz_I与地球自转轴重合，oy_I与ox_I、oz_I构成右手系。地球坐标系{E}：为载体航行提供地 球参考，位于地球质心，初始时刻与绝对惯性系重合，后随地球做同步转动。地理坐标系{G}： 跟踪载体在地球表面的位置，原点o在载体质心，ox_G在当地水平面内指向东，oy_G沿当地子 午线指向北，oz_G沿当地垂线指向天。载体坐标系{B}：与载体固联，用来跟踪载体的姿态， 原点o在载体质心，ox_B沿载体横轴向右，oy_B沿载体纵轴向前，oz_B沿载体立轴向上。导航 坐标系{N}：导航基准坐标系，本发明设导航系与地理系重合。外壳坐标系{S}和相对惯性系 {O}：为传感器的测量提供参考基准，初始时刻，两者重合，原点位于传感器质量块的质心， 三轴分别平行于质量块的三条正交棱边；运动开始后，相对惯性系{O}不变，外壳坐标系{S} 跟随传感器外壳。

已知8-UPS型并联式六维加速度传感器输出以下参数：外壳{S}相对于相对惯性系{O}的 线加速度^Oa_S，角加速度^Oα_S，角速度^Oω_S，^Oω_S在外壳系{S}中的投影^S(^Oω_S)以及姿态矩阵

步骤1.2)，将步骤1.1)中得到的以外壳系{S}为运动主体的参量转换为以载体系{B}为 运动主体的参量，已知导航系{N}和载体系{B}的质心相互重合，外壳系{S}和载体系{B}始终 相互平行，有

${}^{I}a_N={}^{I}a_B={}_O{}^{I}R({}^{O}a_S-{}^O\alpha _S\times {}_S{}^{O}R{}^{B}r_S-{}^O\omega _S\times ({}^O\omega _S\times {}_S{}^{O}R^{B}r_S))$

^B(^Iω_B)＝^B(^Oω_B)＝^S(^Oω_S)

式中，^Ia_N，^Ia_B分别表示导航系{N}和载体系{B}相对于绝对惯性系{I}的加速度，为 绝对惯性系{I}和相对惯性系{O}之间的方位矩阵，^Br_S为传感器的安装位置矢量，^B(^Iω_B)表 示载体系{B}相对于绝对惯性系{I}的角速度在载体系{B}中的投影，^B(^Oω_B)表示载体系{B}相 对于相对惯性系{O}的角速度在载体系{B}中的投影。

步骤1.3)，推导该捷联惯导系统的惯导基本方程，根据步骤1.2)中得到的各项参数值 求解载体的导航加速度。

${}^N\stackrel{·}{V}={}_I{}^{N}R{}^{I}a_B-(2{}_I{}^{N}R{}^I\omega _E+{}_E{}^{N}R{}^E\omega _N)\times {}^{N}V-{}_I{}^{N}R{}^I\omega _E\times ({}_I{}^{N}R{}^I\omega _E\times {}_E{}^{N}R{}^{E}L_N)$

${}_I{}^{N}R={}_E{}^{N}R_I^{E}R$

式中，为导航系{N}相对于绝对惯性系{I}的方位矩阵，为导航的位置矩阵，为 地球的自转矩阵，^NV为导航速度，^Iω_E为地球的自转角速度，^Eω_N为载体的位置角速率，^EL_N为地球系{E}到导航系{N}的位置矢量。

步骤1.4)，由步骤1.3)得到载体的导航加速度，并根据数值积分运算，分别对导航加 速度进行一次积分和二次积分得到载体的导航速度和位置。

${}^{N}V={}^{N}V_0+{\int }_0^t{}^N\stackrel{·}{V}\mathrm{dt}$

$P=P_0+{\int }_0^t{}^{N}V\mathrm{dt}$

步骤1.5)，根据空间几何理论，导航的姿态方程为

${}_B{}^{N}R={}_E{}^{N}R_I^E{R}_O^I{R}_S^{O}R$

式中，为载体的姿态矩阵，在无陀螺捷联惯导系统中，也称为捷联矩阵。

步骤1.6)，根据步骤1.5)可求解出姿态矩阵中的各元素值，已知导航系{N}和载体系{B} 之间的坐标变换可求解出姿态矩阵的表达式为

那么导航姿态角可表示为

$J={\left(\begin{array}{ccc}\arcsin \left(R_{32}\right)& \arctan \left(\frac{{-R}_{31}}{R_{33}}\right)& \arctan \left(\frac{{-R}_{12}}{R_{22}}\right)\end{array}\right)}^T$

式中，θ，γ，分别为载体的俯仰角、横滚角和航向角，且有 θ∈(-π/2,π/2)，γ∈(-π,π)，结合上式可知：俯仰角在定义域内正弦函数唯 一，可单值确定；横滚角和航向角在定义域内正切函数不唯一，需中其他元素辅助判定， 真值判定如表1所示。

表1横滚角γ和航向角的真值判定表

根据步骤1.1)-1.6)的运算，即可完成一体化无陀螺捷联惯导系统的捷联解算过程。

作为本发明一体化无陀螺捷联惯导系统与GPS系统组合导航方法进一步的优化方案，步 骤4)中所述组合导航滤波器对输入的差值进行相应的滤波处理的详细步骤如下：

步骤2.1)，如图2所示，在组合模式上，一体化无陀螺捷联惯导系统与GPS系统组合导 航方法采用松散组合方式，即惯导系统和GPS相互独立工作，互不影响，各自输出载体的位 置、速度和姿态等导航参数信息。在组合滤波方式上，采用间接法，即将惯导输出的导航参 数值与GPS输出的导航参数值对应相减后得到导航参数的误差值，将此误差值作为组合导航 系统滤波的对象。在校正方法上，本发明采用输出校正法，即滤波后得到的最优误差，只用 来修正惯导的输出值，而不对惯导本身进行校正。在组合导航系统滤波器的选择上，本发明 选择卡尔曼滤波器进行。

步骤2.2)，建立捷联惯导系统的误差模型，推导导航位置、速度和姿态的误差微分方程， 其中，位置误差方程为：

$\left(\begin{array}{c}\delta \stackrel{·}{\lambda }=\frac{\sec \phi }{R+h}{\mathrm{\delta V}}_E+\frac{V_E\tan \phi \sec \phi }{R+h}\mathrm{\delta \phi }-\frac{V_E\sec \phi }{{(R+h)}^2}\mathrm{\delta h}\\ \delta \stackrel{·}{\phi }=\frac{1}{R+h}{\mathrm{\delta V}}_N-\frac{V_N}{{(R+h)}^2}\mathrm{\delta h}\\ \delta \stackrel{·}{h}={\mathrm{\delta V}}_U\end{array}\right)$

式中，λ，φ，h为载体所处地球表面的经度、纬度、高度，V_E，V_N，V_U为载体沿东、 北、天向的速度，R为地球半径；

速度误差方程为：

${\delta }^N\stackrel{·}{V}={}_I{}^{N}R{\delta }^I{a}_B+{}_I{}^{N}R\beta \times {}^{I}a_B-(2_I^N{R}^I{\omega }_E+{}_E{}^{N}R^E{\omega }_N)\times {\delta }^{N}V-({2\delta }^N\left({}^I\omega _E\right)+{\delta }^N\left({}^E\omega _N\right))\times {}^{N}V$

式中，β为实际导航系{N}偏离理想导航系(地理坐标系{G})的误差角矢量；

姿态误差方程为：

$\stackrel{·}{ϵ}=-{}_B{}^{N}R{\delta }^B\left({}^I\omega _B\right)+{\delta }^N\left({}^I\omega _N\right)+ϵ\times {}^N\left({}^I\omega _N\right)$

式中，ε为姿态误差角矢量，^N(^Iω_N)为导航系{N}相对于绝对惯性系{I}的角速率在导航 系{N}中的投影。

步骤2.3)，选取捷联惯导系统与GPS系统组合导航系统的状态变量X为9维、状态噪声 变量W为6维、量测变量Z为9维、量测噪声变量V为9维，具体参量如下：

W＝[δ^Ia_Bx δ^Ia_By δ^Ia_Bz δ^S(^Oω_Sx) δ^S(^Oω_Sy) δ^S(^Oω_Sz)]^T

其中，θ，γ，分别为载体的俯仰角、横滚角和航向角，N_E，N_N，N_U分别为GPS测 量得到的载体的位置沿东、北、天方向的距离误差。

步骤2.4)，建立捷联惯导系统与GPS系统组成的组合导航系统的状态空间模型和量测空 间模型，根据步骤2.2)中的误差方程和步骤2.3)中的状态变量，分别求解模型中的状态矩 阵F、系统噪声矩阵G、量测矩阵H和量测噪声矩阵I，得到组合导航系统的状态方程和量 测方程；

步骤2.5)，对步骤2.4)中的状态方程和量测方程进行离散化处理，得到组合导航系统 的时间更新方程和量测更新方程；

步骤2.6)，将输入的差值代入到步骤2.5)的更新方程中，迭代求解出组合导航系统的 最优估计，即捷联惯导系统导航参数的最优误差，并输出。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说 明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限制本发明，凡在 本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护 范围之内。