CN102305635B - 一种光纤捷联罗经***的对准方法 - Google Patents

Abstract

本发明提供的是一种光纤捷联罗经***的对准方法。步骤包括：根据基于双积分重力矢量的并行解析粗对准算法，利用光纤捷联罗经***采集的光纤陀螺和石英加速度计数据，以及地球自转角速率、重力加速度、纬度等信息，完成光纤捷联罗经***的粗对准；根据基于比力的精对准方法，建立以失准角为***状态，以比力信息为***量测的光纤捷联罗经***精对准的卡尔曼滤波模型；利用滤波估计的失准角信息闭环修正姿态矩阵，完成光纤捷联罗经***的精对准。本发明的方法具有如下优点：(1)粗对准算法通过重力矢量的双积分运算和并行融合运算，对***随机误差进行了平滑抑制，提高了粗对准的精度和可靠性；(2)精对准算法的滤波模型简单。

Description

一种光纤捷联罗经***的对准方法

技术领域

本发明涉及一种对载体航向和姿态的测量技术，尤其涉及一种光纤捷联罗经***的自主对准方法。

背景技术

随着光纤陀螺技术的迅速发展，光纤捷联罗经已经成为国内外的研究热点。与传统陀螺罗经相比，光纤捷联罗经具有全固态、体积小、启动快、可靠性高等优点。LITEF公司的LFK-95型光纤捷联罗经的对准时间30min，航向精度为0.7°secL，水平精度为0.5°，法国IXSEA公司的OCTANS光纤捷联罗经能够在5min内完成对准，航向精度达到0.1°secL，水平精度优于0.01°(RMS)。目前，快速精确的自主对准技术是光纤捷联罗经***的一项关键技术。

初始对准一般分为粗对准和精对准两个过程，粗对准要求在很短时间内将姿态误差降低到几度之内，精对准在粗对准基础上实现精确对准。由于载体工作环境如基座持续摇摆、风浪干扰、机械振动等影响，传统解析对准算法往往难以满足实际应用需求。因此，研究晃动基座下自主对准具有重要意义和应用需求。

目前也有部分与本发明有关的研究报告，例如：1、舰载机捷联惯导自对准方案设计与仿真，中国惯性技术学报，2008，16(1)；2、专利申请号为200710144677.2，名称为“大失准角下船用光纤陀螺捷联航姿***系泊精对准方法”。

发明内容

本发明针对现有技术的不足，提出一种基于双积分重力矢量的并行解析粗对准方法和基于比力信息的精对准方法。该方法可实现光纤捷联罗经***在晃动基座下自主对准。本发明的目的是这样实现的：

本发明包括下列步骤：

步骤1定义坐标系。光纤捷联罗经***以东北天坐标系为导航坐标系n系；载体坐标系b系以载体中心为原点，x轴沿横轴指向右，y轴沿纵轴指向前，z轴垂直载体指向上；经线地球坐标系e系以地球中心为原点，并与地球固连，x、y轴在地球赤道平面内，x轴指向载体所在点经线，z轴指向地球自转轴方向；经线地心惯性坐标系i系定义为在粗对准起始时刻将地球坐标系惯性凝固成的右手坐标系；载体惯性坐标系i_b0系定义为在粗对准起始时刻将载体坐标系惯性凝固后的坐标系。

步骤2根据

和f^b，以及地球自转角速率ω_ie、重力加速度g、纬度L等信息，应用基于并行双积分重力矢量信息的粗对准算法，完成光纤捷联罗经***的粗对准，得到初始的姿态矩阵

步骤3根据

和f^b，以及地球自转角速率ω_ie、重力加速度g、纬度L等信息，应用基于并行双积分重力矢量信息的粗对准算法，完成光纤捷联罗经***的粗对准，得到初始的姿态矩阵

在不同起始时刻并行运行r个基于双积分重力矢量的姿态矩阵计算过程，在最后同一时刻获得r个姿态矩阵值，对r个姿态矩阵值进行融合以提高粗对准结果的可靠性，并通过所述的初始姿态矩阵

提取载体的方位角H、纵摇角P和横摇角R以完成粗对准。所述的初始姿态矩阵为

$C_n^c\left(t\right)=\frac{1}{r}\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\left[{\tilde{C}}_n^b\left(t\right)\right]}_i,$ i＝1，2，…，r

其中，为r个并行运算中的第i个中间姿态矩阵

进行正交化处理的结果，即：

${\tilde{C}}_n^b\left(t\right)=C_n^b\left(t\right){\left[{\left(C_n^b\left(t\right)\right)}^T{C}_n^b\left(t\right)\right]}^{-1/2}$

其中，

为中间姿态矩阵，并且， $C_n^b\left(t\right)=C_{i_{\mathrm{bo}}}^b\left(t\right)\·{\tilde{C}}_i^{i_{\mathrm{bo}}}\·C_e^i\left(t\right)\·C_n^e$

步骤3.1计算导航坐标系与经线地球坐标系之间的转移矩阵

$C_n^e=\left[\begin{array}{ccc}0& -\sin L& \cos L\\ 1& 0& 0\\ 0& \cos L& \sin L\end{array}\right]$

步骤3.2计算经线地球坐标系与惯性坐标系之间的转移矩阵

$C_e^i\left(t\right)=\left[\begin{array}{ccc}\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& -\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& 0\\ \sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& \cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& 0\\ 0& 0& 1\end{array}\right]$

步骤3.3计算载体惯性坐标系与载体坐标系之间的转移矩阵

在起始时刻，惯性坐标系与载体坐标系重合，即

的初值为单位阵，根据陀螺仪输出

通过四元数方法求解

步骤3.4计算惯性坐标系与载体惯性坐标系之间的转移矩阵

${\tilde{C}}_i^{i_{\mathrm{bo}}}=\frac{1}{t_3-t_2}{\∫}_{t_2}^{t_3}{C}_i^{i_{\mathrm{bo}}}\left(\mathrm{\τ}\right)\mathrm{d\τ}$

$=\frac{1}{t_3-t_2}{\∫}_{t_2}^{t_3}\left(\begin{array}{cc}{\left[\begin{array}{c}{\left[V^{i_{b0}}\left(t_1\right)\right]}^T\\ {\left[V^{i_{b0}}\left(\mathrm{\τ}\right)\right]}^T\\ {[V^{i_{b0}}\left(t_1\right)\×V^{i_{b0}}\left(\mathrm{\τ}\right)]}^T\end{array}\right]}^{-1}& \left[\begin{array}{c}{\left[V^i\left(t_1\right)\right]}^T\\ {\left[V^i\left(\mathrm{\τ}\right)\right]}^T\\ {[V^i\left(t_1\right)\×V^i\left(\mathrm{\τ}\right)]}^T\end{array}\right]\end{array}\right)\mathrm{d\τ}$

式中， $V^i\left(t\right)=\left[\begin{array}{c}\frac{g\cos L\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ \frac{g\cos L[1-\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ g\sin L\·t\end{array}\right];$

$V^{i_{b0}}\left(t\right)={\∫}_0^t{f}^{i_{b0}}\left(\mathrm{\τ}\right)\mathrm{d\τ}={\∫}_0^t\left[C_b^{i_{b0}}\left(\mathrm{\τ}\right)f^b\left(\mathrm{\τ}\right)\right]\mathrm{d\τ}={\∫}_0^t\left[{\left(C_{i_{b0}}^b\left(\mathrm{\τ}\right)\right)}^T{f}^b\left(\mathrm{\τ}\right)\right]\mathrm{d\τ}.$

步骤4建立以失准角为***状态，以比力信息为***量测的光纤捷联罗经***精对准的卡尔曼滤波模型。

以三个失准角构造***状态，以比力信息构造***量测，得到滤波模型：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

Y(t)＝H(t)X(t)+V(t)

***状态向量为X＝[φ_e，φ_n，φ_u]^T，***矩阵F(t)为：

$F\left(t\right)=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& {-\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {-\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\end{array}\right]$

系泊情况下，由于忽略了晃动引起的干扰加速度，导航坐标系下的东向比力和北向比力为零，则加速度计的输出在导航坐标系下投影的水平分量即为与失准角耦合信息，***量测Y为：

$Y=\left[\begin{array}{c}\frac{\underset{i=k}{\overset{k+N}{\mathrm{\Σ}}}{\left({\tilde{f}}_e\right)}_i}{N}\\ \frac{\underset{i=k}{\overset{k+N}{\mathrm{\Σ}}}{\left({\tilde{f}}_n\right)}_i}{N}\end{array}\right]$

其中，

和为加速度计在导航坐标系下的投影，N为滤波周期的采样次数。量测矩阵H(t)为：

$H\left(t\right)=\left[\begin{array}{ccc}0& g& 0\\ -g& 0& 0\end{array}\right]$

式中，g为当地的重力加速度值，g＝9.8m/s²。

对滤波模型进行离散化处理，即得到离散卡尔曼滤波模型。

步骤5利用步骤(4)滤波估计的失准角φ_e、φ_n、φ_u闭环修正姿态矩阵

获得新的姿态矩阵

然后通过

提取方位角H、纵摇角P和横摇角R，并完成光纤捷联罗经***的初始对准。

$C_n^b=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_u& {-\mathrm{\φ}}_n\\ {-\mathrm{\φ}}_u& 1& {\mathrm{\φ}}_e\\ {\mathrm{\φ}}_n& {-\mathrm{\φ}}_e& 1\end{array}\right]\·C_n^c.$

本发明包括下列有益结果：

本发明的方法具有如下优点：(1)标准的凝固对准算法对重力矢量进行单积分平滑在一定程度上抑制了基座晃动引起的角运动和线运动扰动以及传感器噪声的影响，但

仍然存在波动，从而导致粗对准结果的离散程度较大。本发明的粗对准算法通过重力矢量的双积分运算和并行融合运算两种技术手段，对***随机误差进行了平滑抑制，减小了

的波动，提高了对准精度和可靠性；(2)精对准算法的***状态维数仅有3维，量测维数仅有2维，滤波模型简单，计算量小。

对以上发明的有益效果说明如下：本实验利用光纤捷联罗经***样机进行摇摆台试验，光纤陀螺的精度为0.05°/h(1σ)，加速度计的精度为1mg(1σ)。并行运算的基于双积分重力矢量解析粗对准时间为5min，精对准的滤波周期为10s，精对准运行20min时统计对准结果，整个对准时间为25min。

表1摇摆台试验结果统计表

附图说明

图1为本发明的算法流程图。

图2为本发明的基于双积分重力矢量的并行解析对准算法时序图。

图3为本发明的双积分重力矢量解析对准算法流程图。

图4为本发明的精对准算法流程图。

图5为本发明的摇摆台试验的方位对准误差曲线图。

具体实施方式

下面举例对本发明做详尽描述：

(1)光纤捷联罗经***开机预热后采集惯性测量组件的输出数据，包括三只光纤陀螺仪的输出数据

和三只石英加速度计的输出数据f^b。

(2)根据

和f^b，以及地球自转角速率ω_ie、重力加速度g、纬度L等信息，应用基于并行双积分重力矢量信息的粗对准算法，完成光纤捷联罗经***的粗对准，得到初始的姿态矩阵

在不同起始时刻并行运行r个基于双积分重力矢量的姿态矩阵计算过程，在最后同一时刻获得r个姿态矩阵值，对r个姿态矩阵值进行融合以提高粗对准结果的可靠性，并通过所述的初始姿态矩阵提取载体的方位角H、纵摇角P和横摇角R以完成粗对准。所述的初始姿态矩阵

为

$C_n^c\left(t\right)=\frac{1}{r}\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\left[{\tilde{C}}_n^b\left(t\right)\right]}_i,$ i＝1，2，…，r

其中，r＝3～5，

为r个并行运算中的第i个中间姿态矩阵

进行正交化处理的结果，即：

${\tilde{C}}_n^b\left(t\right)=C_n^b\left(t\right){\left[{\left(C_n^b\left(t\right)\right)}^T{C}_n^b\left(t\right)\right]}^{-1/2}$

其中，

为中间姿态矩阵，并且， $C_n^b\left(t\right)=C_{i_{\mathrm{bo}}}^b\left(t\right)\·{\tilde{C}}_i^{i_{\mathrm{bo}}}\·C_e^i\left(t\right)\·C_n^e$

①计算导航坐标系与经线地球坐标系之间的转移矩阵

$C_n^e=\left[\begin{array}{ccc}0& -\sin L& \cos L\\ 1& 0& 0\\ 0& \cos L& \sin L\end{array}\right]$

②计算经线地球坐标系与惯性坐标系之间的转移矩阵

$C_e^i\left(t\right)=\left[\begin{array}{ccc}\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& -\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& 0\\ \sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& \cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& 0\\ 0& 0& 1\end{array}\right]$

③计算载体惯性坐标系与载体坐标系之间的转移矩阵

在起始时刻，惯性坐标系与载体坐标系重合，即

的初值为单位阵，根据陀螺仪输出

通过四元数方法求解

④计算惯性坐标系与载体惯性坐标系之间的转移矩阵

${\tilde{C}}_i^{i_{\mathrm{bo}}}=\frac{1}{t_3-t_2}{\∫}_{t_2}^{t_3}{C}_i^{i_{\mathrm{bo}}}\left(\mathrm{\τ}\right)\mathrm{d\τ}$

$=\frac{1}{t_3-t_2}{\∫}_{t_2}^{t_3}\left(\begin{array}{cc}{\left[\begin{array}{c}{\left[V^{i_{b0}}\left(t_1\right)\right]}^T\\ {\left[V^{i_{b0}}\left(\mathrm{\τ}\right)\right]}^T\\ {[V^{i_{b0}}\left(t_1\right)\×V^{i_{b0}}\left(\mathrm{\τ}\right)]}^T\end{array}\right]}^{-1}& \left[\begin{array}{c}{\left[V^i\left(t_1\right)\right]}^T\\ {\left[V^i\left(\mathrm{\τ}\right)\right]}^T\\ {[V^i\left(t_1\right)\×V^i\left(\mathrm{\τ}\right)]}^T\end{array}\right]\end{array}\right)\mathrm{d\τ}$

式中， $V^i\left(t\right)=\left[\begin{array}{c}\frac{g\cos L\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ \frac{g\cos L[1-\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ g\sin L\·t\end{array}\right];$

$V^{i_{b0}}\left(t\right)={\∫}_0^t{f}^{i_{b0}}\left(\mathrm{\τ}\right)\mathrm{d\τ}={\∫}_0^t\left[C_b^{i_{b0}}\left(\mathrm{\τ}\right)f^b\left(\mathrm{\τ}\right)\right]\mathrm{d\τ}={\∫}_0^t\left[{\left(C_{i_{b0}}^b\left(\mathrm{\τ}\right)\right)}^T{f}^b\left(\mathrm{\τ}\right)\right]\mathrm{d\τ}.$

(4)建立以失准角为***状态，以比力信息为***量测的光纤捷联罗经***精对准的卡尔曼滤波模型。

以三个失准角构造***状态，以比力信息构造***量测，得到滤波模型：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

Y(t)＝H(t)X(t)+V(t)

***状态向量为X＝[φ_e，φ_n，φ_u]^T，***矩阵F(t)为：

$F\left(t\right)=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& {-\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {-\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\end{array}\right]$

系泊情况下，由于忽略了晃动引起的干扰加速度，导航坐标系下的东向比力和北向比力为零，则加速度计的输出在导航坐标系下投影的水平分量即为与失准角耦合信息，***量测Y为：

$Y=\left[\begin{array}{c}\frac{\underset{i=k}{\overset{k+N}{\mathrm{\Σ}}}{\left({\tilde{f}}_e\right)}_i}{N}\\ \frac{\underset{i=k}{\overset{k+N}{\mathrm{\Σ}}}{\left({\tilde{f}}_n\right)}_i}{N}\end{array}\right]$

其中，

和

为加速度计在导航坐标系下的投影，N为滤波周期的采样次数。

量测矩阵H(t)为：

$H\left(t\right)=\left[\begin{array}{ccc}0& g& 0\\ -g& 0& 0\end{array}\right]$

式中，g为当地的重力加速度值，g＝9.8m/s²。

对滤波模型进行离散化处理，即得到离散卡尔曼滤波模型。

(5)利用滤波估计的失准角φ_e、φ_n、φ_u闭环修正姿态矩阵

获得新的姿态矩阵然后通过提取方位角H、纵摇角P和横摇角R，并完成光纤捷联罗经***的初始对准。

$C_n^b=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_u& {-\mathrm{\φ}}_n\\ {-\mathrm{\φ}}_u& 1& {\mathrm{\φ}}_e\\ {\mathrm{\φ}}_n& {-\mathrm{\φ}}_e& 1\end{array}\right]\·C_n^c$

Claims (1)

1.一种光纤捷联罗经***的对准方法，其特征在于，包括以下步骤：

步骤1定义坐标系：光纤捷联罗经***以东北天坐标系为导航坐标系n系；载体坐标系b系以载体中心为原点，x轴沿横轴指向右，y轴沿纵轴指向前，z轴垂直载体指向上；经线地球坐标系e系以地球中心为原点，x、y轴在地球赤道平面内，x轴指向载体所在点经线，z轴指向地球自转轴方向；经线地心惯性坐标系i系定义为在粗对准起始时刻将地球坐标系惯性凝固成的右手坐标系；载体惯性坐标系i_b0系定义为在粗对准起始时刻将载体坐标系惯性凝固后的坐标系，

步骤2光纤捷联罗经***开机预热后采集惯性测量组件的输出数据，包括三只光纤陀螺仪的输出数据

和三只石英加速度计的输出数据f^b，

步骤3根据

和f^b，以及地球自转角速率ω_ie、重力加速度g、纬度L信息，应用基于并行双积分重力矢量信息的粗对准算法，完成光纤捷联罗经***的粗对准，得到初始的姿态矩阵

在不同起始时刻并行运行r个基于双积分重力矢量的姿态矩阵计算过程，在最后同一时刻获得r个姿态矩阵值，对r个姿态矩阵值进行融合以提高粗对准结果的可靠性，并通过所述的初始姿态矩阵

提取载体的方位角H、纵摇角P和横摇角R以完成粗对准，所述的初始姿态矩阵

为

$C_n^c\left(t\right)=\frac{1}{r}\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\left[{\tilde{C}}_n^b\left(t\right)\right]}_i,i=\mathrm{1,2},\·\·\·,r$

其中，

为r个并行运算中的第i个中间姿态矩阵

进行正交化处理的结果，即：

${\tilde{C}}_n^b\left(t\right)=C_n^b\left(t\right){\left[{\left(C_n^b\left(t\right)\right)}^T{C}_n^b\left(t\right)\right]}^{-1/2}$

其中，

为中间姿态矩阵，并且， $C_n^b\left(t\right)=C_{i_{\mathrm{bo}}}^b\left(t\right)\·{\tilde{C}}_i^{i_{\mathrm{bo}}}\·C_e^i\left(t\right)\·C_n^e$

步骤3.1计算导航坐标系与经线地球坐标系之间的转移矩阵

$C_n^e=\left[\begin{array}{ccc}0& -\sin L& \cos L\\ 1& 0& 0\\ 0& \cos L& \sin L\end{array}\right]$

步骤3.2计算经线地球坐标系与惯性坐标系之间的转移矩阵

$C_e^i\left(t\right)=\left[\begin{array}{ccc}\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& -\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& 0\\ \sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& \cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& 0\\ 0& 0& 1\end{array}\right]$

步骤3.3计算载体惯性坐标系与载体坐标系之间的转移矩阵在起始时刻，惯性坐标系与载体坐标系重合，即

的初值为单位阵，根据陀螺仪输出

通过四元数方法求解

步骤3.4计算惯性坐标系与载体惯性坐标系之间的转移矩阵

${\tilde{C}}_i^{i_{\mathrm{bo}}}=\frac{1}{t_3-t_2}{\∫}_{t_2}^{t_3}{C}_i^{i_{\mathrm{bo}}}\left(\mathrm{\τ}\right)\mathrm{d\τ}$

$=\frac{1}{t_3-t_2}{\∫}_{t_2}^{t_3}\left(\begin{array}{cc}{\left[\begin{array}{c}{\left[V^{i_{b0}}\left(t_1\right)\right]}^T\\ {\left[V^{i_{b0}}\left(\mathrm{\τ}\right)\right]}^T\\ {[V^{i_{b0}}\left(t_1\right)\×V^{i_{b0}}\left(\mathrm{\τ}\right)]}^T\end{array}\right]}^{-1}& \left[\begin{array}{c}{\left[V^i\left(t_1\right)\right]}^T\\ {\left[V^i\left(\mathrm{\τ}\right)\right]}^T\\ {[V^i\left(t_1\right)\×V^i\left(\mathrm{\τ}\right)]}^T\end{array}\right]\end{array}\right)\mathrm{d\τ}$

式中， $V^i\left(t\right)=\left[\begin{array}{c}\frac{g\cos L\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ \frac{g\cos L[1-\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ g\sin L\·t\end{array}\right];$

$V^{i_{b0}}\left(t\right)={\∫}_0^t{f}^{i_{b0}}\left(\mathrm{\τ}\right)\mathrm{d\τ}={\∫}_0^t\left[C_b^{i_{b0}}\left(\mathrm{\τ}\right)f^b\left(\mathrm{\τ}\right)\right]\mathrm{d\τ}={\∫}_0^t\left[{\left(C_{i_{b0}}^b\left(\mathrm{\τ}\right)\right)}^T{f}^b\left(\mathrm{\τ}\right)\right]\mathrm{d\τ},$

步骤4建立以失准角为***状态，以比力信息为***量测的光纤捷联罗经***精对准的卡尔曼滤波模型，

以三个失准角构造***状态，以比力信息构造***量测，得到滤波模型：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

Y(t)＝H(t)X(t)+V(t)

***状态向量为X＝[φ_e,φ_n,φ_u]^T，W(t)为***噪声，V(t)为量测噪声，***矩阵F(t)为：

$F\left(t\right)=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\end{array}\right]$

系泊情况下，忽略晃动引起的干扰加速度，导航坐标系下的东向比力和北向比力为零，则加速度计的输出在导航坐标系下投影的水平分量即为与失准角耦合信息，***量测Y为：

$Y=\left[\begin{array}{c}\frac{\underset{i=k}{\overset{k+N}{\mathrm{\Σ}}}{\left({\tilde{f}}_e\right)}_i}{N}\\ \frac{\underset{i=k}{\overset{k+N}{\mathrm{\Σ}}}{\left({\tilde{f}}_n\right)}_i}{N}\end{array}\right]$

其中，

和

为加速度计在导航坐标系下的投影，N为滤波周期的采样次数，量测矩阵H(t)为：

$H\left(t\right)=\left[\begin{array}{ccc}0& g& 0\\ -g& 0& 0\end{array}\right]$

式中，g为当地的重力加速度值，g=9.8m/s²，

对滤波模型进行离散化处理，即得到离散卡尔曼滤波模型，

步骤5利用步骤(4)滤波估计的失准角φ_e、φ_n、φ_u闭环修正姿态矩阵

获得新的姿态矩阵

然后通过

提取方位角H、纵摇角P和横摇角R，并完成光纤捷联罗经***的初始对准，

$C_n^b=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_u& -{\mathrm{\φ}}_n\\ -{\mathrm{\φ}}_u& 1& {\mathrm{\φ}}_e\\ {\mathrm{\φ}}_n& -{\mathrm{\φ}}_e& 1\end{array}\right]\·C_n^c.$

Images