(一)技术领域

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

(二)背景技术

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

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

(三)发明内容

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

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

本发明包括下列步骤：

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

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

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

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

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

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

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

定义为初始捷联矩阵Cb n′：

$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 }}$

建立与Cb i′相对应的四元数

$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＝[δq0 δq1 δq2 δq3]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＝[δVE δVN δVD  δq0 δq1 δq2  δq3 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]$

其中δVE δVN δVD分别表示东向、北向和天向的速度误差；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)＝[I3×3 03×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}$

利用Qb i求得载体坐标系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)$

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

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

θ＝θ主

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

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

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

(1)Matlab仿真

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

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

ψ＝ψmsin(ωψ+φψ)+k

θ＝θmsin(ωθ+φθ)

γ＝γmsin(ωγ+φγ)

其中：ψ，θ，γ分别表示绕航向角、纵摇角和横摇角的摇摆角度变量；ψm，θm，γm分别表 示相应的摇摆角度幅值；ωψ，ωθ，ωγ分别表示相应的摇摆角频率；φψ，φθ，φγ分别表示相应 的初相位；而ωi＝2π/Ti，i＝ψ，θ，γ，Ti表示相应的摇摆周期；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°；

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

其中，ADx＝0.02m，ADy＝0.02m，ADz＝0.16m；ωDi＝2π/TDi，且TDx＝8s，TDy＝10s， TDz＝10s；Di为[0，2π]上服从均匀分布的随机相位；

赤道半径：Re＝6378393.0m；

椭球度：e＝3.367e-3；

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

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

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

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

加速度计零偏：10-4g0；

加速度计白噪声误差：5×10-5g0：

常数：π＝3.1415926；

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

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

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

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

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

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

加速度计零偏：10-4g0；

加速度计白噪声误差：5×10-5g0：

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

ψ＝ψ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′之间的转换矩阵，定义为初始捷联矩阵Cb n′：

$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)$

建立与Cb i′相对应的四元数

$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＝[δq0 δq1 δq2 δq3]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＝[δVE δVE δVE δV0 δq1 δq2 δq3 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)$

其中δVE δVN δVD分别表示东向、北向和天向的速度误差；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)＝[I3×3 03×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和初始时刻姿态矩阵对应的四元数Qb i′，计算出载 体坐标系b和惯性系i之间的转换矩阵对应的四元数：

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

利用Qb i求得载体坐标系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)$

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

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

θ＝θ主

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