大失准角下船用光纤陀螺捷联航姿系统系泊精对准方法

摘要

本发明提供的是一种大失准角下船用光纤陀螺捷联航姿系统系泊精对准方法。对于系泊状态下的船用光纤陀螺捷联航姿系统，在其采集陀螺仪输出和加速度计输出信息完成粗对准的基础之上(由于纵荡，横荡，垂荡姿态误差很大)，建立载体坐标系和计算地理坐标系之间的转换矩阵，确定四元数误差；建立以四元数误差和速度误差为状态变量的卡尔曼滤波状态方程及速度误差为量测量的量测方程；利用得到的四元数误差和初始时刻姿态矩阵对应的四元数，计算出载体坐标系和惯性系之间的转换矩阵对应的四元数，初始对准。本发明对于大失准角情况，系统方程仍然为线性，可以利用已经比较成熟的卡尔曼滤波进行滤波估计，精度高，可靠性好；对舰船系泊状态对准三种荡不敏感，对准精度高。

说明书

(一)技术领域

本发明涉及的是一种测量方法，特别是涉及一种捷联航姿系统的对准技术，尤其涉及一种船用光纤陀螺捷联航姿系统系泊精对准方法。

(二)背景技术

捷联航姿系统是把惯性仪表固连在载体上，用计算机来完成导航平台功能的导航系统，它与平台惯导相比具有体积小，重量轻，成本低，可靠性高，便于维护等优点，因此得到越来越广泛的应用。而其导航精度很大程度上取决于初始对准的精度。初始对准一般分为粗对准和精对准两阶段，其中粗对准给出姿态矩阵且失准角一般不能太大，精对准利用粗对准给出的姿态矩阵进行进一步的对准，力求得到最准确的捷联矩阵(姿态信息)。粗对准方法直接利用加速度计的测量值，而舰船在系泊条件下由于受到纵荡，横荡，垂荡的影响，加速度计信息受到污染，因而对准效果往往不太好，失准角较大系统误差呈现比较严重的非线性特征。而传统的精对准方法(如二阶调平法)在系泊条件下由于受到纵荡，横荡，垂荡的影响和失准角较大的影响往往不能满足对准的精度要求。因此，如何提高大失准角条件下船用光纤陀螺捷联航姿系统系泊精对准的精度有重要的意义。

目前也有部分与捷联相关的导航系统有关的研究报道，例如专利申请号为200710063358.9，名称为“一种SINS/GPS/磁罗盘组合导航系统的数据融合方法”的专利申请文件中公开的技术方案等。

(三)发明内容

本发明的目的在于提供一种能够有效提高船用光纤陀螺捷联航姿系统系泊对准精度的大失准角下船用光纤陀螺捷联航姿系统系泊精对准方法。

本发明的目的是这样实现的：

本发明包括下列步骤：

(1)光纤陀螺捷联航姿系统预热后采集光纤陀螺仪和石英挠性加速度计输出的数据。

(2)根据加速度计的输出与重力加速度的关系以及陀螺仪输出与地球旋转角速率的关系初步确定此时的姿态信息(纵摇角θ，横摇角γ和航向角ψ)完成捷联航姿系统的粗对准(由于在系泊状态下存在纵荡，横荡，垂荡的影响，此时姿态误差角很大)。 

(3)利用粗对准给出的姿态信息、即纵摇角θ，横摇角γ和航向角ψ，建立载体坐标系b和计算地理坐标系n′之间的转换矩阵，确定四元数误差；

(4)建立以四元数误差和速度误差为状态变量的卡尔曼滤波状态方程及速度误差为量测量的量测方程；

(5)利用步骤(4)估计得到的四元数误差和此时姿态矩阵对应的四元数，计算出载体坐标系b和惯性系i之间的转换矩阵对应的四元数，更新姿态矩阵完成初始对准。

本发明还可以包括如下特征：

1、所述的建立载体坐标系b和计算地理坐标系n′之间的转换矩阵，确定四元数误差包括：

定义为初始捷联矩阵C_bⁿ′：

$C_b^{n^{\prime }}=\left(\begin{array}{ccc}\cos \gamma \cos \psi -\sin \gamma \sin \theta \sin \psi & -\cos \theta \sin \psi & \sin \gamma \cos \psi +\cos \gamma \sin \theta \sin \psi \\ \cos \gamma \sin \psi +\sin \gamma \sin \theta \cos \psi & \cos \theta \cos \psi & \sin \gamma \sin \psi -\cos \gamma \sin \theta \cos \psi \\ -\sin \gamma \cos \theta & \sin \theta & \cos \gamma \cos \theta \end{array}\right)$

利用

$C_i^n\left(t\right)=\left(\begin{array}{ccc}-\sin (\lambda +{\omega }_{\mathrm{ie}}t)& \cos (\lambda +{\omega }_{\mathrm{ie}}t)& 0\\ -\sin L\cos (\lambda +{\omega }_{\mathrm{ie}}t)& -\sin L\sin (\lambda +{\omega }_{\mathrm{ie}}t)& \cos L\\ \cos L\cos (\lambda +{\omega }_{\mathrm{ie}}t)& \cos L\sin (\lambda +{\omega }_{\mathrm{ie}}t)& \sin L\end{array}\right)$

其中λ和L分别为捷联航姿系统所在的经度和纬度，ω_ie为地球自转角速率；

初始时刻载体坐标系b和计算地理坐标系i′之间的转换矩阵

$C_b^{i^{\prime }}={\left[C_i^n\left(0\right)\right]}^T{C}_b^{n^{\prime }}$

建立与C_bⁱ′相对应的四元数

$Q_b^{i^{\prime }}={\left(\begin{array}{cccc}{\tilde{q}}_0& {\tilde{q}}_1& {\tilde{q}}_2& {\tilde{q}}_3\end{array}\right)}^T$

定义载体坐标系b和惯性系i之间的转换矩阵对应的四元数为：

$Q_b^i={\left(\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right)}^T$

定义四元数误差δQ＝[δq₀ δq₁ δq₂ δq₃]^T为：

$\mathrm{\delta Q}=Q_b^{i^{\prime }}-Q_b^i$

2、所述的建立以四元数误差和速度误差为状态变量的卡尔曼滤波状态方程及速度误差为量测量的量测方程的步骤包括：

1)建立卡尔曼滤波状态方程：

使用一阶线性随即微分方程来描述捷联航姿系统的状态误差：

X^&(t)＝F(t)X(t)+G(t)W(t)

其中X(t)为t时刻系统的状态向量；F(t)和G(t)分别为系统状态矩阵和噪声矩阵；W(t)为系统的噪声向量；

系统的状态向量为：

X＝[δV_E δV_N δV_D  δq₀ δq₁ δq₂  δq₃ _x _y _z ε_x ε_y ε_z]^T

系统的白噪声向量为：

$W=\left(\begin{array}{ccccccccccccc}{w}_{{▿}_x}& w_{{▿}_x}& w_{{▿}_x}& w_{{ϵ}_x}& w_{{ϵ}_y}& w_{{ϵ}_z}& 0& 0& 0& 0& 0& 0& 0\end{array}\right)$

其中δV_E δV_N δV_D分别表示东向、北向和天向的速度误差；_x _y _z分别为X、Y、Z轴加速度计的零偏；ε_x ε_y ε_z分别为X、Y、Z轴陀螺的常值漂移；w_x w_x w_x分别为X、Y、Z轴加速度计的白噪声误差；w_εx w_εy w_εz分别为X、Y、Z轴陀螺的白噪声误差；

系统噪声系数矩阵为：

$G\left(t\right)=\left(\begin{array}{cc}{C}_b^{i^{\prime }}& 0_{3\times 3}\\ 0_{3\times 3}& U/2\\ 0_{6\times 3}& 0_{6\times 3}\end{array}\right)$

系统的状态转移矩阵为：

$F\left(t\right)=\left(\begin{array}{c}{F}_s\left(t\right)\\ 0_{6\times 13}\end{array}\right)$

其中：

$F_S\left(t\right)=\left(\begin{array}{cccc}{0}_{3\times 3}& A& C_b^{i^{\prime }}& 0_{3\times 3}\\ 0_{3\times 3}& M& 0_{3\times 3}& U/2\end{array}\right)$

$U=\left(\begin{array}{ccc}{\widetilde{-q}}_1& {\widetilde{-q}}_2& {\widetilde{-q}}_3\\ {\tilde{q}}_0& {\widetilde{-q}}_3& {\tilde{q}}_2\\ {\tilde{q}}_3& {\tilde{q}}_0& {\widetilde{-q}}_1\\ {\widetilde{-q}}_2& {\widetilde{-q}}_1& {\tilde{q}}_0\end{array}\right)$

$M=\left(\begin{array}{cccc}0& {-\omega }_{\mathrm{ibx}}^b& {-\omega }_{\mathrm{iby}}^b& {-\omega }_{\mathrm{ibz}}^b\\ {\omega }_{\mathrm{ibx}}^b& 0& {\omega }_{\mathrm{ibz}}^b& {-\omega }_{\mathrm{iby}}^b\\ {\omega }_{\mathrm{iby}}^b& {-\omega }_{\mathrm{ibz}}^b& 0& {\omega }_{\mathrm{ibx}}^b\\ {\omega }_{\mathrm{ibz}}^b& {\omega }_{\mathrm{iby}}^b& -{\omega }_{\mathrm{ibx}}^b& 0\end{array}\right)$

其中${\omega }_{\mathrm{ib}}^b={\left(\begin{array}{ccc}{\omega }_{\mathrm{ibx}}^b& {\omega }_{\mathrm{iby}}^b& {\omega }_{\mathrm{ibz}}^b\end{array}\right)}^T$为陀螺输出的载体系相对于惯性系角速率；

$A=2((g^i\times )Y^T-{\mathrm{Ng}}^i{\left(Q_b^{i^{\prime }}\right)}^T)\mathrm{\delta Q}$

$Y=\left(\begin{array}{ccc}{\widetilde{-q}}_1& {\widetilde{-q}}_2& {\widetilde{-q}}_3\\ {\tilde{q}}_0& {\tilde{q}}_3& {\widetilde{-q}}_2\\ {\widetilde{-q}}_3& {\tilde{q}}_0& {\tilde{q}}_1\\ {\tilde{q}}_2& {\widetilde{-q}}_1& {\tilde{q}}_0\end{array}\right)$

$g^i=\left(\begin{array}{c}-g\cos L\cos (\lambda +{\omega }_{\mathrm{ie}}t)\\ -g\cos L\sin (\lambda +{\omega }_{\mathrm{ie}}t)\\ -g\sin L\end{array}\right)$

N可以取单位阵；

2)建立卡尔曼滤波的量测方程：

使用一阶线性随即微分方程来描述捷联航姿系统的量测方程如下：

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

其中，Z(t)表示t时刻系统的量测向量；H(t)表示系统的量测矩阵；V(t)表示系统的测量噪声；

系统量测矩阵为：

H(t)＝[I_3×3 0_3×10]

量测量为：

$Z\left(t\right)=V_f^i\left(t\right)+V_g^i\left(t\right)$

其中

$V_g^i\left(t\right)={\int }_1^{\prime }{g}^i\mathrm{dt}$

$V_f^i\left(t\right)={\int }_1^{\prime }{C}_b^{i^{\prime }}{f}^b\mathrm{dt}$

3、所述的计算出载体坐标系b和惯性系i之间的转换矩阵对应的四元数的方法为：

$Q_b^i=Q_b^{i^{\prime }}-\mathrm{\delta Q}$

利用Q_bⁱ求得载体坐标系b和惯性系i之间的转换矩阵：

$C_b^i\left(t\right)=\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)$

$C_b^n\left(t\right)=C_b^i\left(t\right)C_i^n\left(t\right)$

根据C_bⁿ(t)得到载体姿态，即纵摇角、横摇角和航向角的主值如下：

由上述三个主值按照如下公式判断真值：

θ＝θ_主

至此，初始对准完成，可以进入导航状态。

本发明的方法具有如下优点：(1)对于大失准角情况，系统方程仍然为线性，可以利用研究已经比较成熟的卡尔曼滤波进行滤波估计，估计的精度高，可靠性好；(2)该方法对舰船系泊状态对准时存在的纵荡、横荡、垂荡扰动不敏感，对准精度高。

对本发明的有益效果说明如下：

(1)Matlab仿真

在以下的仿真条件下，对该方法进行仿真实验：

捷联航姿系统作三轴摇摆运动。载体以正弦规律绕航向角、纵摇角和横摇角摇摆，其数学模型为：

ψ＝ψ_msin(ω_ψ+φ_ψ)+k

θ＝θ_msin(ω_θ+φ_θ)

γ＝γ_msin(ω_γ+φ_γ)

其中：ψ，θ，γ分别表示绕航向角、纵摇角和横摇角的摇摆角度变量；ψ_m，θ_m，γ_m分别表示相应的摇摆角度幅值；ω_ψ，ω_θ，ω_γ分别表示相应的摇摆角频率；φ_ψ，φ_θ，φ_γ分别表示相应的初相位；而ω_i＝2π/T_i，i＝ψ，θ，γ，T_i表示相应的摇摆周期；k为真航迹。仿真时取ψ_m＝6°，θ_m＝3°，γ_m＝3°，T_ψ＝9s，T_θ＝6s，T_γ＝8s，k＝0。

载体初始位置：北纬45.7796°，东经126.6705°；

载体真实姿态：ψ＝30°，θ＝30°，γ＝30°

初始姿态误差角：横摇误差角角1°，纵摇误差角1°，方位误差角10°；

由垂荡、纵荡和横荡引起的线加速度为

其中，A_Dx＝0.02m，A_Dy＝0.02m，A_Dz＝0.16m；ω_Di＝2π/T_Di，且T_Dx＝8s，T_Dy＝10s，T_Dz＝10s；_Di为[0，2π]上服从均匀分布的随机相位；

赤道半径：R_e＝6378393.0m；

椭球度：e＝3.367e-3；

由万有引力可得的地球表面重力加速度：g₀＝9.78049；

地球自转角速度(弧度/秒)：7.2921158e-5；

陀螺仪常值漂移：0.01度/小时；

陀螺仪白噪声误差：0.005度/小时；

加速度计零偏：10^-4g₀；

加速度计白噪声误差：5×10^-5g₀：

常数：π＝3.1415926；

利用发明所述方法得到估计航向角、纵摇角和横摇角曲线分别如图1、图2和图3所示；

航向失准角误差、纵摇失准角误差和横摇失准角误差曲线分别如图4、图5和图6所示。结果表明在大失准角且有垂荡、纵荡和横荡干扰条件下，采用本发明的方法可以获得较高的对准精度。

(2)光纤陀螺捷联航姿系统的三轴转台实验

将自行研制的光纤陀螺捷联航姿系统放在三轴转台上进行静止和三轴摇摆实验，实验中用到了粗对准信息，所用光纤陀螺捷联航姿系统的器件精度和实验环境如下：

陀螺仪常值漂移：0.01度/小时；

陀螺仪白噪声误差：0.005度/小时；

加速度计零偏：10^-4g₀；

加速度计白噪声误差：5×10^-5g₀：

载体以正弦规律绕航向角、纵摇角和横摇角摇摆，其数学模型为：

ψ＝ψ_msin(ω_ψ+φ_ψ)+k

θ＝θ_msin(ω_θ+φ_θ)

γ＝γ_msin(ω_γ+φ_γ)

ψ_m＝5°，θ_m＝5°，γ_m＝5°，T_ψ＝4s，T_θ＝4s，T_γ＝4s

载体真实姿态：ψ＝30°，θ＝0°，γ＝0°

利用发明所述方法得到在三轴转台对准实验静止状态航向角、纵摇角和横摇角曲线分别如图7、图8和图9所示；三轴摇摆状态航向角、纵摇角和横摇角曲线分别如图10、图11和图12所示。图7至图12都只截取了精对准阶段。结果表明在该种状态下对准精度可以满足实际需要。

(3)光纤陀螺捷联航姿系统的系泊实验

本实验是利用光纤陀螺捷联航姿系统进行的系泊对准实验。器件精度与光纤陀螺捷联航姿系统的三轴转台实验相同。本次实验利用高精度的船载组合导航系统的输出姿态信息作为姿态基准，自行研制的光纤陀螺捷联航姿系统与姿态基准的误差曲线如图13、图14和图15所示。图13至图15都只截取了精对准阶段。结果表明利用本发明的方法在系泊对准中完全达到了实用的水平。

(四)附图说明

图1为利用Matlab仿真得到的航向角曲线图；

图2为利用Matlab仿真得到的纵摇角曲线图；

图3为利用Matlab仿真得到的横摇角曲线图；

图4为利用Matlab仿真得到的航向失准角误差曲线图；

图5为利用Matlab仿真得到的纵摇失准角误差曲线图；

图6为利用Matlab仿真得到的横摇失准角误差曲线图；

图7为利用三轴转台实验得到的静止状态航向角曲线图；

图8为利用三轴转台实验得到的静止状态纵摇角曲线图；

图9为利用三轴转台实验得到的静止状态横摇角曲线图；

图10为利用三轴转台实验得到的三轴摇摆状态航向角曲线图；

图11为利用三轴转台实验得到的三轴摇摆状态纵摇角曲线图；

图12为利用三轴转台实验得到的三轴摇摆状态横摇角曲线图；

图13为海试时与姿态基准的航向角误差曲线图；

图14为海试时与姿态基准的纵摇角误差曲线图；

图15为海试时与姿态基准的横摇角误差曲线图。

(五)具体实施方式

下面举例对本发明做更详细地描述：

(1)光纤陀螺捷联航姿系统预热后采集光纤陀螺仪和石英挠性加速度计输出的数据。

根据加速度计的输出与重力加速度的关系以及陀螺仪输出与地球旋转角速率的关系初步确定此时的姿态信息(纵摇角θ，横摇角γ和航向角ψ)完成捷联航姿系统的粗对准(由于在系泊状态下存在纵荡，横荡，垂荡的影响，此时姿态误差角很大)。

(2)利用粗对准给出的姿态信息，即纵摇角θ，横摇角γ和航向角ψ建立载体坐标系b

和计算地理坐标系n′之间的转换矩阵，定义为初始捷联矩阵C_bⁿ′：

$C_b^{n^{\prime }}=\left(\begin{array}{ccc}\cos \gamma \cos \psi -\sin \gamma \sin \theta \sin \psi & -\cos \theta \sin \psi & \sin \gamma \cos \psi +\cos \gamma \sin \theta \sin \psi \\ \cos \gamma \sin \psi +\sin \gamma \sin \theta \cos \psi & \cos \theta \cos \psi & \sin \gamma \sin \psi -\cos \gamma \sin \theta \cos \psi \\ -\sin \gamma \cos \theta & \sin \theta & \cos \gamma \cos \theta \end{array}\right)---\left(1\right)$

利用

$C_i^n\left(t\right)=\left(\begin{array}{ccc}-\sin (\lambda +{\omega }_{\mathrm{ie}}t)& \cos (\lambda +{\omega }_{\mathrm{ie}}t)& 0\\ -\sin L\cos (\lambda +{\omega }_{\mathrm{ie}}t)& -\sin L\sin (\lambda +{\omega }_{\mathrm{ie}}t)& \cos L\\ \cos L\cos (\lambda +{\omega }_{\mathrm{ie}}t)& \cos L\sin (\lambda +{\omega }_{\mathrm{ie}}t)& \sin L\end{array}\right)---\left(2\right)$

其中λ和L分别为捷联航姿系统所在的经度和纬度，ω_ie为地球自转角速率；

初始时刻载体坐标系b和计算地理坐标系i′之间的转换矩阵

$C_b^{i^{\prime }}={\left[C_i^n\left(0\right)\right]}^T{C}_b^{n^{\prime }}---\left(3\right)$

建立与C_bⁱ′相对应的四元数

$Q_b^{i^{\prime }}={\left(\begin{array}{cccc}{\tilde{q}}_0& {\tilde{q}}_1& {\tilde{q}}_2& {\tilde{q}}_3\end{array}\right)}^T---\left(4\right)$

定义载体坐标系b和惯性系i之间的转换矩阵对应的四元数为：

$Q_b^i={\left(\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right)}^T---\left(5\right)$

定义四元数误差δQ＝[δq₀ δq₁ δq₂ δq₃]^T为：

$\mathrm{\delta Q}=Q_b^{i^{\prime }}-Q_b^i---\left(6\right)$

(3)建立以四元数误差和速度误差为状态变量的卡尔曼滤波状态方程及速度误差为量测量的量测方程：

1)建立卡尔曼滤波状态方程：

使用一阶线性随机微分方程来描述捷联航姿系统的状态误差如下：

X^&(t)＝F(t)X(t)+G(t)W(t)    (7)

其中X(t)为t时刻系统的状态向量；F(t)和G(t)分别为系统状态矩阵和噪声矩阵；W(t为系统的噪声向量；

系统的状态向量为：

X＝[δV_E δV_E δV_E δV₀ δq₁ δq₂ δq₃ _x _y _z ε_x ε_y ε_z]^T    (8)

系统的白噪声向量为：

$W=\left(\begin{array}{ccccccccccccc}{w}_{{▿}_x}& w_{{▿}_x}& w_{{▿}_x}& w_{{ϵ}_x}& w_{{ϵ}_y}& w_{{ϵ}_z}& 0& 0& 0& 0& 0& 0& 0\end{array}\right)---\left(9\right)$

其中δV_E δV_N δV_D分别表示东向、北向和天向的速度误差；_x _y _x分别为X、Y、Z轴加速度计的零偏；ε_x ε_y ε_z分别为X、Y、Z轴陀螺的常值漂移；w_x w_x w_x分别为X、Y、Z轴加速度计的白噪声误差；w_εx w_εy w_εz分别为X、Y、Z轴陀螺的白噪声误差；

系统噪声系数矩阵为：

$G\left(t\right)=\left(\begin{array}{cc}{C}_b^{i^{\prime }}& 0_{3\times 3}\\ 0_{3\times 3}& U/2\\ 0_{6\times 3}& 0_{6\times 3}\end{array}\right)---\left(10\right)$

系统的状态转移矩阵为：

$F\left(t\right)=\left(\begin{array}{c}{F}_s\left(t\right)\\ 0_{6\times 13}\end{array}\right)---\left(11\right)$

其中：

$F_S\left(t\right)=\left(\begin{array}{cccc}{0}_{3\times 3}& A& C_b^{i^{\prime }}& 0_{3\times 3}\\ 0_{3\times 3}& M& 0_{3\times 3}& U/2\end{array}\right)---\left(12\right)$

$U=\left(\begin{array}{ccc}{\widetilde{-q}}_1& {\widetilde{-q}}_2& {\widetilde{-q}}_3\\ {\tilde{q}}_0& {\widetilde{-q}}_3& {\tilde{q}}_2\\ {\tilde{q}}_3& {\tilde{q}}_0& {\widetilde{-q}}_1\\ {\widetilde{-q}}_2& {\widetilde{-q}}_1& {\tilde{q}}_0\end{array}\right)---\left(13\right)$

$M=\left(\begin{array}{cccc}0& {-\omega }_{\mathrm{ibx}}^b& {-\omega }_{\mathrm{iby}}^b& {-\omega }_{\mathrm{ibz}}^b\\ {\omega }_{\mathrm{ibx}}^b& 0& {\omega }_{\mathrm{ibz}}^b& {-\omega }_{\mathrm{iby}}^b\\ {\omega }_{\mathrm{iby}}^b& {-\omega }_{\mathrm{ibz}}^b& 0& {\omega }_{\mathrm{ibx}}^b\\ {\omega }_{\mathrm{ibz}}^b& {\omega }_{\mathrm{iby}}^b& {-\omega }_{\mathrm{ibx}}^b& 0\end{array}\right)---\left(14\right)$

其中${\omega }_{\mathrm{ib}}^b={\left(\begin{array}{ccc}{\omega }_{\mathrm{ibx}}^b& {\omega }_{\mathrm{iby}}^b& {\omega }_{\mathrm{ibz}}^b\end{array}\right)}^T$为陀螺输出的载体系相对于惯性系角速率；

$A=2\left(\right(g^i\times )Y^T-N{g}^i(Q_b^{i^{\prime }}{)}^T)\mathrm{\delta Q}---\left(15\right)$

$Y=\left(\begin{array}{ccc}{\widetilde{-q}}_1& {\widetilde{-q}}_2& {\widetilde{-q}}_3\\ {\tilde{q}}_0& {\tilde{q}}_3& {\widetilde{-q}}_2\\ {\widetilde{-q}}_3& {\tilde{q}}_0& {\tilde{q}}_1\\ {\tilde{q}}_2& {\widetilde{-q}}_1& {\tilde{q}}_0\end{array}\right)---\left(16\right)$

$g^i=\left(\begin{array}{c}-g\cos L\cos (\lambda +{\omega }_{\mathrm{ie}}t)\\ -g\cos L\sin (\lambda +{\omega }_{\mathrm{ie}}t)\\ -g\sin L\end{array}\right)---\left(17\right)$

N可以取单位阵；

2)建立卡尔曼滤波的量测方程：

使用一阶线性随机微分方程来描述捷联航姿系统的量测方程如下：

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

其中，Z(t)表示t时刻系统的量测向量；H(t)表示系统的量测矩阵；V(t)表示系统的测量噪声；

系统量测矩阵为：

H(t)＝[I_3×3 0_3×10]    (19)

量测量为：

$Z\left(t\right)=V_f^i\left(t\right)+V_g^i\left(t\right)---\left(20\right)$

其中

$V_g^i\left(t\right)={\int }_1^{\prime }{g}^i\mathrm{dt}---\left(21\right)$

$V_f^i\left(t\right)={\int }_1^{\prime }{C}_b^{i^{\prime }}{f}^b\mathrm{dt}---\left(22\right)$

(4)利用步骤(3)估计得到的δQ和初始时刻姿态矩阵对应的四元数Q_bⁱ′，计算出载体坐标系b和惯性系i之间的转换矩阵对应的四元数：

$Q_b^i=Q_b^{i^{\prime }}-\mathrm{\delta Q}---\left(23\right)$

利用Q_bⁱ求得载体坐标系b和惯性系i之间的转换矩阵：

$C_b^i\left(t\right)=\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)---\left(25\right)$

$C_b^n\left(t\right)=C_b^i\left(t\right)C_i^n\left(t\right)---\left(26\right)$

根据C_bⁿ(t)可以得到载体姿态，即纵摇角、横摇角和航向角的主值如下：

由上述三个主值判断真值的公式如下：

θ＝θ_主

至此，初始对准完成，可以进入导航状态。