CN103674064A - 捷联惯性导航系统的初始标定方法 - Google Patents

Abstract

一种捷联惯性导航系统的初始标定方法，包括步骤：在捷联惯性导航系统保持静止不动的状态下，分别采集陀螺仪和加速度计的输出数据；对捷联惯性导航系统进行粗对准解算，得到初始姿态角；构建初始标定系统误差的离散系统模型，基于所述离散系统模型将未知的北向陀螺仪常值漂移量化为多个已知的备选方案，利用多个卡尔曼滤波器，并根据所述初始姿态角对可观测的北向陀螺仪常值漂移进行估计，得到北向陀螺仪常值漂移误差的估计值。本发明的技术，可以有效估计出北向陀螺漂移误差参数，利用估计出的高精度陀螺仪漂移误差，为捷联惯性导航系统的高精度运行提供了有效支持，有效改善了捷联惯性导航系统的可观测性，从而提高了初始标定的精度。

Description

捷联惯性导航系统的初始标定方法

技术领域

本发明涉及惯性导航技术领域，特别是涉及一种捷联惯性导航系统的初始标定方法。

背景技术

捷联惯性导航系统（Strapdown Inertial Navigation System，SINS）的基本原理是根据牛顿提出的相对惯性空间的力学定律，利用陀螺仪和加速度计测量载体相对于惯性空间的角运动和线运动参数，根据捷联惯性导航编排方程，进行相应的积分运算，连续、实时地提供位置、速度和姿态等导航信息。由于捷联惯性导航系统完全依靠自身的惯性敏感元件，不依赖任何外界导航参数，因此，它具有隐蔽性好，不受气候条件限制，抗干扰能力强和无信号丢失等优点，是一种完全自主式、全天候的导航系统。因此，被广泛应用于航空、航天和航海等各个领域。初始标定是捷联惯性导航系统工作的重要组成部分，标定精度对捷联惯性导航系统导航工作时的精度有直接的影响。因此，初始标定是捷联惯性导航系统工作过程中十分关键的一步。

通常，捷联惯性导航系统初始标定在粗对准完成后进行。粗对准阶段就是在静基座条件下，根据陀螺仪和加速度计输出与已知地球自转角速度和重力加速度的关系，直接计算出载体的初始姿态。初始标定是在粗对准的基础上进行的，通常利用现代控制理论中的状态空间方法，得到惯性导航系统初始标定的误差方程，对陀螺仪和加速度计输出的数据进行捷联解算，然后进行估计，获得惯性器件的漂移误差。传统的标定方法利用标准卡尔曼滤波估计方法，对惯性器件的漂移误差进行估计。然而，通过系统可观测度分析可知，系统中的惯性器件漂移误差的状态可观测度较低，收敛速度较慢，所需估计时间较长，无法对陀螺漂移误差进行精确估计。

发明内容

基于此，有必要针对上述估计时间较长，无法对陀螺漂移误差进行精确估计的问题，提供一种捷联惯性导航系统的初始标定方法。

一种捷联惯性导航系统的初始标定方法，包括如下步骤：

在捷联惯性导航系统保持静止不动的状态下，分别采集陀螺仪和加速度计的输出数据；

根据加速度计输出数据与重力加速度之间的关系以及陀螺仪的输出数据与地球自转角速度之间的关系，对捷联惯性导航系统进行粗对准解算，得到初始姿态角，包括航向角、俯仰角和横滚角；

构建捷联惯性导航系统的初始标定系统误差的离散系统模型，包括状态转移矩阵和量测矩阵；

基于所述离散系统模型将未知的北向陀螺仪常值漂移量化为多个已知的备选方案，利用多个卡尔曼滤波器，并根据所述初始姿态角对可观测的北向陀螺仪常值漂移进行估计，得到北向陀螺仪常值漂移误差的估计值。

上述捷联惯性导航系统的初始标定方法，将陀螺仪的常值漂移误差作为模型的不确定参数，通过将不确定参数量化为多个确定的备选结果，利用多个卡尔曼滤波器分别进行估计。提高了滤波器的收敛速度和精度，进而提高了陀螺仪漂移误差估计的收敛速度和精度，可以有效估计出北向陀螺漂移误差参数，利用估计出的高精度陀螺仪漂移误差，为捷联惯性导航系统的高精度运行提供了有效支持。可广泛应用于各种中高精度捷联惯性导航系统的初始标定，有效改善了捷联惯性导航系统的可观测性，从而提高了初始标定的精度。

附图说明

图1为一个实施例的捷联惯性导航系统的初始标定方法流程图。

具体实施方式

下面结合附图对本发明的捷联惯性导航系统的初始标定方法的具体实施方式作详细描述。

参见图1所示，图1为一个实施例的捷联惯性导航系统的初始标定方法流程图，主要包括如下步骤：

步骤S10，在捷联惯性导航系统保持静止不动的状态下，分别采集陀螺仪和加速度计的输出数据。

步骤S20，根据加速度计输出数据与重力加速度之间的关系以及陀螺仪的输出数据与地球自转角速度之间的关系，对捷联惯性导航系统进行粗对准解算，得到初始姿态角；包括航向角

俯仰角θ₁和横滚角γ₁。

步骤S30，构建捷联惯性导航系统的初始标定系统误差的离散系统模型，包括状态转移矩阵和量测矩阵。

步骤S40，基于所述离散系统模型将未知的北向陀螺仪常值漂移量化为多个已知的备选方案，利用多个卡尔曼滤波器，并根据所述初始姿态角对可观测的北向陀螺仪常值漂移进行估计，得到北向陀螺仪常值漂移误差的估计值。

上述捷联惯性导航系统的初始标定方法，将陀螺仪的常值漂移误差作为模型的不确定参数，通过将不确定参数量化为多个确定的备选结果，利用多个卡尔曼滤波器分别进行估计。提高了滤波器的收敛速度和精度，进而提高了陀螺仪漂移误差估计的收敛速度和精度，可以有效估计出北向陀螺漂移误差参数，利用估计出的高精度陀螺仪漂移误差，为捷联惯性导航系统的高精度运行提供了有效支持。可广泛应用于各种中高精度捷联惯性导航系统的初始标定，有效改善了捷联惯性导航系统的可观测性，从而提高了初始标定的精度。

在一个实施例中，所述步骤S10在捷联惯性导航系统保持静止不动的状态下，分别采集陀螺仪和加速度计的输出数据的步骤，具体包括如下：

步骤S101，在捷联惯性导航系统开机后，预热30分钟，直至陀螺仪和加速度计进入稳定工作状态。

步骤S102，在捷联惯性导航系统预热进入稳定工作状态后，保持捷联惯性导航系统保持静止不动，采集5分钟的陀螺仪和加速度计的输出数据。

在一个实施例中，所述步骤S20根据加速度计输出数据与重力加速度之间的关系以及陀螺仪的输出数据与地球自转角速度之间的关系，对捷联惯性导航系统进行粗对准解算，得到初始姿态角的步骤，具体包括如下：

步骤S201，根据加速度计输出数据与重力加速度之间的关系以及陀螺仪的输出数据与地球自转角速度之间的关系计算地理坐标系与载体坐标系间的欧拉角转换矩阵。

步骤S202，根据所述欧拉角转换矩阵计算粗对准过程中的俯仰角θ₁、横滚角γ₁和航向角

需要说明的是，上述粗对准及计算初始姿态角过程中，所采用的技术手段并不影响步骤S30的离散系统模型的构建过程及步骤S40的北向陀螺仪常值漂移误差的估计值计算过程，步骤S30与步骤S40的过程是在粗对准的基础上完成。

在一个实施例中，所述步骤S30构建捷联惯性导航系统的初始标定系统误差的离散系统模型的步骤，具体包括如下：

步骤S301，在取东北天地理坐标为系导航坐标系，以及忽略位置误差和天向通道的速度误差的条件下建立捷联惯性导航系统的初始标定的状态方程。

步骤S302，选取捷联惯性导航系统的两个水平误差设为观测量，建立捷联惯性导航系统的初始标定的量测方程。

步骤S303，对所述初始标定的状态方程和量测方程进行离散化，获得离散化的初始标定的系统误差方程。

在一个实施例中，步骤S40基于所述离散系统模型将未知的北向陀螺仪常值漂移量化为多个已知的备选方案，利用多个卡尔曼滤波器，并根据所述初始姿态角对可观测的北向陀螺仪常值漂移进行估计，得到北向陀螺仪常值漂移误差的估计值的步骤，具体包括如下：

步骤S401，基于所述离散系统模型，采用多模型估计方式将北向陀螺仪常值漂移误差设为离散系统模型的不确定参数，并将所述不确定参数量化为多个确定的备选结果。

优选地，对于上述量化不确定参数获取备选结果的步骤，具体包括：根据估计精度和计算复杂度确定备选结果的个数，并根据陀螺仪的精度水平，量化得到多个确定的备选结果。

步骤S402，利用多个卡尔曼滤波器，并根据所述初始姿态角和多个确定的备选结果对可观测的北向陀螺仪常值漂移分别进行估计，获得北向陀螺仪常值漂移误差的估计值。

在一个实施例中，步骤S402利用多个卡尔曼滤波器，并根据所述初始姿态角和多个确定的备选结果对可观测的北向陀螺仪常值漂移分别进行估计，获得北向陀螺仪常值漂移误差的估计值的步骤，具体包括如下：

步骤a，对所述多个确定的备选结果进行参数初始化处理，获得备选结果的初始状态及其对应多个卡尔曼滤波器的初始概率。

步骤b，以所述初始姿态角为初始值进行捷联解算，确定载体的水平速度，获得北向陀螺仪常值漂移的量测量。

步骤c，根据所述离散系统模型和确定的备选结果初始状态计算状态一步预测和一步预测均方误差阵。

步骤d，根据所述离散系统模型的量测矩阵和所述一步预测均方误差阵获取新息方差，计算卡尔曼滤波增益和估计均方误差，根据所述北向陀螺仪常值漂移的量测量和状态一步预测获取各个卡尔曼滤波器的新息序列，根据所述新息序列分别求解各个卡尔曼滤波器的状态估计值。

步骤e，根据所述新息方差和各个卡尔曼滤波器的新息序列求解各个卡尔曼滤波器的新息概率，根据所述新息概率对相应的卡尔曼滤波器的初始概率进行更新，根据各个卡尔曼滤波器的状态估计值和更新的各个卡尔曼滤波器的概率获取北向陀螺仪常值漂移误差的估计值。

为了更加清晰本发明的技术方案，下面针对于上述各个实施例的过程作进一步的详细阐述。

在步骤S201中，加速度计输出数据与重力加速度以及陀螺仪输出与地球自转角速度关系如下：

$\left[\begin{array}{c}{f}_{x1}\\ f_{y1}\\ f_{z1}\end{array}\right]=C_{n^{\′}}^b\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]---\left(1\right)$

$\left[\begin{array}{c}{\mathrm{\ω}}_{x1}\\ {\mathrm{\ω}}_{y1}\\ {\mathrm{\ω}}_{z1}\end{array}\right]=C_{n^{\′}}^b\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos \\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}L\right]---\left(2\right)$

其中，f_x1、f_y1和f_z1以及ω_x1、ω_y1和ω_z1分别为静止位置上的捷联惯性导航系统在x轴、y轴和z轴输出的比力和角速率；g为重力加速度；ω_ie为地球自转角速率在导航坐标系中的投影，其在东、北和天向上的投影为Ω=[0 ω_iecosL ω_iesinL]^T，上标T表示向量的转置；L表示当地纬度；

为计算地理坐标系n'与载体坐标系b之间的欧拉角转换矩阵。

在步骤S202中，根据所述欧拉角转换矩阵计算粗对准过程中的俯仰角θ₁、横滚角γ₁和航向角

的计算方法，具体包括如下：

所述计算得到计算地理坐标系n'与载体坐标系b之间的欧拉角转换矩阵步骤包括如下：

$\left.\begin{array}{c}{C}_{n^{\′}}^b={\left(C_b^{n^{\′}}\right)}^T=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]\\ C_{13}=\frac{f_{X1}}{g}{C}_{23}=\frac{f_{y1}}{g}{C}_{33}=\frac{f_{z1}}{g}\\ C_{12}=\frac{{\mathrm{\ω}}_{x1}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{f_{x1}\tan L}{g}\\ C_{22}=\frac{{\mathrm{\ω}}_{y1}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{f_{y1}\tan L}{g}\\ C_{32}=\frac{{\mathrm{\ω}}_{z1}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{f_{z1}\tan L}{g}\\ C_{11}=C_{22}{C}_{33}-C_{23}{C}_{32}\\ C_{21}=-C_{12}{C}_{33}+C_{23}{C}_{32}\\ C_{21}=C_{12}{C}_{23}-C_{13}{C}_{22}\end{array}\right\}---\left(3\right)$

航向角

俯仰角θ₁和横滚角γ₁的主值计算公式为：

若

θ₁和γ₁的取值范围分别定义为[0,2π]、

和[-π,+π]，航向角

俯仰角θ₁和横滚角γ₁的真实值按如下的方法确定：

式中，

为欧拉角转换矩阵，所确定的

θ₁和γ₁为捷联惯性导航系统粗对准的航向角、俯仰角和横滚角。

在步骤S301中，初始标定的状态方程如下：

$\left[\begin{array}{c}{\stackrel{\·}{x}}_1\left(t\right)\\ {\stackrel{\·}{x}}_2\left(t\right)\end{array}\right]=\left[\begin{array}{cc}{F}_{5\×5}& I_{5\×1}\\ 0_{1\×5}& 0_{1\×1}\end{array}\right]\left[\begin{array}{c}{x}_1\left(t\right)\\ x_2\left(t\right)\end{array}\right]+G\left(t\right)w\left(t\right)---\left(6\right)$

式中，状态变量x₁(t)=[δv_E(t) δv_N(t) φ_E(t) φ_N(t) φ_U(t)]^T，x₂(t)=ε_N(t)，上标T表示向量的转置；其中，δv和φ分别为系统的速度误差和姿态误差，ε为陀螺仪的常值漂移；下标E、N、U分别表示导航坐标系下的东向、北向和天向，w(t)=[w_ax(t) w_ay(t) w_az(t) w_gx(t) w_gy(t) w_gz(t)]^T为系统高斯白噪声向量且E[w(t)w^T(t)]=Q(t)，其中各个元素均为和时间相关的量，包括三个方向加速度计的随机偏置噪声和三个方向陀螺仪的随机漂移噪声，下标中的第一个值a和g分别代表加速度计和陀螺仪，第二个值x、y和z分别代表载体坐标系的右方、前方和上方；

系统状态转移阵中的F_5×5和I_5×1表达式分别如下：

$F_{5\×5}=\left[\begin{array}{ccccc}0& {2\mathrm{\Ω}}_U& 0& -g& 0\\ {-2\mathrm{\Ω}}_U& 0& g& 0& 0\\ 0& 0& 0& {\mathrm{\Ω}}_U& {-\mathrm{\Ω}}_N\\ 0& 0& {-\mathrm{\Ω}}_U& 0& 0\\ 0& 0& {\mathrm{\Ω}}_N& 0& 0\end{array}\right]---\left(7\right)$

式中，Ω=[0 ω_iecosL ω_iesinL]^T=[0 Ω_N Ω_U]^T为地球自转角速度ω_ie在东向、北向和天向上的投影，L表示当地纬度；g为当地的重力加速度，I_5×1=[0 0 0 1 0]^T；

G(t)为系统噪声转移矩阵，其表达式为：

$G\left(t\right)=\left[\begin{array}{cc}{\left(C_b^{n^{\′}}\right)}_{2\×3}& 0_{2\×3}\\ 0_{\text{3\×3}}& C_b^{n^{\′}}\\ 0_{1\×3}& 0_{1\×3}\end{array}\right]---\left(8\right)$

式中，为载体坐标系b与计算地理坐标系n'之间的欧拉角转换矩阵，

为矩阵的前两行元素组成的矩阵。

在步骤S302中，初始标定的量测方程如下：

选取两个水平速度误差δv_E和δv_N为观测量，建立系统的量测方程为：

$z\left(t\right)=\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×4}\end{array}\right]\left[\begin{array}{c}{x}_1\left(t\right)\\ x_2\left(t\right)\end{array}\right]+v\left(t\right)---\left(9\right)$

式中，量测向量为Z(t)=[δv_E(t) δv_N(t)]^T，量测噪声向量为 $v\left(t\right)={\left[\begin{array}{cc}{v}_{{\mathrm{\δv}}_E}\left(t\right)& v_{{\mathrm{\δv}}_N}\left(t\right)\end{array}\right]}^T$ 为高斯白噪声，且E[v(t)v^T(t)]=R(t)，包括东向和北向速度量测噪声。

在步骤S303中，离散系统模型的方程为：

$\begin{array}{c}{x}_k={\mathrm{\Φ}}_{k/k-1}{x}_{k-1}+{\mathrm{\Γ}}_{k-1}{w}_{k-1}\\ z_k=H_k{x}_k+v_k\end{array}---\left(10\right)$

式中，Φ_k/k-1和Γ_k-1分别为状态转移矩阵和系统噪声转移阵的离散化形式，H_k为量测阵的离散化形式，过程噪声和量测噪声的方差分别为E[w(k)w^T(k)]=Q(k)和E[v(k)v^T(k)]=R(k)。

对于步骤S402计算北向陀螺仪常值漂移误差的估计值的过程中。

步骤a中的量化后的备选结果的初始化状态值如下：

${\hat{x}}_0\left(i\right)={\left[\begin{array}{cc}{0}_{1\×5}& \mathrm{\ϵ}\left(i\right)\end{array}\right]}^T(i=\mathrm{1,2},...,n)---\left(11\right)$

式中，

为备选结果的初始化状态值，i(i=1,2,…,n)为确定备选结果的个数，ε(i)为量化得到的多个确定的备选结果。

初始化各滤波器概率如下：

$P_0\left(i\right)=\frac{1}{n}(i=\mathrm{1,2},...,n)---\left(12\right)$

步骤b中的所述北向陀螺仪常值漂移的量测量如下：

$z_k={\left[\begin{array}{cc}{\text{\δv}}_E& {\mathrm{\δv}}_N\end{array}\right]}^T---\left(13\right)$

式中，δv_Eδv_N为根据初始标定的状态方程两个水平速度误差选取的观测量。

步骤c中的所述状态一步预测如下：

${\hat{x}}_{k/k-1}\left(i\right)={\mathrm{\Φ}}_{k/k-1}{\hat{x}}_{k-1}\left(i\right)(i=\mathrm{1,2},...,n)---\left(14\right)$

所述一步预测均方误差阵如下：

$P_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{P}_{k-1}{\mathrm{\Φ}}_{k/k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{{\mathrm{\Γ}}_{k-1}}^T---\left(15\right)$

式中，Φ_k/k-1和Γ_k-1分别为离散系统模型的状态转移矩阵和系统噪声转移阵，为初始化状态值，Q_k-1为过程噪声方差矩阵，P_k-1为系统协方差矩阵。

步骤d中的所述新息方差如下：

$U_k=H_k{P}_{k/k-1}{H}_k^T+R_k---\left(16\right)$

式中，H_k为离散化的量测矩阵，P_k/k-1为一步预测均方误差阵，Rk为根据陀螺仪、加速度计噪声水平和量测量确定的量测噪声矩阵。

所述卡尔曼滤波增益如下：

$K_k=p_{k/k-1}{H}_k^T{U_k}^{-1}---\left(17\right)$

所述估计均方误差如下：

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(18\right)$

所述各个卡尔曼滤波器的新息序列如下：

$J_k\left(i\right)=z_k-H_k{\hat{x}}_{k/k-1}(i=\mathrm{1,2},..n)---\left(19\right)$

所述各个卡尔曼滤波器的状态估计值如下：

${\hat{x}}_k\left(i\right)={\hat{x}}_{k/k-1}\left(i\right)+K_k{J}_k\left(i\right)(i=\mathrm{1,2},...,n)---\left(20\right)$

步骤e中的所述各个卡尔曼滤波器的新息概率如下：

$p_k\left(i\right)={\frac{1}{2\mathrm{\π}\sqrt{\left|U_k\right|}}e}^{-\frac{J_k{\left(i\right)}^{\′{\left(U_k\right)}^{-1}{J}_k\left(i\right)}}{2}(i=1,2,...,n)---\left(21\right)}$

所述各个卡尔曼滤波器的概率的更新如下：

$P_k{\left(i\right)}^{\′}=\frac{p_k\left(i\right)P_{k-1}\left(i\right)}{\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{p}_k\left(j\right)P_{k-1}\left(j\right)}(i=\mathrm{1,2},...,n)---\left(12\right)$

式中，P_k(i)'为根据卡尔曼滤波器的初始概率P₀(i)对进行P_k(i)更新后的各个卡尔曼滤波器的概率。

所述北向陀螺仪常值漂移误差的估计值如下：

${\hat{x}}_k=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_k{\left(i\right)}^{\′}{\hat{x}}_k\left(i\right)---\left(23\right)$

式中，

中第六个元素为北向陀螺仪常值漂移误差的估计值。

基于上述实施例阐述的内容，本发明的捷联惯性导航系统的初始标定方法的一个具体实施方式可以如下：

步骤1、捷联惯性导航系统预热准备：捷联惯性导航系统开机后，预热30分钟，使陀螺仪和加速度计的工作状态稳定。

步骤2、采集陀螺仪和加速度计数据：捷联惯性导航系统预热准备完毕后，保持捷联惯性导航系统静止不动，采集5分钟的陀螺仪和加速度计的输出数据。

步骤3、粗对准，计算初始姿态角；

步骤（3-1）、计算计算地理坐标系与载体坐标系间的欧拉角转换矩阵

导航坐标系取为东北天地理坐标系，可知加速度计输出与重力加速度，以及陀螺仪输出与地球自转角速度有如下的关系：

$\left[\begin{array}{c}{f}_{x1}\\ f_{y1}\\ f_{z1}\end{array}\right]=C_{n^{\′}}^b\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]---\left(1\right)$

$\left[\begin{array}{c}{\mathrm{\ω}}_{x1}\\ {\mathrm{\ω}}_{y1}\\ {\mathrm{\ω}}_{z1}\end{array}\right]=C_{n^{\′}}^b\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos \\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}L\right]---\left(2\right)$

其中，f_x1、f_y1和f_z1以及ω_x1、ω_y1和ω_z1分别为静止位置上捷联惯性导航系统在x轴、y轴和z轴输出的比力和角速率；g为重力加速度；ω_ie为地球自转角速率在导航坐标系中的投影，其在东、北和天向上的投影为Ω=[0 ω_iecosL ω_iesinL]^T，上标T表示向量的转置；L表示当地纬度；

为计算地理坐标系n'与载体坐标系b之间的欧拉角转换矩阵。

步骤（3-2）、计算粗对准过程中的俯仰角θ₁、横滚角γ₁和航向角

具体计算过程如下：

由公式（1）和（2）可计算得到计算地理坐标系n'与载体坐标系b之间的欧拉角转换矩阵

$\left.\begin{array}{c}{C}_{n^{\′}}^b={\left(C_b^{n^{\′}}\right)}^T=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]\\ C_{13}=\frac{f_{X1}}{g}{C}_{23}=\frac{f_{y1}}{g}{C}_{33}=\frac{f_{z1}}{g}\\ C_{12}=\frac{{\mathrm{\ω}}_{x1}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{f_{x1}\tan L}{g}\\ C_{22}=\frac{{\mathrm{\ω}}_{y1}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{f_{y1}\tan L}{g}\\ C_{32}=\frac{{\mathrm{\ω}}_{z1}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{f_{z1}\tan L}{g}\\ C_{11}=C_{22}{C}_{33}-C_{23}{C}_{32}\\ C_{21}=-C_{12}{C}_{33}+C_{23}{C}_{32}\\ C_{21}=C_{12}{C}_{23}-C_{13}{C}_{22}\end{array}\right\}---\left(3\right)$

航向角

俯仰角θ₁和横滚角γ₁的主值计算公式为：

若θ₁和γ₁的取值范围分别定义为[0,2π]、

和[-π,+π]，那么航向角

俯仰角θ₁和横滚角γ₁的真实值按如下的方法确定：

由上式所确定的

θ₁和γ₁即为捷联惯性导航系统粗对准的航向角、俯仰角和横滚角。

步骤4、对北向陀螺仪常值漂移进行多滤波器联合估计；使用简化的初始标定系统误差方程，基于多模型估计的思想，将未知的北向陀螺仪常值漂移量化为多个已知的备选方案，采用多个卡尔曼滤波器，对可观测的北向陀螺仪常值漂移进行估计。北向陀螺仪常值漂移的估计过程如下：

步骤（4-1）、建立简化的初始标定系统误差方程：

导航坐标系取为东北天地理坐标系，位置误差和天向通道的速度误差忽略。由于水平通道的加速度计常值漂移，以及天向和东向的陀螺仪常值漂移不可观测，因此，将其从传统的十个状态中去除。

初始标定的状态方程如下：

$\left[\begin{array}{c}{\stackrel{\·}{x}}_1\left(t\right)\\ {\stackrel{\·}{x}}_2\left(t\right)\end{array}\right]=\left[\begin{array}{cc}{F}_{5\×5}& I_{5\×1}\\ 0_{1\×5}& 0_{1\×1}\end{array}\right]\left[\begin{array}{c}{x}_1\left(t\right)\\ x_2\left(t\right)\end{array}\right]+G\left(t\right)w\left(t\right)---\left(6\right)$

式中，状态变量x₁(t)=[δv_E(t) δv_N(t) φ_E(t) φ_N(t) φ_U(t)]^T，x₂(t)=ε_N(t)，上标T表示向量的转置；其中，δv和φ分别为系统的速度误差和姿态误差，ε为陀螺仪的常值漂移；下标E、N、U分别表示导航坐标系下的东向、北向和天向；其中，w(t)=[w_ax(t) w_ay(t) w_az(t) w_gx(t) w_gy(t) w_gz(t)]^T为系统高斯白噪声向量且E[w(t)w^T(t)]=Q(t)，其中各个元素均为和时间相关的量，包括三个方向加速度计的随机偏置噪声和三个方向陀螺仪的随机漂移噪声，下标中的第一个值a和g分别代表加速度计和陀螺仪，第二个值x、y和z分别代表载体坐标系的右方、前方和上方。

系统状态转移阵中的F_5×5和I_5×1表达式分别如下：

$F_{5\×5}=\left[\begin{array}{ccccc}0& {2\mathrm{\Ω}}_U& 0& -g& 0\\ {-2\mathrm{\Ω}}_U& 0& g& 0& 0\\ 0& 0& 0& {\mathrm{\Ω}}_U& {-\mathrm{\Ω}}_N\\ 0& 0& {-\mathrm{\Ω}}_U& 0& 0\\ 0& 0& {\mathrm{\Ω}}_N& 0& 0\end{array}\right]---\left(7\right)$

式中，Ω=[0 ω_iecosL ω_iesinL]^T=[0 Ω_NΩ_U]^T为地球自转角速度ω_ie在东向、北向和天向上的投影，L表示当地纬度；g为当地的重力加速度，I_5×1=[0 0 0 1 0]^T。

G(t)为系统噪声转移矩阵，其表达式为：

$G\left(t\right)=\left[\begin{array}{cc}{\left(C_b^{n^{\′}}\right)}_{2\×3}& 0_{2\×3}\\ 0_{\text{3\×3}}& C_b^{n^{\′}}\\ 0_{1\×3}& 0_{1\×3}\end{array}\right]---\left(8\right)$

式中，

为载体坐标系b与计算地理坐标系n'之间的欧拉角转换矩阵，

为

矩阵的前两行元素组成的矩阵，具体可由公式（3）获得。

初始标定的量测方程如下：

选取两个水平速度误差δv_E和δv_N为观测量，建立系统的量测方程为：

$z\left(t\right)=\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×4}\end{array}\right]\left[\begin{array}{c}{x}_1\left(t\right)\\ x_2\left(t\right)\end{array}\right]+v\left(t\right)---\left(9\right)$

式中，量测向量Z(t)=[δv_E(t)δv_N(t)]^T，量测噪声向量 $v\left(t\right)={\left[\begin{array}{cc}{v}_{{\mathrm{\δv}}_e}\left(t\right)& v_{{\mathrm{\δv}}_N}\end{array}\right]}^T$ 为高斯白噪声，且E[v(t)v^T(t)]=R(t)，包括东向和北向速度量测噪声。

对公式（6）和（9）进行离散化，建立初始标定的离散系统模型。可得离散化后的系统方程为

$\begin{array}{c}{x}_k={\mathrm{\Φ}}_{k/k-1}{x}_{k-1}+{\mathrm{\Γ}}_{k-1}{w}_{k-1}\\ z_k=H_k{x}_k+v_k\end{array}---\left(10\right)$

式中，Φ_k/k-1和Γ_k-1分别为状态转移矩阵和系统噪声转移阵的离散化形式，H_k为量测阵的离散化形式，过程噪声和量测噪声的方差分别为E[w(k)w^T(k)]=Q(k)和E[v(k)v^T(k)]=R(k)。

步骤（4-2）、北向陀螺仪常值漂移误差估计：

基于多模型估计的思想，将北向陀螺仪常值漂移误差作为模型不确定参数，将不确定参数量化为多个确定的备选结果，利用多个卡尔曼滤波器分别进行估计，获得北向陀螺仪常值漂移误差。

步骤（a）、参数初始化：根据估计精度和计算复杂度之间的折中，确定备选结果的个数i(i=1,2,…,n)，并根据陀螺仪的精度水平，量化得到多个确定的备选结果ε(i)，并将量化后的备选结果ε(i)初始化状态

如下：

${\hat{x}}_0\left(i\right)={\left[\begin{array}{cc}{0}_{1\×5}& \mathrm{\ϵ}\left(i\right)\end{array}\right]}^T(i=\mathrm{1,2},...,n)---\left(11\right)$

由于各个滤波器的概率相同，初始化各滤波器概率如下：

$P_0\left(i\right)=\frac{1}{n}(i=\mathrm{1,2},...,n)---\left(12\right)$

同时，根据陀螺仪、加速度计噪声水平和量测量的精度，确定过程噪声矩阵Q(k)和量测噪声矩阵R(k)，初始化系统协方差矩阵P₀。

步骤（b）、捷联解算：以步骤（3）获得的初始航向角

俯仰角θ₁和横滚角γ₁为初值，进行捷联解算，获得载体的水平速度输出，即可获得量测量如下：

$z_k={\left[\begin{array}{cc}{\text{\δv}}_E& {\mathrm{\δv}}_N\end{array}\right]}^T---\left(13\right)$

步骤（c）、时间更新：利用公式（10）获得的状态转移矩阵Φ_k/k-1和公式（11）获得的初始化状态

计算状态一步预测如下：

${\hat{x}}_{k/k-1}\left(i\right)={\mathrm{\Φ}}_{k/k-1}{\hat{x}}_{k-1}\left(i\right)(i=\mathrm{1,2},...,n)---\left(14\right)$

同时计算一步预测均方误差阵如下：

$P_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{P}_{k-1}{\mathrm{\Φ}}_{k/k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{{\mathrm{\Γ}}_{k-1}}^T---\left(15\right)$

步骤（d）、量测更新：利用公式（10）获得的量测矩阵H_k和公式（15）获得的一步预测均方误差阵P_k/k-1，可得新息方差如下：

$U_k=H_k{P}_{k/k-1}{H}_k^T+R_k---\left(16\right)$

进一步可计算卡尔曼滤波增益如下：

$K_k=p_{k/k-1}{H}_k^T{U_k}^{-1}---\left(17\right)$

同时可得估计均方误差如下：

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(18\right)$

利用公式（13）获得的量测量zk和公式（14）获得的状态一步预测可得各滤波器新息如下：

$J_k\left(i\right)=z_k-H_k{\hat{x}}_{k/k-1}(i=\mathrm{1,2},..n)---\left(19\right)$

进一步，可求解各滤波器状态估计如下：

${\hat{x}}_k\left(i\right)={\hat{x}}_{k/k-1}\left(i\right)+K_k{J}_k\left(i\right)(i=\mathrm{1,2},...,n)---\left(20\right)$

步骤（e）、多滤波器融合：利用公式（16）获得的新息方差U_k和公式（19）获得的各滤波器新息J_k(i)，各滤波器的新息概率求解如下：

$P_k{\left(i\right)}^{\′}=\frac{p_k\left(i\right)P_{k-1}\left(i\right)}{\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{p}_k\left(j\right)P_{k-1}\left(j\right)}(i=\mathrm{1,2},...,n)---\left(21\right)$

利用公式（12）获得的各滤波器初始概率P0(i)，各滤波器概率更新如下所示：

$P_k{\left(i\right)}^{\′}=\frac{p_k\left(i\right)P_{k-1}\left(i\right)}{\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{p}_k\left(j\right)P_{k-1}\left(j\right)}(i=\mathrm{1,2},...,n)---\left(22\right)$

进一步，可得状态估计结果如下：

${\hat{k}}_k=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_k{\left(i\right)}^{\′}{\hat{x}}_k\left(i\right)---\left(22\right)$

状态估计

中第六个元素即为北向陀螺仪常值漂移误差的估计值。

步骤5、将上述步骤获得的北向陀螺漂移误差作为捷联惯性导航系统初始标定的结果，初始标定解算完毕。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种捷联惯性导航系统的初始标定方法，其特征在于，包括如下步骤：

在捷联惯性导航系统保持静止不动的状态下，分别采集陀螺仪和加速度计的输出数据；

根据加速度计输出数据与重力加速度之间的关系以及陀螺仪的输出数据与地球自转角速度之间的关系，对捷联惯性导航系统进行粗对准解算，得到初始姿态角，包括航向角、俯仰角和横滚角；

构建捷联惯性导航系统的初始标定系统误差的离散系统模型，包括状态转移矩阵和量测矩阵；

基于所述离散系统模型将未知的北向陀螺仪常值漂移量化为多个已知的备选方案，利用多个卡尔曼滤波器，并根据所述初始姿态角对可观测的北向陀螺仪常值漂移进行估计，得到北向陀螺仪常值漂移误差的估计值。

2.根据权利要求1所述的捷联惯性导航系统的初始标定方法，其特征在于，在捷联惯性导航系统保持静止不动的状态下，分别采集陀螺仪和加速度计的输出数据的步骤包括：

在捷联惯性导航系统开机后，预热30分钟，直至陀螺仪和加速度计进入稳定工作状态；

在捷联惯性导航系统预热进入稳定工作状态后，保持捷联惯性导航系统保持静止不动，采集5分钟的陀螺仪和加速度计的输出数据。

3.根据权利要求1所述的捷联惯性导航系统的初始标定方法，其特征在于，根据加速度计输出数据与重力加速度之间的关系以及陀螺仪的输出数据与地球自转角速度之间的关系，对捷联惯性导航系统进行粗对准解算，得到初始姿态角的步骤包括：

根据加速度计输出数据与重力加速度之间的关系以及陀螺仪的输出数据与地球自转角速度之间的关系计算地理坐标系与载体坐标系间的欧拉角转换矩阵；

根据所述欧拉角转换矩阵计算粗对准过程中的俯仰角θ₁、横滚角γ₁和航向角

4.根据权利要求1所述的捷联惯性导航系统的初始标定方法，其特征在于，所述构建捷联惯性导航系统的初始标定系统误差的离散系统模型的步骤包括：

在取东北天地理坐标为系导航坐标系，以及忽略位置误差和天向通道的速度误差的条件下建立捷联惯性导航系统的初始标定的状态方程；

选取捷联惯性导航系统的两个水平误差设为观测量，建立捷联惯性导航系统的初始标定的量测方程；

对所述初始标定的状态方程和量测方程进行离散化，获得离散化的初始标定的系统误差方程。

5.根据权利要求1所述的捷联惯性导航系统的初始标定方法，其特征在于，基于所述离散系统模型将未知的北向陀螺仪常值漂移量化为多个已知的备选方案，利用多个卡尔曼滤波器，并根据所述初始姿态角对可观测的北向陀螺仪常值漂移进行估计，得到北向陀螺仪常值漂移误差的估计值的步骤包括：

基于所述离散系统模型，采用多模型估计方式将北向陀螺仪常值漂移误差设为离散系统模型的不确定参数，并将所述不确定参数量化为多个确定的备选结果；

利用多个卡尔曼滤波器，并根据所述初始姿态角和多个确定的备选结果对可观测的北向陀螺仪常值漂移分别进行估计，获得北向陀螺仪常值漂移误差的估计值。

6.根据权利要求5所述的捷联惯性导航系统的初始标定方法，其特征在于，利用多个卡尔曼滤波器，并根据所述初始姿态角和多个确定的备选结果对可观测的北向陀螺仪常值漂移分别进行估计，获得北向陀螺仪常值漂移误差的估计值的步骤包括：

对所述多个确定的备选结果进行参数初始化处理，获得备选结果的初始状态及其对应多个卡尔曼滤波器的初始概率；

以所述初始姿态角为初始值进行捷联解算，确定载体的水平速度，获得北向陀螺仪常值漂移的量测量；

根据所述离散系统模型和确定的备选结果初始状态计算状态一步预测和一步预测均方误差阵；

根据所述离散系统模型的量测矩阵和所述一步预测均方误差阵获取新息方差，计算卡尔曼滤波增益和估计均方误差，根据所述北向陀螺仪常值漂移的量测量和状态一步预测获取各个卡尔曼滤波器的新息序列，根据所述新息序列分别求解各个卡尔曼滤波器的状态估计值；

根据所述新息方差和各个卡尔曼滤波器的新息序列求解各个卡尔曼滤波器的新息概率，根据所述新息概率对相应的卡尔曼滤波器的初始概率进行更新，根据各个卡尔曼滤波器的状态估计值和更新的各个卡尔曼滤波器的概率获取北向陀螺仪常值漂移误差的估计值。

7.根据权利要求6所述的捷联惯性导航系统的初始标定方法，其特征在于，将所述不确定参数量化为多个确定的备选结果的步骤包括：

根据估计精度和计算复杂度确定备选结果的个数，并根据陀螺仪的精度水平，量化得到多个确定的备选结果。

8.根据权利要求1至7任一项所述的捷联惯性导航系统的初始标定方法，其特征在于，加速度计输出数据与重力加速度以及陀螺仪输出与地球自转角速度关系如下：

$\left[\begin{array}{c}{f}_{x1}\\ f_{y1}\\ f_{z1}\end{array}\right]=C_{n^{\′}}^b\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$

$\left[\begin{array}{c}{\mathrm{\ω}}_{x1}\\ {\mathrm{\ω}}_{y1}\\ {\mathrm{\ω}}_{z1}\end{array}\right]=C_{n^{\′}}^b\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos \\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}L\right]$

其中，f_x1、f_y1和f_z1以及ω_x1、ω_y1和ω_z1分别为静止位置上的捷联惯性导航系统在x轴、y轴和z轴输出的比力和角速率；g为重力加速度；ω_ie为地球自转角速率在导航坐标系中的投影，其在东、北和天向上的投影为Ω=[0 ω_iecosL ω_iesinL]^T，上标T表示向量的转置；L表示当地纬度；

为计算地理坐标系n'与载体坐标系b之间的欧拉角转换矩阵；

根据所述欧拉角转换矩阵计算粗对准过程中的俯仰角θ₁、横滚角γ₁和航向角

具体计算的步骤包括：

所述计算得到计算地理坐标系n'与载体坐标系b之间的欧拉角转换矩阵步骤包括如下：

$C_{n^{\′}}^b={\left(C_b^{n^{\′}}\right)}^T=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

$C_{13}=\frac{f_{x1}}{g}{C}_{23}=\frac{f_{y1}}{g}{C}_{33}=\frac{f_{z1}}{g}$

$C_{12}=\frac{{\mathrm{\ω}}_{x1}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{f_{x1}\tan L}{g}$

$C_{22}=\frac{{\mathrm{\ω}}_{y1}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{f_{x1}\tan L}{g}$

$C_{32}=\frac{{\mathrm{\ω}}_{z1}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{f_{z1}\tan L}{g}$

C₁₁=C₂₂C₃₃-C₂₃C₃₂

C₂₁=-C₁₂C₃₃+C₁₃C₃₂

C₂₁=C₁₂C₂₃-C₁₃C₂₂

航向角

俯仰角θ₁和横滚角γ₁的主值计算公式为：

若

θ₁和γ₁的取值范围分别定义为[0,2π]、

和[-π,+π]，航向角

俯仰角θ₁和横滚角γ₁的真实值按如下的方法确定：

式中，

为欧拉角转换矩阵，所确定的

θ₁和γ₁为捷联惯性导航系统粗对准的航向角、俯仰角和横滚角。

9.根据权利要求1至7任一项所述的捷联惯性导航系统的初始标定方法，其特征在于，

初始标定的状态方程如下：

$\left[\begin{array}{c}{\stackrel{\·}{x}}_1\left(t\right)\\ {\stackrel{\·}{x}}_2\left(t\right)\end{array}\right]=\left[\begin{array}{cc}{F}_{5\×5}& I_{5\×1}\\ 0_{1\×5}& 0_{1\×1}\end{array}\right]\left[\begin{array}{c}{x}_1\left(t\right)\\ x_2\left(t\right)\end{array}\right]+G\left(t\right)w\left(t\right)$

式中，状态变量x₁(t)=[δv_E(t) δv_N(t) φ_E(t) φ_N(t) φ_U(t)]^T，x₂(t)=ε_N(t)，上标T表示向量的转置；其中，δv和φ分别为系统的速度误差和姿态误差，ε为陀螺仪的常值漂移；下标E、N、U分别表示导航坐标系下的东向、北向和天向，w(t)=[w_ax(t) w_ay(t) w_az(t) w_gx(t) w_gy(t) w_gz(t)]^T为系统高斯白噪声向量且E[w(t)w^T(t)]=Q(t)，其中各个元素均为和时间相关的量，包括三个方向加速度计的随机偏置噪声和三个方向陀螺仪的随机漂移噪声，下标中的第一个值a和g分别代表加速度计和陀螺仪，第二个值x、y和z分别代表载体坐标系的右方、前方和上方；

系统状态转移阵中的F_5×5和I_5×1表达式分别如下：

$F_{5\×5}=\left[\begin{array}{ccccc}0& {2\mathrm{\Ω}}_U& 0& -g& 0\\ {-2\mathrm{\Ω}}_U& 0& g& 0& 0\\ 0& 0& 0& {\mathrm{\Ω}}_U& {-\mathrm{\Ω}}_N\\ 0& 0& {-\mathrm{\Ω}}_U& 0& 0\\ 0& 0& {\mathrm{\Ω}}_N& 0& 0\end{array}\right]$

式中，Ω=[0 ω_iecosL ω_iesinL]^T=[0 Ω_NΩ_U]^T为地球自转角速度ω_ie在东向、北向和天向上的投影，L表示当地纬度；g为当地的重力加速度，I_5×1=[0 0 0 1 0]^T；

G(t)为系统噪声转移矩阵，其表达式为：

$G\left(t\right)=\left[\begin{array}{cc}{\left(C_b^{n^{\′}}\right)}_{2\×3}& 0_{2\×3}\\ 0_{\text{3\×3}}& C_b^{n^{\′}}\\ 0_{1\×3}& 0_{1\×3}\end{array}\right]$

式中，为载体坐标系b与计算地理坐标系n'之间的欧拉角转换矩阵，

为

矩阵的前两行元素组成的矩阵；

初始标定的量测方程如下：

选取两个水平速度误差δv_E和δv_N为观测量，建立系统的量测方程为：

$z\left(t\right)=\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×4}\end{array}\right]\left[\begin{array}{c}{x}_1\left(t\right)\\ x_2\left(t\right)\end{array}\right]+v\left(t\right)$

式中，量测向量为Z(t)=[δv_E(t) δv_N(t)]^T，量测噪声向量为 $v\left(t\right)={\left[\begin{array}{cc}{v}_{{\mathrm{\δv}}_E}\left(t\right)& v_{{\mathrm{\δv}}_N}\left(t\right)\end{array}\right]}^T$ 为高斯白噪声，且E[v(t)v^T(t)]=R(t)，包括东向和北向速度量测噪声；

离散系统模型的方程为：

$\begin{array}{c}{x}_k={\mathrm{\Φ}}_{k/k-1}{x}_{k-1}+{\mathrm{\Γ}}_{k-1}{w}_{k-1}\\ z_k=H_k{x}_k+v_k\end{array}$

式中，Φ_k/k-1和Γ_k-1分别为状态转移矩阵和系统噪声转移阵的离散化形式，H_k为量测阵的离散化形式，过程噪声和量测噪声的方差分别为E[w(k)w^T(k)]=Q(k)和E[v(k)v^T(k)]=R(k)。

10.根据权利要求1至7任一项所述的捷联惯性导航系统的初始标定方法，其特征在于，

量化后的备选结果的初始化状态值如下：

${\hat{x}}_0\left(i\right)={\left[\begin{array}{cc}{0}_{1\×5}& \mathrm{\ϵ}\left(i\right)\end{array}\right]}^T(i=\mathrm{1,2},...,n)$

式中，

为备选结果的初始化状态值，i(i=1,2,…,n)为确定备选结果的个数，ε(i)为量化得到的多个确定的备选结果；

初始化各滤波器概率如下：

$P_0\left(i\right)=\frac{1}{n}(i=\mathrm{1,2},...,n)$

所述北向陀螺仪常值漂移的量测量如下：

$z_k={\left[\begin{array}{cc}{\text{\δv}}_E& {\mathrm{\δv}}_N\end{array}\right]}^T$

式中，δv_Eδv_N为根据初始标定的状态方程两个水平速度误差选取的观测量；

所述状态一步预测如下：

${\hat{x}}_{k/k-1}\left(i\right)={\mathrm{\Φ}}_{k/k-1}{\hat{x}}_{k-1}\left(i\right)(i=\mathrm{1,2},...,n)$

所述一步预测均方误差阵如下：

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

式中，Φ_k/k-1和Γ_k-1分别为离散系统模型的状态转移矩阵和系统噪声转移阵，

为初始化状态值，Q_k-1为过程噪声方差矩阵，P_k-1为系统协方差矩阵；

所述新息方差如下：

$U_k=H_k{P}_{k/k-1}{H}_k^T+R_k$

式中，H_k为离散化的量测矩阵，P_k/k-1为一步预测均方误差阵，R_k为根据陀螺仪、加速度计噪声水平和量测量确定的量测噪声矩阵；

所述卡尔曼滤波增益如下：

$K_k=p_{k/k-1}{H}_k^T{U_k}^{-1}$

所述估计均方误差如下：

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

所述各个卡尔曼滤波器的新息序列如下：

$J_k\left(i\right)=z_k-H_k{\hat{x}}_{k/k-1}(i=\mathrm{1,2},..n)$

所述各个卡尔曼滤波器的状态估计值如下：

${\hat{x}}_k\left(i\right)={\hat{x}}_{k/k-1}\left(i\right)+K_k{J}_k\left(i\right)(i=\mathrm{1,2},...,n)$

所述各个卡尔曼滤波器的新息概率如下：

$p_k\left(i\right)={\frac{1}{2\mathrm{\π}\sqrt{\left|U_k\right|}}e}^{-\frac{J_k{\left(i\right)}^{\′{\left(U_k\right)}^{-1}{J}_k\left(i\right)}}{2}(i=1,2,...,n)}$

所述各个卡尔曼滤波器的概率的更新如下：

$P_k{\left(i\right)}^{\′}=\frac{p_k\left(i\right)P_{k-1}\left(i\right)}{\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{p}_k\left(j\right)P_{k-1}\left(j\right)}$

式中，P_k(i)'为根据卡尔曼滤波器的初始概率P₀(i)对进行P_k(i)更新后的各个卡尔曼滤波器的概率；

所述北向陀螺仪常值漂移误差的估计值如下：

${\hat{x}}_k=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_k{\left(i\right)}^{\′}{\hat{x}}_k\left(i\right)$

式中，

中第六个元素为北向陀螺仪常值漂移误差的估计值。

Images