CN103630134A - 一种基于2d-hmm/kf预滤波器的系泊对准方法 - Google Patents

Abstract

本发明公开了一种基于2D-HMM/KF预滤波器的系泊对准方法，步骤包括：将光纤陀螺捷联惯导系统安装于载体上，对系统进行预热，然后采集光纤陀螺和加速度计输出数据；构建惯性器件2D-HMM模型并结合卡尔曼滤波设计2D-HMM/KF滤波器；将采集到的光纤陀螺和加速度计数据通过滤波器进行预处理；设计基于惯性系的系泊对准方法，提取出惯性系下低频的重力矢量信息；将滤波后的光纤陀螺和加速度计数据输入至惯性系对准算法计算出载体姿态，完成初始对准过程。本发明可以有效降低系泊环境下载体受到的高频噪声，增强了系统的抗干扰能力，在不需要外界信息辅助的情况下实现快速精确的系泊自对准。

Description

一种基于2D-HMM/KF预滤波器的系泊对准方法

技术领域：

本发明属于惯性技术领域，具体的说是一种基于2D-HMM/KF(二维隐式马尔科夫模型/卡尔曼滤波)预滤波器的系泊对准方法。

背景技术：

初始对准是捷联惯导系统进入导航之前必不可少的过程，它的两个重要指标是精确性和快速性。初始对准误差是惯性导航系统主要的误差源之一，因此，在导航前得到精确的初始姿态尤为重要。

初始对准按对准时对外界信息需求可分为自主式对准和非自主式。在环境干扰不大的准静基座对准环境下，传统解析式自对准可以得到较为满意的结果。而对于诸如舰船在风浪作用下作大幅度角运动的对准环境，由于摇摆引起的干扰角速度远大于地球自转角速度，无法从陀螺输出中提取地球自转角速度这一有用信息，导致解析式自对准方法不可用。非自主式对准是利用其它导航设备(如GPS、计程仪等)提供的信息(如位置和速度等)作为观测信息，通过滤波实现精确对准，但该方法对外界信息要求较高，且不具有自主性。专利公开号为CN102620729A的文件中，公开了一种“机抖激光陀螺惯性测量单元数字滤波器设计方法”，直接设计低通滤波器对原始信息进行滤波，但该方法需要根据干扰特性设计滤波器，且存在较大的滤波延迟。

发明内容：

本发明的目的在于克服现有技术的不足，提供一种基于2D-HMM/KF预滤波器的系泊对准方法。

为了解决背景技术所存在的问题，本发明采用以下技术方案：

一种基于2D-HMM/KF预滤波器的系泊对准方法，它包括如下步骤：

步骤一：将光纤陀螺捷联惯导系统安装于载体上，对系统进行预热，然后采集光纤陀螺和加速度计输出数据；

步骤二：构建陨性器件2D-HMM模型并结合卡尔曼滤波设计2D-HMM/KF滤波器；

步骤三：将采集到的光纤陀螺和加速度计数据通过滤波器进行预处理；

步骤四：设计基于惯性系的系泊对准方法，提取出惯性系下低频的重力矢量信息；

步骤五：将滤波后的光纤陀螺和加速度计数据输入至惯性系对准算法计算出载体姿态，完成初始对准过程。

进一步地，在所述步骤二中，惯性器件2D-HMM模型可表示为：

$\left\{\begin{array}{c}{X}_{k+1}={\mathrm{FX}}_k+{\mathrm{\ξ}}_k\\ Z_{k+1}={\mathrm{HX}}_k+v_k\end{array}\right.$

其中，X为状态向量，Z为观测向量，F为状态转移矩阵，H为观测矩阵，ξ_k为系统噪声向量，v_k为量测噪声向量，X∈R^N，Z∈R^M，

且F_ij，H_ij＞0；

依据惯性器件的输出特性，状态模型和观测模型可以表示成以下形式：

$\left[\begin{array}{c}{x}_{k+1}\\ x_k\end{array}\right]=\left[\begin{array}{cc}{F}_{11}& F_{12}\\ F_{21}& F_{22}\end{array}\right]\left[\begin{array}{c}{x}_k\\ x_{k-1}\end{array}\right]+{\mathrm{\ξ}}_k$

$\left[\begin{array}{c}{z}_{k+1}\\ z_k\end{array}\right]=\left[\begin{array}{cc}{H}_{11}& H_{12}\\ H_{21}& H_{22}\end{array}\right]\left[\begin{array}{c}{x}_k\\ x_{k-1}\end{array}\right]+v_k$

式中，状态变量x为惯性器件的滤波估值，观测量z表示为惯性器件的原始输出，系统噪声ξ_k和量测噪声v_k皆为零均值的白噪声，满足：

$\left\{\begin{array}{c}E\left[{\mathrm{\ξ}}_k\right]=0,\mathrm{Cov}[{\mathrm{\ξ}}_k,{\mathrm{\ξ}}_j]=E\left[{\mathrm{\ξ}}_k{\mathrm{\ξ}}_j^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[v_k\right]=0,\mathrm{Cov}[v_k,v_j]=E\left[v_k{v}_j^T\right]=R_k{\mathrm{\δ}}_{\mathrm{kj}}\\ \mathrm{Cov}\left[{\mathrm{\ξ}}_k{v}_j\right]=E\left[{\mathrm{\ξ}}_k{v}_j^T\right]=0\end{array}\right..$

进一步地，在所述步骤二中，2D-HMM／KF滤波器的滤波方程为：

$\left\{\begin{array}{c}{\hat{X}}_{k/k-1}=F_{k,k-1}{\hat{X}}_{k-1}\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_{\mathrm{off}}(Z_k-H_k{\hat{X}}_{k/k-1})\end{array}\right.$

其中，

分别为k-1和k时刻滤波估值，

为k时刻预测状态向量，Z_k为k时刻观测值，F_k，k-1为一步转移矩阵，H_k为量测阵，K_off为滤波增益；

K_off的计算过程如下：

$\left\{\begin{array}{c}{P}_{k-1}^{-1}=P_{k-1/k-2}^{-1}+H_{k-1}^T{R}_{k-1}^{-1}{H}_{k-1}\\ P_{k/k-1}=F_{k/k-1}{P}_{k-1}{F}_{k/k-1}^T+Q_{k-1}\\ K_{\mathrm{off}}=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\end{array}\right.$

其中，P_k为估计均方误差，P_k／k-1为一步预测均方误差，Q_k系统噪声方差阵，R_k为量测噪声方差阵。

进一步地，在所述步骤四中，

选取东北天(ENU)地理坐标系为导航坐标系(n系)，定义初始时刻t₀时刻的载体坐标系为载体惯性坐标系i_b0系；

载体姿态矩阵可由下式计算得到：

$C_b^n=C_i^n{C}_{i_{b0}}^i{C}_b^{i_{b0}}$

其中，b系为载体坐标系，f系为惯性系，为载体坐标系到载体惯性坐标系的转换矩阵，可由陀螺输出利用四元素法实时更新得到：

${\stackrel{\·}{C}}_b^{i_{b0}}=C_b^{i_{b0}}{\mathrm{\Ω}}_{i_{b0}b}^b$

式中，

的反对称矩阵，为载体坐标系相对于载体惯性坐标系的角速度在载体系下的投影；

矩阵

可根据载体所在位置的经纬度信息以及时间间隔计算得到：

其中，λ为当地经度，

为当地纬度，ω_ie为地球自转角速率，Δt为时间间隔；

的计算方法如下：

建立[t₀，t_k1]和[t₀，t_k2]两个区间内重力矢量的积分

和

作为参考矢量建立i_b0系与f系之间的转换关系：

$C_i^{i_{b0}}={\left[\begin{array}{c}{\left(V_{t_{k1}}^i\right)}^T\\ {\left(V_{t_{k2}}^i\right)}^T\\ {(V_{t_{k1}}^i\×V_{t_{k2}}^i)}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left(\widehat{V_{t_{k1}}^{i_{b0}}}\right)}^T\\ {\left(\widehat{V_{t_{k2}}^{i_{b0}}}\right)}^T\\ {(\widehat{V_{t_{k1}}^{i_{b0}}}\×\widehat{V_{t_{k2}}^{i_{b0}}})}^T\end{array}\right]$

其中，重力矢量积分Vⁱ和

可以表示为：

$\left\{\begin{array}{c}{V}_{t_k}^i={\∫}_{t_0}^{t_k}{g}^i\mathrm{dt}=C_n^i{\∫}_{t_0}^{t_k}{g}^n\mathrm{dt}\\ {\hat{V}}_{t_k}^{i_{b0}}={\∫}_{t_0}^{t_k}{C}_b^{i_{b0}}{\tilde{f}}^b\mathrm{dt}=C_i^{i_{b0}}{\∫}_{t_0}^{t_k}{C}_n^i{g}^n\mathrm{dt}+{\∫}_{t_0}^{t_k}{C}_b^{i_{b0}}{\mathrm{\δ\α}}^b\mathrm{dt}\end{array}\right.$

其中，gⁿ为导航系下的重力矢量，δα^b为载体晃动引起的周期性干扰线加速度。

进一步地，在所述步骤五中，

通过步骤四获得载体姿态矩阵

将姿态矩阵表示成以下形式：

$C_b^n=\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]$

则根据可以得到载体姿态，即纵摇角θ、横摇角γ和航向角ψ的主值如下：

θ_主=sin^-1(C₃₂)

根据纵摇角(-90°，90°)、横摇角(-90°，90°)和航向角(-180°，180°)的定义域限制，由主值确定真值的公式如下：

θ=θ_主

γ=γ_主

本发明对比现有技术有如下的有益效果：本发明可以有效降低系泊环境下载体受到的高频噪声，增强了系统的抗干扰能力，在不需要外界信息辅助的情况下实现快速精确的系泊自对准。

附图说明：

图1为本发明的流程图。

图2为滤波前后陀螺输出对比。

图3为滤波前后加速度计输出对比。

图4为东向水平失准角对比曲线。

图5为北向水平失准角对比曲线。

图6为方位失准角对比曲线。

具体实施方式：

下面结合附图和具体实施方式对本发明作进一步描述：

图1为本发明的流程图。

一种基于2D-HMM/KF预滤波器的系泊对准方法，其特征在于，它由以下步骤实现：

步骤一：将光纤陀螺捷联惯导系统安装于载体上，对系统进行预热，然后采集光纤陀螺和加速度计输出数据；

步骤二：构建惯性器件2D-HMM模型并结合卡尔曼滤波设计2D-HMM/KF滤波器；

步骤三：将采集到的光纤陀螺和加速度计数据通过滤波器进行预处理；

步骤四：设计基于惯性系的系泊对准方法，提取出惯性系下低频的重力矢量信息；

步骤五：将滤波后的光纤陀螺和加速度计数据输入至惯性系对准算法计算出载体姿态，完成初始对准过程。

进一步地，在所述步骤二中，惯性器件2D-HMM模型可表示为：

$\left\{\begin{array}{c}{X}_{k+1}={\mathrm{FX}}_k+{\mathrm{\ξ}}_k\\ Z_{k+1}={\mathrm{HX}}_k+v_k\end{array}\right.$

其中，X为状态向量，Z为观测向量，F为状态转移矩阵，H为观测矩阵，ξ_k为系统噪声向量，v_k为量测噪声向量，X∈R^N，Z∈R^M，

且F_ij，H_ij＞0；

依据惯性器件的输出特性，状态模型和观测模型可以表示成以下形式：

$\left[\begin{array}{c}{x}_{k+1}\\ x_k\end{array}\right]=\left[\begin{array}{cc}{F}_{11}& F_{12}\\ F_{21}& F_{22}\end{array}\right]\left[\begin{array}{c}{x}_k\\ x_{k-1}\end{array}\right]+{\mathrm{\ξ}}_k$

$\left[\begin{array}{c}{z}_{k+1}\\ z_k\end{array}\right]=\left[\begin{array}{cc}{H}_{11}& H_{12}\\ H_{21}& H_{22}\end{array}\right]\left[\begin{array}{c}{x}_k\\ x_{k-1}\end{array}\right]+v_k$

式中，状态变量x为惯性器件的滤波估值，观测量z表示为惯性器件的原始输出，系统噪声ξ_k和量测噪声v_k皆为零均值的白噪声，满足：

$\left\{\begin{array}{c}E\left[{\mathrm{\ξ}}_k\right]=0,\mathrm{Cov}[{\mathrm{\ξ}}_k,{\mathrm{\ξ}}_j]=E\left[{\mathrm{\ξ}}_k{\mathrm{\ξ}}_j^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[v_k\right]=0,\mathrm{Cov}[v_k,v_j]=E\left[v_k{v}_j^T\right]=R_k{\mathrm{\δ}}_{\mathrm{kj}}\\ \mathrm{Cov}\left[{\mathrm{\ξ}}_k{v}_j\right]=E\left[{\mathrm{\ξ}}_k{v}_j^T\right]=0\end{array}\right..$

进一步地，在所述步骤二中，2D-HMM/KF滤波器的滤波方程为：

$\left\{\begin{array}{c}{\hat{X}}_{k/k-1}=F_{k,k-1}{\hat{X}}_{k-1}\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_{\mathrm{off}}(Z_k-H_k{\hat{X}}_{k/k-1})\end{array}\right.$

其中，分别为k-1和k时刻滤波估值，

为k时刻预测状态向量，Z_k为k时刻观测值，F_k，k-1为一步转移矩阵，H_k为量测阵，K_off为滤波增益；

K_off的计算过程如下：

$\left\{\begin{array}{c}{P}_{k-1}^{-1}=P_{k-1/k-2}^{-1}+H_{k-1}^T{R}_{k-1}^{-1}{H}_{k-1}\\ P_{k/k-1}=F_{k/k-1}{P}_{k-1}{F}_{k/k-1}^T+Q_{k-1}\\ K_{\mathrm{off}}=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\end{array}\right.$

其中，P_k为估计均方误差，P_k/k-1为一步预测均方误差，Q_k系统噪声方差阵，R_k为量测噪声方差阵。

进一步地，在所述步骤四中，

选取东北天(ENU)地理坐标系为导航坐标系(n系)，定义初始时刻t₀时刻的载体坐标系为载体惯性坐标系i_b0系；

载体姿态矩阵可由下式计算得到：

$C_b^n=C_i^n{C}_{i_{b0}}^i{C}_b^{i_{b0}}$

其中，b系为载体坐标系，f系为惯性系，

为载体坐标系到载体惯性坐标系的转换矩阵，可由陀螺输出利用四元素法实时更新得到：

${\stackrel{\·}{C}}_b^{i_{b0}}=C_b^{i_{b0}}{\mathrm{\Ω}}_{i_{b0}b}^b$

式中，的反对称矩阵，

为载体坐标系相对于载体惯性坐标系的角速度在载体系下的投影；

矩阵

可根据载体所在位置的经纬度信息以及时间间隔计算得到：

其中，λ为当地经度，

为当地纬度，ω_ie为地球自转角速率，Δt为时间间隔；

的计算方法如下：

建立[t₀，t_k1]和[t₀，t_k2]两个区间内重力矢量的积分

和

作为参考矢量建立i_b0系与i系之间的转换关系：

$C_i^{i_{b0}}={\left[\begin{array}{c}{\left(V_{t_{k1}}^i\right)}^T\\ {\left(V_{t_{k2}}^i\right)}^T\\ {(V_{t_{k1}}^i\×V_{t_{k2}}^i)}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left(\widehat{V_{t_{k1}}^{i_{b0}}}\right)}^T\\ {\left(\widehat{V_{t_{k2}}^{i_{b0}}}\right)}^T\\ {(\widehat{V_{t_{k1}}^{i_{b0}}}\×\widehat{V_{t_{k2}}^{i_{b0}}})}^T\end{array}\right]$

其中，重力矢量积分Vⁱ和可以表示为：

$\left\{\begin{array}{c}{V}_{t_k}^i={\∫}_{t_0}^{t_k}{g}^i\mathrm{dt}=C_n^i{\∫}_{t_0}^{t_k}{g}^n\mathrm{dt}\\ \widehat{V_{t_k}^{i_{b0}}}={\∫}_{t_0}^{t_k}{C}_b^{i_{b0}}{\tilde{f}}^b\mathrm{dt}=C_i^{i_{b0}}{\∫}_{t_0}^{t_k}{C}_n^i{g}^n\mathrm{dt}+{\∫}_{t_0}^{t_k}{C}_b^{i_{b0}}{\mathrm{\δ\α}}^b\mathrm{dt}\end{array}\right.$

其中，gⁿ为导航系下的重力矢量，δα^b为载体晃动引起的周期性干扰线加速度。

进一步地，在所述步骤五中，

通过步骤四获得载体姿态矩阵

将姿态矩阵表示成以下形式：

$C_b^n=\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]$

则根据

可以得到载体姿态，即纵摇角θ、横摇角γ和航向角ψ的主值如下：

θ_主=sin^-1(C₃₂)

根据纵摇角(-90°，90°)、横摇角(-90°，90°)和航向角(-180°，180°)的定义域限制，由主值确定真值的公式如下：

θ=θ_主

γ=γ_主

结合以下实验对本发明的优益效果作进一步的说明：

(1)为了确切描述引入2D-HMM/KF滤波器后对惯性器件输出的改善，选取一组代表性的转台试验数据进行对比分析。设计加速度计和陀螺HMMKF滤波器，两个滤波器的系统矩阵和量测矩阵设置相同，分别设为：

$F_{k,k-1}=\left[\begin{array}{cc}1& 0\\ 0& 0\end{array}\right],H_k=\left[\begin{array}{cc}1& 0\\ 0& 0\end{array}\right]$

加速度计和陀螺的滤波参数分别记为Q_a，R_a和Q_g，R_g，设置如下：

Q_a=diag{5e^-4g 5e^-4g}²，R_α=diag{100e^-4g 100e^-4g}²

Q_g=diag{0.005°/h 0.005°/h}²，R_g=diag{0.1°/h 0.1°/h}²

滤波前后陀螺仪和加速度计输出对比曲线如图2和图3所示。结果可以看出陀螺和加速度计原始输出的标准方差分别为18.73°/h、1.67×10^-3m/s²。经过2D-HMM/KF滤波后陀螺和加速度计的输出标准方差分别为3.79°/h、1.79×10^-4m/s²。可见，2D-HMM/KF滤波能快速有效地滤除对准过程以及器件本身引起的高频噪声的干扰。

(2)为了验证系泊环境下该方法的对准效果，在某地进行多次系泊对准实验。试验时将法国IXSEA公司的PHINS和自研光纤陀螺SINS固定在同一钢板上，以PHINS与GPS组合作为姿态基准，将光纤捷联系统解算出的姿态信息与其进行对比。使用时先将PHINS与GPS做组合对准，然后PHINS进入导航状态，PHINS与GPS组合的水平精度为0.01度，方位精度为0.02度。且对比引入2D-HMM/KF前后的惯性系对准的效果，结果如图4至图6所示。

由结果可得，有无2D-HMM/KF的多次水平对准曲线大致吻合，但方位误差相差较大，未加入2D-HMM/KF的惯性系对准方法的对准误差波动较大，加入滤波器后的对准方法在维持较高的水平对准精度的前提下，滤除了对准环境下的噪声，将方位精度从原有的0.0658°提高到0.0446°，且能维持更好的稳定性。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于2D-HMM/KF预滤波器的系泊对准方法，其特征在于，它由以下步骤实现：

步骤一：将光纤陀螺捷联惯导系统安装于载体上，对系统进行预热，然后采集光纤陀螺和加速度计输出数据；

步骤二：构建惯性器件2D-HMM模型并结合卡尔曼滤波设计2D-HMM/KF滤波器；

步骤三：将采集到的光纤陀螺和加速度计数据通过滤波器进行预处理；

步骤四：设计基于惯性系的系泊对准方法，提取出惯性系下低频的重力矢量信息；

步骤五：将滤波后的光纤陀螺和加速度计数据输入至惯性系对准算法计算出载体姿态，完成初始对准过程。

2.如权利要求1所述的一种基于2D-HMM/KF预滤波器的系泊对准方法，其特征在于，在所述步骤二中，惯性器件2D-HMM模型可表示为：

$\left\{\begin{array}{c}{X}_{k+1}={\mathrm{FX}}_k+{\mathrm{\ξ}}_k\\ Z_{k+1}={\mathrm{HX}}_k+v_k\end{array}\right.$

其中，X为状态向量，Z为观测向量，F为状态转移矩阵，H为观测矩阵，ξ_k为系统噪声向量，v_k为量测噪声向量，X∈R^N，Z∈R^M， $\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{F}_{\mathrm{ij}}=1,$ $\underset{i=1}{\overset{M}{\mathrm{\Σ}}}\underset{j=1}{\overset{M}{\mathrm{\Σ}}}{H}_{\mathrm{ij}}=1,$ 且F_ij，H_ij＞0；

依据惯性器件的输出特征，状态模型和观测模型可以表示成以下形式：

$\left[\begin{array}{c}{x}_{k+1}\\ x_k\end{array}\right]=\left[\begin{array}{cc}{F}_{11}& F_{12}\\ F_{21}& F_{22}\end{array}\right]\left[\begin{array}{c}{x}_k\\ x_{k-1}\end{array}\right]+{\mathrm{\ξ}}_k$

$\left[\begin{array}{c}{z}_{k+1}\\ z_k\end{array}\right]=\left[\begin{array}{cc}{H}_{11}& H_{12}\\ H_{21}& H_{22}\end{array}\right]\left[\begin{array}{c}{x}_k\\ x_{k-1}\end{array}\right]+v_k$

式中，状态变量x为惯性器件的滤波估值，观测量z表示为惯性器件的原始输出，系统噪声ξ_k和量测噪声v_k皆为零均值的白噪声，满足：

$\left\{\begin{array}{c}E\left[{\mathrm{\ξ}}_k\right]=0,\mathrm{Cov}[{\mathrm{\ξ}}_k,{\mathrm{\ξ}}_j]=E\left[{\mathrm{\ξ}}_k{\mathrm{\ξ}}_j^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[v_k\right]=0,\mathrm{Cov}[v_k,v_j]=E\left[v_k{v}_j^T\right]=R_k{\mathrm{\δ}}_{\mathrm{kj}}\\ \mathrm{Cov}\left[{\mathrm{\ξ}}_k{v}_j\right]=E\left[{\mathrm{\ξ}}_k{v}_j^T\right]=0\end{array}\right..$

3.如权利要求1所述的一种基于2D-HMM/KF的捷联惯导系统系泊对准方法，其特征在于，在所述步骤二中，2D-HMM/KF滤波器的滤

波方程为： $\left\{\begin{array}{c}{\hat{X}}_{k/k-1}=F_{k,k-1}{\hat{X}}_{k-1}\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_{\mathrm{off}}(Z_k-H_k{\hat{X}}_{k/k-1})\end{array}\right.$

其中，分别为k-1和k时刻滤波估值，

为k时刻预测状态向量，Z_k为k时刻观测值，F_k，k-1为一步转移矩阵，H_k为量测阵，K_off为滤波增益；

K_off的计算过程如下：

$\left\{\begin{array}{c}{P}_{k-1}^{-1}=P_{k-1/k-2}^{-1}+H_{k-1}^T{R}_{k-1}^{-1}{H}_{k-1}\\ P_{k/k-1}=F_{k/k-1}{P}_{k-1}{F}_{k/k-1}^T+Q_{k-1}\\ K_{\mathrm{off}}=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\end{array}\right.$

其中，P_k为估计均方误差，P_k/k-1为一步预测均方误差，Q_k系统噪声方差阵，R_k为量测噪声方差阵。

4.如权利要求1所述的一种基于2D-HMM/KF的捷联惯导系统系泊对准方法，其特征在于，在所述步骤四中，

选取东北天(ENU)地理坐标系为导航坐标系(n系)，定义初始时刻t₀时刻的载体坐标系为载体惯性坐标系i_b0系；

载体姿态矩阵可由下式计算得到：

$C_b^n=C_i^n{C}_{i_{b0}}^i{C}_b^{i_{b0}}$

其中，b系为载体坐标系，f系为惯性系，

为载体坐标系到载体惯性坐标系的转换矩阵，可由陀螺输出利用四元素法实时更新得到：

${\stackrel{\·}{C}}_b^{i_{b0}}=C_b^{i_{b0}}{\mathrm{\Ω}}_{i_{b0}b}^b$

式中，

的反对称矩阵，

为载体坐标系相对于载体惯性坐标系的角速度在载体系下的投影；

矩阵

可根据载体所在位置的经纬度信息以及时间间隔计算得到：

其中，λ为当地经度，为当地纬度，ω_ie为地球自转角速率，Δt为时间间隔；

的计算方法如下：

建立[t₀，t_k1]和[t₀，t_k2]两个区间内重力矢量的积分

和

作为参考矢量建立i_b0系与i系之间的转换关系：

$C_i^{i_{b0}}={\left[\begin{array}{c}{\left(V_{t_{k1}}^i\right)}^T\\ {\left(V_{t_{k2}}^i\right)}^T\\ {(V_{t_{k1}}^i\×V_{t_{k2}}^i)}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left({\hat{V}}_{t_{k1}}^{i_{b0}}\right)}^T\\ {\left({\hat{V}}_{t_{k2}}^{i_{b0}}\right)}^T\\ {({\hat{V}}_{t_{k1}}^{i_{b0}}\×{\hat{V}}_{t_{k2}}^{i_{b0}})}^T\end{array}\right]$

其中，重力矢量积分Vⁱ和

可以表示为：

$\left\{\begin{array}{c}{V}_{t_k}^i={\∫}_{t_0}^{t_k}{g}^i\mathrm{dt}=g{C}_n^i{\∫}_{t_0}^{t_k}{g}^n\mathrm{dt}\\ {\hat{V}}_{t_k}^{i_{b0}}={\∫}_{t_0}^{t_k}{\tilde{f}}^b\mathrm{dt}=C_i^{i_{b0}}{\∫}_{t_0}^{t_k}{C}_n^i{g}^n\mathrm{dt}+{\∫}_{t_0}^{t_k}{C}_b^{i_{b0}}{\mathrm{\δa}}^b\mathrm{dt}\end{array}\right.$

其中，gⁿ为导航系下的重力矢量，δα^b为载体晃动引起的周期性干扰线加速度。

5.如权利要求1所述的一种基于2D-HMM/KF的捷联惯导系统系泊对准方法，其特征在于，在所述步骤五中，

通过步骤四获得载体姿态矩阵将姿态矩阵表示成以下形式：

$C_b^n=\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]$

则根据

可以得到载体姿态，即纵摇角θ、横摇角γ和航向角ψ的主值如下：

θ_主=sin^-1(C₃₂)

根据纵摇角(-90°，90°)、横摇角(-90°，90°)和航向角(-180°，180°)的定义域限制，由主值确定真值的公式如下：

θ=θ_主γ=γ_主

Images