CN103245360B - 晃动基座下的舰载机旋转式捷联惯导系统自对准方法 - Google Patents

Abstract

一种晃动基座下的舰载机旋转式捷联惯导系统自对准方法,属导航领域。首先获得载体所在位置的经度、纬度，其次采集惯性测量单元中光纤陀螺和石英挠性加速度计的输出信号，再采用双轴转台旋转惯性测量单元IMU对惯性器件的常值误差进行自动补偿，组成旋转式捷联惯导系统。然后以惯性坐标系下的重力加速度作为参考矢量，计算出粗初始姿态阵。再建立系统的状态方程和量测方程，设计渐消自适应Kalman滤波器精确估计载体的失准角，用失准角修正捷联姿态矩阵，完成初始对准，进入导航状态。本方法隔离了舰船晃动对舰载机初始对准的影响，通过渐消自适应Kalman滤波法估计系统的初始姿态阵，抑制了量测噪声中动态随机干扰，实现舰载机旋转式捷联惯导系统快速自对准。

Description

晃动基座下的舰载机旋转式捷联惯导系统自对准方法

技术领域

本发明涉及的是一种海上导航领域的初始对准技术，特别涉及的是一种晃动基座下的舰载机旋转式捷联惯导系统自对准方法。

背景技术

旋转调制技术作为一种惯性器件常值误差的自补偿方法，能够自动地对陀螺常值漂移和加速度计零位偏置进行调制，从而提高系统的长时间导航能力，充分发挥惯性导航的“自主性”优势。同常规捷联惯导系统相比，采用旋转调制技术的捷联惯导系统有望在相同器件水平下大幅提高系统的导航精度，具有体积小、质量轻、造价低、可靠性高、隐蔽性好等优点，成为航海领域中运载体主要的导航方式。

在海上军事领域，特别是水面舰船和舰载飞行器，对长时间高精度自主定位的需求越来越强烈。舰载机在进入导航工作前必须进行初始对准，初始对准的精度直接影响舰载机的安全与作战能力。

按照基座的运动状态，可将初始对准分为静基座对准和动基座对准。大量文献和工程实践证明，对于静基座下的旋转式捷联惯导系统，其初始对准方法大多采用了传统的捷联惯导对准方法，即利用惯性传感器的输出获取粗略的初始姿态阵，再利用Kalman滤波法进行精对准，无论在时间还是精度上，都达到很好要求。但实际应用中，舰船在航行时常常受到风浪的干扰而产生摇摆、俯仰和偏航等复杂运动，给舰载机的初始对准造成困难，导致陀螺测量到的地球自转角速度信噪比大幅度下降，传统解析粗对准误差较大，不满足精对准对初始姿态信息的要求。此外，传统Kalman滤波法在晃动基座下由于受到垂荡、纵荡、横荡等不确定性干扰的影响，造成对准精度下降、甚至滤波发散等问题。因此，对舰载机旋转式捷联惯导系统，采用常规的初始对准方法，难以达到快速高精度自主对准的目的。

发明内容

本发明的目的在于提供一种能够有效解决舰载机在海风、浪涌、发动机振动等随机干扰环境下自对准问题的晃动基座下舰载机旋转式捷联惯导系统自对准方法。

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

本发明晃动基座下舰载机旋转式捷联惯导系统自对准方法，其特征是：

（1）通过全球定位系统GPS获得载体所在位置的经度λ、纬度L，将它们装订至导航计算机中；

（2）捷联惯导系统进行预热准备，将标定后的捷联惯导系统安装在双轴转台上，启动系统，采集惯性测量单元IMU中光纤陀螺和石英挠性加速度计的输出信号；

（3）系统在进行初始对准和导航过程中，采用双轴转台旋转惯性测量单元IMU来进行惯性器件常值误差的自动补偿，抑制三个轴向的陀螺常值漂移和加速度计零位偏置对系统导航精度的影响，组成旋转式捷联惯导系统，其具体步骤如下：

首先，对晃动基座下舰载机旋转式捷联惯导系统自对准过程中使用的坐标系进行定义：

1）地球坐标系（e系）：原点位于地心，oz_e轴沿地球自转轴方向，ox_e轴位于赤道平面内，从地心指向载体所在点的子午线，oy_e轴位于赤道平面内，ox_e、oy_e和oz_e轴满足右手定则。

2）地心惯性坐标系（i系）：在粗对准起始时刻t₀将o-x_ey_ez_e惯性凝固后形成的坐标系。

3）导航坐标系（n系）：本文采用地理坐标系为导航坐标系，原点位于载体重心，ox_n轴指向东，oy_n轴指向北，oz_n轴指向天。

4）载体坐标系（b系）：原点位于载体重心，ox_b、oy_b和oz_b轴分别沿载体横轴指向右、纵轴指向前，立轴指向上。

5）基座惯性坐标系（i_b0系）：在t₀时刻将载体坐标系经惯性凝固后的坐标系。

6）IMU坐标系（s系）：原点位于惯性测量单元IMU的重心，初始时刻IMU坐标系与载体坐标系重合，然后IMU以角速度ω绕旋转轴转动。IMU坐标系是随着IMU位置改变的一个时变坐标系。

惯性测量单元IMU是由三个相互正交的光纤陀螺和三个石英挠性加速度计构成。IMU被装在一个双环框架中，外环轴与载体坐标系的Ozb轴平行，内环轴位于垂直于外环轴的平面。IMU绕内环轴以角速度ω₁连续旋转，同时内环框架和IMU一起绕外环轴以角速度ω₂连续旋转（其中，ω₁是ω₂的整数倍），IMU绕内环轴旋转4周后以相同角速度反向继续连续旋转。同样，绕外环轴的旋转在旋转4周后改变方向，以此不停地进行下去；由此得到载体坐标系到IMU坐标系的姿态矩阵为：

$C_b^s=C_b^{\mathrm{sx}}{C}_b^{\mathrm{sz}}$

$=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\ω}}_{1}t& \sin {\mathrm{\ω}}_{1}t\\ 0& -\sin {\mathrm{\ω}}_{1}t& \cos {\mathrm{\ω}}_{1}t\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\ω}}_{2}t& \sin {\mathrm{\ω}}_{2}t& 0\\ -\sin {\mathrm{\ω}}_{2}t& \cos {\mathrm{\ω}}_{2}t& 0\\ 0& 0& 1\end{array}\right]$

$=\left[\begin{array}{ccc}\cos {\mathrm{\ω}}_{2}t& \sin {\mathrm{\ω}}_{2}t& 0\\ -\cos {\mathrm{\ω}}_{1}t\sin {\mathrm{\ω}}_{2}t& \cos {\mathrm{\ω}}_{1}t\cos {\mathrm{\ω}}_{2}t& \sin {\mathrm{\ω}}_{1}t\\ \sin {\mathrm{\ω}}_{1}t\sin {\mathrm{\ω}}_{2}t& -\sin {\mathrm{\ω}}_{1}t\cos {\mathrm{\ω}}_{2}t& \cos {\mathrm{\ω}}_{1}t\end{array}\right]$

其中，为IMU绕内环轴旋转的姿态转换阵，为IMU绕外环轴旋转的姿态转换阵。

（4）对采集到的光纤陀螺和石英挠性加速度计的数据进行处理，以惯性坐标系下的重力加速度作为参考矢量，计算出粗略的初始姿态阵。

在旋转式捷联惯导系统的粗对准算法中，姿态矩阵可分散成4个矩阵求取；

设对准位置的纬度为L，姿态矩阵的表达式为：

$C_s^n\left(t\right)=C_e^n{C}_i^e\left(t\right)C_{i_{b0}}^i{C}_s^{i_{b0}}\left(t\right),$

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

式中，为地球坐标系到导航坐标系的姿态转换阵，为惯性坐标系到地球坐标系的姿态转换阵，为基座惯性坐标系到惯性坐标系的姿态转换阵，为IMU坐标系到基座惯性坐标系的姿态转换阵，可表示t为粗对准过程时间。其中，为载体坐标系到基座惯性坐标系的姿态转换阵，并且t₀时刻的为单位矩阵，即

利用四元数法求解姿态更新公式从而得到

其中，为的反对称阵，为摇摆状态下的旋转式捷联惯导系统中IMU坐标系下的陀螺输出，包含周期变化的地球自转角速度摇摆引起的干扰角速度δω^s、IMU旋转角速度和陀螺常值漂移ε，即：

${\mathrm{\ω}}_{i_{b0}s}^s={\mathrm{\ω}}_{\mathrm{ie}}^s+{\mathrm{\δ\ω}}^s+{\mathrm{\ω}}_{\mathrm{bs}}^s+\mathrm{\ϵ}$

采用双矢量定姿法求解为：

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

式中，Vⁱ为i系下的重力加速度的积分，为i_b0系下的重力加速度的积分。

$V^i\left(t_k\right)={\∫}_{t_0}^{t_k}(-g^i)\mathrm{dt}=-{\∫}_{t_0}^{t_k}{C}_e^i{C}_n^e{g}^n\mathrm{dt}=\left[\begin{array}{c}\frac{g\cos L}{{\mathrm{\ω}}_{\mathrm{ie}}}\sin {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_k\\ \frac{g\cos L}{{\mathrm{\ω}}_{\mathrm{ie}}}(1-\cos {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_k)\\ \mathrm{\Δ}{t}_{k}g\sin L\end{array}\right],$ Δt_k=t_k-t₀

其中，gⁱ为惯性坐标系下的重力加速度，gⁿ为导航坐标系下的重力加速度，ω_ie为地球自转角速度，t₀为粗对准开始时刻，t_k为粗对准结束时刻。

$V^{i_{b0}}\left(t_k\right)=V_f^{i_{b0}}\left(t_k\right)-V_{\mathrm{LA}}^{i_{b0}}\left(t_k\right)$

式中，为基座惯性坐标系下的速度，为基座惯性坐标系下的加速度计输出的积分，为基座惯性坐标系下的杆臂干扰加速度的积分。

$V_f^{i_{b0}}\left(t_k\right)={\∫}_{t_0}^{t_k}{f}^{i_{b0}}\mathrm{dt}={\∫}_{t_0}^{t_k}{C}_b^{i_{b0}}{C}_s^b{f}^s\mathrm{dt},$

$V_{\mathrm{LA}}^{i_{b0}}={\∫}_{t_0}^{t_k}{C}_b^{i_{b0}}{C}_s^b{a}_{\mathrm{LA}}^s\mathrm{dt}=C_s^{i_{b0}}({\mathrm{\ω}}_{\mathrm{is}}^s\×r^s),$

式中，为基座惯性坐标系下的加速度计输出，f^s为IMU坐标系下的加速度计输出，包含重力加速度g^s、线振动引起的干扰加速度杆臂干扰加速度和加速度计零偏即：

$f^s=-g^s+\mathrm{\δ}{a}_D^s+a_{\mathrm{LA}}^s+\▿$

（5）粗对准结束后，进入精对准阶段：建立惯性坐标系下的旋转式捷联惯导系统的失准角方程和速度误差方程。以速度误差、失准角、陀螺常漂移、加速度计零偏为状态量，建立系统的状态方程，以速度误差为观测量，建立系统的量测方程；

1）建立惯性坐标系下的旋转式捷联惯导系统的失准角方程和速度误差方程：

其中，为惯性坐标系下的失准角，δV为惯性坐标系下的速度误差，为IMU坐标系到惯性坐标系的姿态转换阵，ε_b、ε_w分别为陀螺的常值漂移和量测高斯白噪声；分别为加速度计的常值偏置误差和量测高斯白噪声。

2）建立系统的状态方程：

用一阶线性微分方程来描述旋转式捷联惯导系统的状态方程：

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W,$

其中，X(t)为t时刻系统的状态矢量，A(t)、B(t)分别为系统的状态转移矩阵和噪声矩阵，W为系统的噪声向量；

系统的状态矢量为：

系统的白噪声向量表示为：

$W={[{\▿}_{\mathrm{wx}},{\▿}_{\mathrm{wy}},{\▿}_{\mathrm{wz}},{\mathrm{\ϵ}}_{\mathrm{wx}},{\mathrm{\ϵ}}_{\mathrm{wy}},{\mathrm{\ϵ}}_{\mathrm{wz}},\mathrm{0,0,0,0,0,0}]}^T,$

其中，分别为惯性坐标系下东向、北向、天向的速度误差；分别为惯性坐标系下东向、北向、天向的失准角；ε_bx﹑ε_by﹑ε_bz分别为IMU坐标系下X、Y、Z轴的陀螺常值漂移；分别为IMU坐标系下X、Y、Z轴的加速度计常值零偏；ε_wx﹑ε_wy﹑ε_wz为陀螺X、Y、Z轴的高斯白噪声；为加速度计X、Y、Z轴的高斯白噪声。

系统的状态转移矩阵为：

$A=\left[\begin{array}{ccccc}{0}_{3\×3}& & -(g^i\×)& 0_{3\×3}& C_s^i\\ & 0_{3\×6}& -C_s^i& 0_{3\×3}& \\ & & 0_{3\×12}& & \\ & & 0_{3\×12}& & \end{array}\right],$

系统的噪声矩阵为：

$B=\left[\begin{array}{ccccccc}{C}_s^i& & 0_{3\×3}& & 0_{3\×3}& & 0_{3\×3}\\ & 0_{3\×3}& & -C_s^i& & 0_{3\×6}& \\ & & & 0_{3\×12}& & & \\ & & & 0_{3\×12}& & & \end{array}\right],$

3）建立系统的量测方程：

用一阶线性微分方程来描述旋转式捷联惯导系统的量测方程：

$Z\left(t\right)=H\left(t\right)X\left(t\right)+V_w+{\mathrm{\δV}}_D^i,$

其中，Z(t)为t时刻系统的量测向量，H(t)为系统的量测矩阵，为不确定性量测干扰，主要由垂荡、纵荡、横荡产生的干扰速度构成，V_w为量测高斯白噪声。

系统的量测矩阵为：

H(t)=[I_3×30_3×9]，

系统的量测向量为：

$Z=V_f^i+V_g^i-V_{\mathrm{LA}}^i,$

$V_f^i={\∫}_{t_1}^t{C}_s^i{f}^s\mathrm{d\τ},$

$V_g^i={\∫}_{t_1}^t{g}^i\mathrm{d\τ},$

$V_{\mathrm{LA}}^i{=\∫}_{t_1}^t{C}_s^i{a}_{\mathrm{LA}}^s\mathrm{d\τ}=C_s^i({\mathrm{\ω}}_{\mathrm{is}}^s\×r^s),$

其中，表示惯性坐标系下的加速度计输出的积分，表示惯性坐标系下的重力加速度的积分，表示惯性坐标系下的杆臂干扰加速度的积分；

（6）设计渐消自适应Kalman滤波器精确估计载体的失准角，用失准角修正捷联姿态矩阵，完成初始对准，进入导航状态。

旋转式捷联惯导系统的渐消自适应Kalman滤波方程为：

状态一步预测方程为：

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1},$

一步预测均方误差方程：

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

滤波增益方程：

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1},$

状态估值计算方程：

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}],$

估计均方误差方程：

P_k=[I-K_kH_k]P_k,k-1，

式中，渐消因子λ_k为：

${\mathrm{\λ}}_k=\frac{{\mathrm{\ϵ}}_k^T{\mathrm{\ϵ}}_k-\mathrm{tr}[H_k{Q}_{k-1}{H}_k^T+R_k]}{\mathrm{tr}\left[H_k{\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T{H}_k^T\right]}$

其中，残差表示为：

${\mathrm{\ϵ}}_k=Z_k-H_k{\hat{X}}_{k,k-1},$

λ_k值在滤波过程中自适应调整，以保证滤波器对量测值中不确定性干扰U_k的鲁棒性；

由于器件误差的存在，计算惯性坐标系与真实惯性坐标系并不重合。设i'系与i系间的失准角为通过步骤（5）精确估计出失准角则从i系到i′系的转换矩阵为：

$C_s^i\left(t\right)={\left[C_i^{i^{\′}}\left(t\right)\right]}^{-1}{C}_s^{i^{\′}}\left(t\right),$

$C_s^{i^{\′}}\left(t\right)=C_s^{i^{\′}}\left(0\right)\mathrm{\Δ}{C}_s^i,$

其中，是通过粗对准过程建立的IMU坐标系与计算惯性坐标系之间的转换矩阵，作为精对准开始时的转换矩阵。

是计算方向余弦矩阵的递归变量，根据四元数算法求得，即：

${\stackrel{\·}{C}}_s^i=C_s^i({\mathrm{\ω}}_{\mathrm{is}}^s\×),$

根据式求取精确的捷联姿态矩阵

其中，按照上述步骤（4）中的方法进行计算；

完成初始对准，进入导航状态。

本发明的优点：基于双轴转动机构的舰载机旋转式捷联惯导系统，在系统进行初始对准和导航时可以克服惯性器件常值误差对系统导航精度的影响，提高系统精度；在较为恶劣的对准环境中，特别是对晃动基座上载体角运动和线运动引起的不确定性干扰，通过渐消自适应Kalman滤波法可以精确地估计旋转式捷联惯导系统的初始姿态阵，有效地抑制了量测噪声中的动态随机干扰，减小了环境对系统的影响，使对准精度大幅提高；克服了传统解析粗对准和传统Kalman滤波法在动态随机干扰环境下无法使用的缺点，在没有其他外辅助测量设备、不增加成本的情况下，达到舰载机高精度快速自对准的适用要求。

四、附图说明

图1为旋转式捷联惯导系统的旋转方案图；

图2为粗对准阶段东向姿态误差角曲线图；

图3为粗对准阶段北向姿态误差角曲线图；

图4为粗对准阶段天向姿态误差角曲线图；

图5为精对准阶段失准角误差曲线图（以50次粗对准仿真结果中姿态误差角的均值作为精对准的初始失准角的滤波结果）；

图6为精对准阶段失准角误差曲线图（以50次粗对准仿真结果中姿态误差角的最大值作为精对准的初始失准角的滤波结果）；

图7为本发明的晃动基座下舰载机旋转式捷联惯导系统自对准方法流程图。

五、具体实施方式

下面结合附图对本发明的具体实施方式进行详细的描述：

结合图1～7，本发明晃动基座下舰载机旋转式捷联惯导系统自对准方法的步骤如图7所示，具体如下：

（1）通过全球定位系统GPS获得载体所在位置的经度λ、纬度L，将它们装订至导航计算机中；

（2）捷联惯导系统进行预热准备，将标定后的捷联惯导系统安装在双轴转台上，启动系统，采集惯性测量单元IMU中光纤陀螺和石英挠性加速度计的输出信号；

（3）系统在进行初始对准和导航过程中，采用双轴转台旋转惯性测量单元IMU来进行惯性器件常值误差的自动补偿，抑制三个轴向的陀螺常值漂移和加速度计零位偏置对系统导航精度的影响，组成旋转式捷联惯导系统，其具体步骤如下：

首先，对晃动基座下舰载机旋转式捷联惯导系统自对准过程中使用的坐标系进行定义：

1）地球坐标系（e系）：原点位于地心，oz_e轴沿地球自转轴方向，ox_e轴位于赤道平面内，从地心指向载体所在点的子午线，oy_e轴位于赤道平面内，ox_e、oy_e和oz_e轴满足右手定则。

2）地心惯性坐标系（i系）：在粗对准起始时刻t₀将o-x_ey_ez_e惯性凝固后形成的坐标系。

3）导航坐标系（n系）：本文采用地理坐标系为导航坐标系，原点位于载体重心，ox_n轴指向东，oy_n轴指向北，oz_n轴指向天。

4）载体坐标系（b系）：原点位于载体重心，ox_b、oy_b和oz_b轴分别沿载体横轴指向右、纵轴指向前，立轴指向上。

5）基座惯性坐标系（i_b0系）：在t₀时刻将载体坐标系经惯性凝固后的坐标系。

6）IMU坐标系（s系）：原点位于惯性测量单元IMU的重心，初始时刻IMU坐标系与载体坐标系重合，然后IMU以角速度ω绕旋转轴转动。IMU坐标系是随着IMU位置改变的一个时变坐标系。

惯性测量单元IMU是由三个相互正交的光纤陀螺和三个石英挠性加速度计构成。IMU被装在一个双环框架中，外环轴与载体坐标系的Ozb轴平行，内环轴位于垂直于外环轴的平面。IMU绕内环轴以角速度ω₁连续旋转，同时内环框架和IMU一起绕外环轴以角速度ω₂连续旋转（其中，ω₁是ω₂的整数倍），IMU绕内环轴旋转4周后以相同角速度反向继续连续旋转。同样，绕外环轴的旋转在旋转4周后改变方向，以此不停地进行下去；由此得到载体坐标系到IMU坐标系的姿态矩阵为：

$C_b^s=C_b^{\mathrm{sx}}{C}_b^{\mathrm{sz}}$

$=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\ω}}_{1}t& \sin {\mathrm{\ω}}_{1}t\\ 0& -\sin {\mathrm{\ω}}_{1}t& \cos {\mathrm{\ω}}_{1}t\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\ω}}_{2}t& \sin {\mathrm{\ω}}_{2}t& 0\\ -\sin {\mathrm{\ω}}_{2}t& \cos {\mathrm{\ω}}_{2}t& 0\\ 0& 0& 1\end{array}\right]$

$=\left[\begin{array}{ccc}\cos {\mathrm{\ω}}_{2}t& \sin {\mathrm{\ω}}_{2}t& 0\\ -\cos {\mathrm{\ω}}_{1}t\sin {\mathrm{\ω}}_{2}t& \cos {\mathrm{\ω}}_{1}t\cos {\mathrm{\ω}}_{2}t& \sin {\mathrm{\ω}}_{1}t\\ \sin {\mathrm{\ω}}_{1}t\sin {\mathrm{\ω}}_{2}t& -\sin {\mathrm{\ω}}_{1}t\cos {\mathrm{\ω}}_{2}t& \cos {\mathrm{\ω}}_{1}t\end{array}\right]$

其中，为IMU绕内环轴旋转的姿态转换阵，为IMU绕外环轴旋转的姿态转换阵。

（4）对采集到的光纤陀螺和石英挠性加速度计的数据进行处理，以惯性坐标系下的重力加速度作为参考矢量，计算出粗略的初始姿态阵。

在旋转式捷联惯导系统的粗对准算法中，姿态矩阵可分散成4个矩阵求取；

设对准位置的纬度为L，姿态矩阵的表达式为：

$C_s^n\left(t\right)=C_e^n{C}_i^e\left(t\right)C_{i_{b0}}^i{C}_s^{i_{b0}}\left(t\right),$

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

式中，为地球坐标系到导航坐标系的姿态转换阵，为惯性坐标系到地球坐标系的姿态转换阵，为基座惯性坐标系到惯性坐标系的姿态转换阵，为IMU坐标系到基座惯性坐标系的姿态转换阵，可表示t为粗对准过程时间。其中，为载体坐标系到基座惯性坐标系的姿态转换阵，并且t₀时刻的为单位矩阵，即

利用四元数法求解姿态更新公式从而得到

其中，的反对称阵，为摇摆状态下的旋转式捷联惯导系统中IMU坐标系下的陀螺输出，包含周期变化的地球自转角速度摇摆引起的干扰角速度δω^s、IMU旋转角速度和陀螺常值漂移ε，即：

${\mathrm{\ω}}_{i_{b0}s}^s={\mathrm{\ω}}_{\mathrm{ie}}^s+{\mathrm{\δ\ω}}^s+{\mathrm{\ω}}_{\mathrm{bs}}^s+\mathrm{\ϵ}$

采用双矢量定姿法求解为：

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

式中，Vⁱ为i系下的重力加速度的积分，为i_b0系下的重力加速度的积分。

$V^i\left(t_k\right)={\∫}_{t_0}^{t_k}(-g^i)\mathrm{dt}=-{\∫}_{t_0}^{t_k}{C}_e^i{C}_n^e{g}^n\mathrm{dt}=\left[\begin{array}{c}\frac{g\cos L}{{\mathrm{\ω}}_{\mathrm{ie}}}\sin {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_k\\ \frac{g\cos L}{{\mathrm{\ω}}_{\mathrm{ie}}}(1-\cos {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_k)\\ \mathrm{\Δ}{t}_{k}g\sin L\end{array}\right],$ Δt_k=t_k-t₀

其中，gⁱ为惯性坐标系下的重力加速度，gⁿ为导航坐标系下的重力加速度，ω_ie为地球自转角速度，t₀为粗对准开始时刻，t_k为粗对准结束时刻。

$V^{i_{b0}}\left(t_k\right)=V_f^{i_{b0}}\left(t_k\right)-V_{\mathrm{LA}}^{i_{b0}}\left(t_k\right)$

式中，为基座惯性坐标系下的速度，为基座惯性坐标系下的加速度计输出的积分，为基座惯性坐标系下的杆臂干扰加速度的积分。

$V_f^{i_{b0}}\left(t_k\right)={\∫}_{t_0}^{t_k}{f}^{i_{b0}}\mathrm{dt}={\∫}_{t_0}^{t_k}{C}_b^{i_{b0}}{C}_s^b{f}^s\mathrm{dt},$

$V_{\mathrm{LA}}^{i_{b0}}={\∫}_{t_0}^{t_k}{C}_b^{i_{b0}}{C}_s^b{a}_{\mathrm{LA}}^s\mathrm{dt}=C_s^{i_{b0}}({\mathrm{\ω}}_{\mathrm{is}}^s\×r^s),$

式中，为基座惯性坐标系下的加速度计输出，f^s为IMU坐标系下的加速度计输出，包含重力加速度g^s、线振动引起的干扰加速度杆臂干扰加速度和加速度计零偏即：

$f^s=-g^s+\mathrm{\δ}{a}_D^s+a_{\mathrm{LA}}^s+\▿$

（5）粗对准结束后，进入精对准阶段：建立惯性坐标系下的旋转式捷联惯导系统的失准角方程和速度误差方程。以速度误差、失准角、陀螺常漂移、加速度计零偏为状态量，建立系统的状态方程，以速度误差为观测量，建立系统的量测方程；

1）建立惯性坐标系下的旋转式捷联惯导系统的失准角方程和速度误差方程：

其中，为惯性坐标系下的失准角，δV为惯性坐标系下的速度误差，为IMU坐标系到惯性坐标系的姿态转换阵，ε_b、ε_w分别为陀螺的常值漂移和量测高斯白噪声；分别为加速度计的常值偏置误差和量测高斯白噪声。

2）建立系统的状态方程：

用一阶线性微分方程来描述旋转式捷联惯导系统的状态方程：

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W,$

其中，X(t)为t时刻系统的状态矢量，A(t)、B(t)分别为系统的状态转移矩阵和噪声矩阵，W为系统的噪声向量；

系统的状态矢量为：

系统的白噪声向量表示为：

$W={[{\▿}_{\mathrm{wx}},{\▿}_{\mathrm{wy}},{\▿}_{\mathrm{wz}},{\mathrm{\ϵ}}_{\mathrm{wx}},{\mathrm{\ϵ}}_{\mathrm{wy}},{\mathrm{\ϵ}}_{\mathrm{wz}},\mathrm{0,0,0,0,0,0}]}^T,$

其中，分别为惯性坐标系下东向、北向、天向的速度误差；分别为惯性坐标系下东向、北向、天向的失准角；ε_bx﹑ε_by﹑ε_bz分别为IMU坐标系下X、Y、Z轴的陀螺常值漂移；分别为IMU坐标系下X、Y、Z轴的加速度计常值零偏；ε_wx﹑ε_wy﹑ε_wz为陀螺X、Y、Z轴的高斯白噪声；为加速度计X、Y、Z轴的高斯白噪声。

系统的状态转移矩阵为：

$A=\left[\begin{array}{ccccc}{0}_{3\×3}& & -(g^i\×)& 0_{3\×3}& C_s^i\\ & 0_{3\×6}& -C_s^i& 0_{3\×3}& \\ & & 0_{3\×12}& & \\ & & 0_{3\×12}& & \end{array}\right],$

系统的噪声矩阵为：

$B=\left[\begin{array}{ccccccc}{C}_s^i& & 0_{3\×3}& & 0_{3\×3}& & 0_{3\×3}\\ & 0_{3\×3}& & -C_s^i& & 0_{3\×6}& \\ & & & 0_{3\×12}& & & \\ & & & 0_{3\×12}& & & \end{array}\right],$

3）建立系统的量测方程：

用一阶线性微分方程来描述旋转式捷联惯导系统的量测方程：

$Z\left(t\right)=H\left(t\right)X\left(t\right)+V_w+{\mathrm{\δV}}_D^i,$

其中，Z(t)为t时刻系统的量测向量，H(t)为系统的量测矩阵，为不确定性量测干扰，主要由垂荡、纵荡、横荡产生的干扰速度构成，V_w为量测高斯白噪声。

系统的量测矩阵为：

H(t)=[I_3×30_3×9]，

系统的量测向量为：

$Z=V_f^i+V_g^i-V_{\mathrm{LA}}^i,$

$V_f^i={\∫}_{t_1}^t{C}_s^i{f}^s\mathrm{d\τ},$

$V_g^i={\∫}_{t_1}^t{g}^i\mathrm{d\τ},$

$V_{\mathrm{LA}}^i{=\∫}_{t_1}^t{C}_s^i{a}_{\mathrm{LA}}^s\mathrm{d\τ}=C_s^i({\mathrm{\ω}}_{\mathrm{is}}^s\×r^s),$

其中，表示惯性坐标系下的加速度计输出的积分，表示惯性坐标系下的重力加速度的积分，表示惯性坐标系下的杆臂干扰加速度的积分；

（6）设计渐消自适应Kalman滤波器精确估计载体的失准角，用失准角修正捷联姿态矩阵，完成初始对准，进入导航状态。

旋转式捷联惯导系统的渐消自适应Kalman滤波方程为：

状态一步预测方程为：

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1},$

一步预测均方误差方程：

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

滤波增益方程：

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1},$

状态估值计算方程：

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}],$

估计均方误差方程：

P_k=[I-K_kH_k]P_k,k-1，

式中，渐消因子λ_k为：

${\mathrm{\λ}}_k=\frac{{\mathrm{\ϵ}}_k^T{\mathrm{\ϵ}}_k-\mathrm{tr}[H_k{Q}_{k-1}{H}_k^T+R_k]}{\mathrm{tr}\left[H_k{\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T{H}_k^T\right]}$

其中，残差表示为：

${\mathrm{\ϵ}}_k=Z_k-H_k{\hat{X}}_{k,k-1},$

λ_k值在滤波过程中自适应调整，以保证滤波器对量测值中不确定性干扰U_k的鲁棒性；

由于器件误差的存在，计算惯性坐标系与真实惯性坐标系并不重合。设i'系与i系间的失准角为通过步骤（5）精确估计出失准角则从i系到i′系的转换矩阵为：

$C_s^i\left(t\right)={\left[C_i^{i^{\′}}\left(t\right)\right]}^{-1}{C}_s^{i^{\′}}\left(t\right),$

$C_s^{i^{\′}}\left(t\right)=C_s^{i^{\′}}\left(0\right)\mathrm{\Δ}{C}_s^i,$

其中，是通过粗对准过程建立的IMU坐标系与计算惯性坐标系之间的转换矩阵，作为精对准开始时的转换矩阵。

是计算方向余弦矩阵的递归变量，根据四元数算法求得，即：

${\stackrel{\·}{C}}_s^i=C_s^i({\mathrm{\ω}}_{\mathrm{is}}^s\×),$

根据式求取精确的捷联姿态矩阵

其中，按照上述步骤（4）中的方法进行计算；

完成初始对准，进入导航状态。

本发明的有益效果如下：

在以下仿真环境下，对该方法进行Matlab仿真实验：

舰船受风浪波动影响，其航向角ψ、俯仰角θ、横滚角γ作周期变化：

存在横荡、纵荡和垂荡引起的线速度：

i=x,y,z

A_Dx=0.02m，A_Dy=0.03m，A_Dz=0.3m；ω_Di=2π/T_Di,且T_Dx=7s，T_Dy=6s，T_Dz=8s；为[0,2π]上服从均匀分布的随机相位；

初始地理位置：东经118°，北纬40°；

双轴旋转转速：ω₁=3ω₂=18°/s；

陀螺漂移：三个方向轴上的陀螺常值漂移为0.01°/h，随机漂移为加速度计零位偏置：三个方向轴上的加速度计常值偏置为1×10^-4g，随机偏置为

杆臂（单位为m）：[103010]^T；

赤道半径：R_e=6378165.0m；

椭球扁率：e=3.352e-3；

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

地球自转角速度（单位为rad/s）：7.2921158e-5；

常数：π=3.1415926；

首先对旋转式捷联惯导系统进行50次粗对准仿真，对准过程采用惯性系粗对准算法，每次粗对准仿真时间为120s，且取t_k1=50s，t_k2=120s。然后为了对比验证渐消自适应Kalman滤波和传统Kalman滤波算法对不确定性干扰的抑制效果，进行两组精对准仿真实验。每组仿真历时600s，并且以50次粗对准仿真结果中姿态误差角的均值和最大值分别作为精对准的初始失准角。

利用本发明所述方法得到粗对准阶段东向、北向、天向姿态误差角曲线如图2、3、4所示；精对准阶段东向、北向、天向失准角的估计误差曲线如图5、6所示，图5为以50次粗对准仿真结果中姿态误差角的均值作为精对准的初始失准角的滤波结果，图6为以50次粗对准仿真结果中姿态误差角的最大值作为精对准的初始失准角的滤波结果。

实验结果和结论：

由图2～4可知，在晃动基座上，本发明所述粗对准算法能有效地对舰载机旋转式捷联惯导系统的姿态作粗略的估算，姿态误差角可视为小角，在此基础上利用渐消自适应Kalman滤波法对姿态阵作精确估计完成精对准。由图5、6可知，在动态干扰情况下，渐消自适应Kalman滤波器能够较好地抑制不确定性干扰，滤波估计稳定。相比之下，传统Kalman滤波器虽然收敛速度快，但是滤波精度低，方位姿态角的滤波估计发生畸变、甚至发散。

实验结果表明：

本发明所述的粗对准方法具有较强的抗干扰能力，特别是对晃动基座上载体角运动和线运动引起的不确定性干扰的抑制效果明显。基于渐消自适应Kalman的自主精对准方法，有效地抑制了量测噪声中的不确定性干扰，实现舰载机旋转式捷联惯导系统的快速精确自对准。

Claims (1)

1.晃动基座下舰载机旋转式捷联惯导系统自对准方法，其特征是：

(1)通过全球定位系统GPS获得载体所在位置的经度λ、纬度L，将它们装订至导航计算机中；

(2)捷联惯导系统进行预热准备，将标定后的捷联惯导系统安装在双轴转台上，启动系统，采集惯性测量单元IMU中光纤陀螺和石英挠性加速度计的输出信号；

(3)系统在进行初始对准和导航过程中，采用双轴转台旋转惯性测量单元IMU来进行惯性器件常值误差的自动补偿，抑制三个轴向的陀螺常值漂移和加速度计零位偏置对系统导航精度的影响，组成旋转式捷联惯导系统，其具体步骤如下：

首先，对晃动基座下舰载机旋转式捷联惯导系统自对准过程中使用的坐标系进行定义：

1)地球坐标系(e系)：原点位于地心，oz_e轴沿地球自转轴方向，ox_e轴位于赤道平面内，从地心指向载体所在点的子午线，oy_e轴位于赤道平面内，ox_e、oy_e和oz_e轴满足右手定则；

2)地心惯性坐标系(i系)：在粗对准起始时刻t₀将o-x_ey_ez_e惯性凝固后形成的坐标系；

3)导航坐标系(n系)：本文采用地理坐标系为导航坐标系，原点位于载体重心，ox_n轴指向东，oy_n轴指向北，oz_n轴指向天；

4)载体坐标系(b系)：原点位于载体重心，ox_b、oy_b和oz_b轴分别沿载体横轴指向右、纵轴指向前，立轴指向上；

5)基座惯性坐标系(i_b0系)：在t₀时刻将载体坐标系经惯性凝固后的坐标系；

6)IMU坐标系(s系)：原点位于惯性测量单元IMU的重心，初始时刻IMU坐标系与载体坐标系重合，然后IMU以角速度ω绕旋转轴转动；IMU坐标系是随着IMU位置改变的一个时变坐标系；

惯性测量单元IMU是由三个相互正交的光纤陀螺和三个石英挠性加速度计构成；IMU被装在一个双环框架中，外环轴与载体坐标系的Ozb轴平行，内环轴位于垂直于外环轴的平面；IMU绕内环轴以角速度ω₁连续旋转，同时内环框架和IMU一起绕外环轴以角速度ω₂连续旋转，其中，ω₁是ω₂的整数倍，IMU绕内环轴旋转4周后以相同角速度反向继续连续旋转；同样，绕外环轴的旋转在旋转4周后改变方向，以此不停地进行下去；由此得到载体坐标系到IMU坐标系的姿态矩阵为：

$1\begin{array}{c}{C}_b^s=C_b^{\mathrm{sx}}{C}_b^{\mathrm{sz}}\\ =\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\ω}}_{1}t& \sin {\mathrm{\ω}}_{1}t\\ 0& -\sin {\mathrm{\ω}}_{1}t& \cos {\mathrm{\ω}}_{1}t\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\ω}}_{2}t& \sin {\mathrm{\ω}}_{2}t& 0\\ -\sin {\mathrm{\ω}}_{2}t& \cos {\mathrm{\ω}}_{2}t& 0\\ 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{ccc}\cos {\mathrm{\ω}}_{2}t& \sin {\mathrm{\ω}}_{2}t& 0\\ -\cos {\mathrm{\ω}}_{1}t\sin {\mathrm{\ω}}_{2}t& \cos {\mathrm{\ω}}_{1}t\cos {\mathrm{\ω}}_{2}t& \sin {\mathrm{\ω}}_{1}t\\ \sin {\mathrm{\ω}}_{1}t\sin {\mathrm{\ω}}_{2}t& -\sin {\mathrm{\ω}}_{1}t\cos {\mathrm{\ω}}_{2}t& \cos {\mathrm{\ω}}_{1}t\end{array}\right]\end{array}$

其中，为IMU绕内环轴旋转的姿态转换阵，为IMU绕外环轴旋转的姿态转换阵；

(4)对采集到的光纤陀螺和石英挠性加速度计的数据进行处理，以惯性坐标系下的重力加速度作为参考矢量，计算出粗略的初始姿态阵；

在旋转式捷联惯导系统的粗对准算法中，姿态矩阵可分散成4个矩阵求取；

设对准位置的纬度为L，姿态矩阵的表达式为：

$C_s^n\left(t\right)=C_e^n{C}_i^e\left(t\right)C_{i_{b0}}^i{C}_s^{i_{b0}}\left(t\right),$

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

式中，为地球坐标系到导航坐标系的姿态转换阵，为惯性坐标系到地球坐标系的姿态转换阵，为基座惯性坐标系到惯性坐标系的姿态转换阵，为IMU坐标系到基座惯性坐标系的姿态转换阵，可表示t为粗对准过程时间；其中，为载体坐标系到基座惯性坐标系的姿态转换阵，并且t₀时刻的为单位矩阵，即 $C_s^{i_{b0}}\left(t_0\right)=I;$

利用四元数法求解姿态更新公式从而得到

其中，为的反对称阵，为摇摆状态下的旋转式捷联惯导系统中IMU坐标系下的陀螺输出，包含周期变化的地球自转角速度摇摆引起的干扰角速度δω^s、IMU旋转角速度和陀螺常值漂移ε，即：

${\mathrm{\ω}}_{i_{b0}s}^s={\mathrm{\ω}}_{\mathrm{ie}}^s+{\mathrm{\δ\ω}}^s+{\mathrm{\ω}}_{\mathrm{bs}}^s+\mathrm{\ϵ}$

采用双矢量定姿法求解为：

${\hat{C}}_{i_0}^i={\left[\begin{array}{c}{\left(V^i\right)}^T\left(t_{k1}\right)\\ {\left(V^i\right)}^T\left(t_{k2}\right)\\ {[V^i\left(t_{k1}\right)\×V^i\left(t_{k2}\right)}^T\end{array}\right]}^T\left[\begin{array}{c}{\left(V^{i_{b0}}\right)}^T\left(t_{k1}\right)\\ {\left(V^{i_{b0}}\right)}^T\left(t_{k2}\right)\\ {[V^{i_{b0}}\left(t_{k1}\right)\×V^{i_{b0}}\left(t_{k2}\right)]}^T\end{array}\right],$

式中，Vⁱ为i系下的重力加速度的积分，为i_b0系下的重力加速度的积分；

$V^i\left(t_k\right)={\∫}_{t_0}^{t_k}(-g^i)\mathrm{dt}-{\∫}_{t_0}^{t_k}{C}_e^i{C}_n^e{g}^n\mathrm{dt}=\left[\begin{array}{c}\frac{g\cos L}{{\mathrm{\ω}}_{\mathrm{ie}}}\sin {\mathrm{\ω}}_{\mathrm{ie}}{\mathrm{\Δt}}_k\\ \frac{g\cos L}{{\mathrm{\ω}}_{\mathrm{ie}}}(1-\cos {\mathrm{\ω}}_{\mathrm{ie}}{\mathrm{\Δt}}_k)\\ {\mathrm{\Δt}}_{k}g\sin L\end{array}\right],{\mathrm{\Δt}}_k=t_k-t_0$

其中，gⁱ为惯性坐标系下的重力加速度，gⁿ为导航坐标系下的重力加速度，ω_ie为地球自转角速度，t₀为粗对准开始时刻，t_k为粗对准结束时刻；

$V^{i_{b0}}\left(t_k\right)=V_f^{i_{b0}}\left(t_k\right)-V_{\mathrm{LA}}^{i_{b0}}\left(t_k\right)$

式中，为基座惯性坐标系下的速度，为基座惯性坐标系下的加速度计输出的积分，为基座惯性坐标系下的杆臂干扰加速度的积分；

$V_f^{i_{b0}}\left(t_k\right)={\∫}_{t_0}^{t_k}{f}^{i_{b0}}\mathrm{dt}={\∫}_{t_0}^{t_k}{C}_b^{i_{b0}}{C}_s^b{f}^s\mathrm{dt},$

$V_{\mathrm{LA}}^{i_{b0}}={\∫}_{t_0}^{t_k}{C}_b^{i_{b0}}{C}_s^b{a}_{\mathrm{LA}}^s\mathrm{dt}=C_s^{i_{b0}}({\mathrm{\ω}}_{\mathrm{is}}^s\×r^s),$

式中，为基座惯性坐标系下的加速度计输出，f^s为IMU坐标系下的加速度计输出，包含重力加速度g^s、线振动引起的干扰加速度杆臂干扰加速度和加速度计零偏即：

$f^s=-g^s+{\mathrm{\δa}}_D^s+a_{\mathrm{LA}}^s+\▿$

(5)粗对准结束后，进入精对准阶段：建立惯性坐标系下的旋转式捷联惯导系统的失准角方程和速度误差方程；以速度误差、失准角、陀螺常漂移、加速度计零偏为状态量，建立系统的状态方程，以速度误差为观测量，建立系统的量测方程；

1)建立惯性坐标系下的旋转式捷联惯导系统的失准角方程和速度误差方程：

其中，为惯性坐标系下的失准角，δV为惯性坐标系下的速度误差，为IMU坐标系到惯性坐标系的姿态转换阵，ε_b、ε_w分别为陀螺的常值漂移和量测高斯白噪声； 分别为加速度计的常值偏置误差和量测高斯白噪声；

2)建立系统的状态方程：

用一阶线性微分方程来描述旋转式捷联惯导系统的状态方程：

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W,$

其中，X(t)为t时刻系统的状态矢量，A(t)、B(t)分别为系统的状态转移矩阵和噪声矩阵，W为系统的噪声向量；

系统的状态矢量为：

系统的白噪声向量表示为：

$W={[{\▿}_{\mathrm{wx}},{\▿}_{\mathrm{wy}},{\▿}_{\mathrm{wz}},{\mathrm{\ϵ}}_{\mathrm{wx}},{\mathrm{\ϵ}}_{\mathrm{wy}},{\mathrm{\ϵ}}_{\mathrm{wz}},\mathrm{0,0,0,0,0,0}]}^T,$

其中，分别为惯性坐标系下东向、北向、天向的速度误差；分别为惯性坐标系下东向、北向、天向的失准角；ε_bx﹑ε_by﹑ε_bz分别为IMU坐标系下X、Y、Z轴的陀螺常值漂移；分别为IMU坐标系下X、Y、Z轴的加速度计常值零偏；ε_wx﹑ε_wy﹑ε_wz为陀螺X、Y、Z轴的高斯白噪声；为加速度计X、Y、Z轴的高斯白噪声；

系统的状态转移矩阵为：

系统的噪声矩阵为：

3)建立系统的量测方程：

用一阶线性微分方程来描述旋转式捷联惯导系统的量测方程：

$Z\left(t\right)=H\left(t\right)X\left(t\right)+V_w+{\mathrm{\δV}}_D^i,$

其中，Z(t)为t时刻系统的量测向量，H(t)为系统的量测矩阵，为不确定性量测干扰，主要由垂荡、纵荡、横荡产生的干扰速度构成，V_w为量测高斯白噪声；

系统的量测矩阵为：

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

系统的量测向量为：

$Z=V_f^i+V_g^i-V_{\mathrm{LA}}^i,$

$V_f^i={\∫}_{t_1}^t{C}_s^i{f}^s\mathrm{d\τ},$

$V_g^i={\∫}_{t_1}^t{g}^i\mathrm{d\τ},$

$V_{\mathrm{LA}}^i={\∫}_{t_1}^t{C}_s^i{a}_{\mathrm{LA}}^s\mathrm{d\τ}=C_s^i({\mathrm{\ω}}_{\mathrm{is}}^s\×r^s),$

其中，表示惯性坐标系下的加速度计输出的积分，表示惯性坐标系下的重力加速度的积分，表示惯性坐标系下的杆臂干扰加速度的积分；

(6)设计渐消自适应Kalman滤波器精确估计载体的失准角，用失准角修正捷联姿态矩阵，完成初始对准，进入导航状态；

旋转式捷联惯导系统的渐消自适应Kalman滤波方程为：

状态一步预测方程为：

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1},$

一步预测均方误差方程：

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

滤波增益方程：

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1},$

状态估值计算方程：

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}],$

估计均方误差方程：

P_k＝[I-K_kH_k]P_k,k-1，

式中，渐消因子λ_k为：

${\mathrm{\λ}}_k=\frac{{\mathrm{\ϵ}}_k^T{\mathrm{\ϵ}}_k-\mathrm{tr}[H_k{Q}_{k-1}{H}_k^T+R_k]}{\mathrm{tr}\left[H_k{\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T{H}_k^T\right]}$

其中，残差表示为：

${\mathrm{\ϵ}}_k=Z_k-H_k{\hat{X}}_{k,k-1},$

λ_k值在滤波过程中自适应调整，以保证滤波器对量测值中不确定性干扰U_k的鲁棒性；

由于器件误差的存在，计算惯性坐标系与真实惯性坐标系并不重合；设i'系与i系间的失准角为通过步骤(5)精确估计出失准角则从i系到i′系的转换矩阵为：

$C_s^i\left(t\right)={\left[C_i^{i^{\′}}\left(t\right)\right]}^{-1}{C}_s^{i^{\′}}\left(t\right),$

$C_s^{i^{\′}}\left(t\right)=C_s^{i^{\′}}\left(0\right)\mathrm{\Δ}{C}_s^i,$

其中，是通过粗对准过程建立的IMU坐标系与计算惯性坐标系之间的转换矩阵，作为精对准开始时的转换矩阵；

是计算方向余弦矩阵的递归变量，根据四元数算法求得，即：

${\stackrel{\·}{C}}_s^i=C_s^i({\mathrm{\ω}}_{\mathrm{is}}^s\×),$

根据式求取精确的捷联姿态矩阵

其中，和按照上述步骤(4)中的方法进行计算；

完成初始对准，进入导航状态。