CN103471616A - 一种动基座sins大方位失准角条件下初始对准方法 - Google Patents

Abstract

本发明公开了一种动基座SINS大方位失准角条件下初始对准方法。利用GPS信息确定载体的初始位置参数，采集光纤陀螺仪和石英加速度计输出的数据，运用解析法来完成动基座SINS的粗对准，初步确定载体的姿态信息。建立动基座SINS在大方位失准角情况下的非线性状态方程，并建立动基座条件下以速度误差为观测量的量测方程，利用CKF算法估计出平台失准角，利用平台失准角修正系统的捷联初始姿态矩阵，从而得到精确的捷联初始姿态矩阵，从而完成动基座SINS的精对准过程。本发明可以大幅提高SINS在动基座且方位为大失准角情况下的初始对准精度，为导航过程提供了更加准确的初始姿态矩阵。

Description

一种动基座SINS大方位失准角条件下初始对准方法

技术领域

本发明涉及的是一种导航领域的初始对准技术，特别是涉及一种在海况不佳情况下舰船的动基座初始对准方法。

背景技术

初始对准是捷联惯性导航系统(Strapdown Inertial Navigation System，SINS)的关键技术之一，对准精度和对准速度是初始对准的重要技术指标。初始对准过程分为粗对准和精对准两部分，传统的初始对准过程利用粗对准粗略的估计出失准角的大小，然后利用线性微分方程误差模型，利用标准Kalman滤波最优估计方法精确估计出失准角的大小，从而有效的解决SINS初始对准问题。目前静基座初始对准方法已经很成熟，但是在舰船动基座情况下，尤其是在海况不佳的航行过程中，载体会产生大幅晃动，方位粗对准的精度很难保证，从而增加了初始对准的难度。因此研究动基座SINS的初始对准方法就显得十分必要。

在动基座中SINS中，粗对准精度较差，一般失准角较大，线性模型不能准确的描述真实的系统，需要利用非线性模型来描述SINS的误差特性。对于非线性系统来说，通常使用的滤波方法为扩展卡尔曼滤波(Extend Kalman Filter，EKF)。虽然EKF实现简便、使用广泛，但是当系统为强非线性时，很容易产生线性化误差，造成滤波器精度下降，甚至发散。容积卡尔曼滤波(CubatureKalmanFilter，CKF)是根据Cubature变换，通过一组具有相同权重的点集经过非线性系统方程的转换计算这组转换后的点集来给出下一时刻系统状态的预测。由于其估计精度高，不容易发散且计算量小的优点，CKF在非线性滤波问题中的应用越来越广泛。

发明内容

本发明的目的是提供可以在大方位失准角SINS动基座情况下大幅提高系统初始对准精度的一种基于CKF的初始对准算法。

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

(1)利用全球定位系统(Global Position System，GPS)信息确定载体的初始位置信息，根据光纤陀螺捷联惯导系统预热后采集光纤陀螺仪和石英挠性加速度计输出的数据，初步确定此时的姿态信息(纵摇角θ，横摇角γ和航向角ψ)，完成捷联惯导系统的粗对准。

(2)建立动基座SINS大方位失准角下的非线性误差模型

SINS非线性姿态误差方程为：

$\stackrel{\·}{\mathrm{\φ}}=(I-{\left(C_n^{n^{\′}}\right)}^T){\mathrm{\ω}}_{\mathrm{in}}^{n^{\′}}+\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{in}}^n-C_b^{n^{\′}}({\mathrm{\ϵ}}^b+w_g^b)$

其中φ=[φ_x  φ_y  φ_z]^T为失准角，表示计算导航坐标系n′与导航坐标系n之间的转动角度，n系到n′系的方向余弦矩阵用

表示；

为载体系b系到计算导航坐标系n′系的方向余弦矩阵；

为导航系n系相对于惯性系i系的转动角速度在计算导航系n′系上的取值，

为

的计算误差；ε^b为陀螺仪的常值漂移误差，

为其零均值高斯白噪声。

大方位失准角条件下的有：

$C_n^{n^{\′}}=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -{\sin \mathrm{\φ}}_z& \cos {\mathrm{\φ}}_z& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z& {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z& 1\end{array}\right]$

动基座下速度误差方程为：

$\mathrm{\δ}\stackrel{\·}{v}=[I-{\left(C_n^{n^{\′}}\right)}^T]C_b^{n^{\′}}{f}_{\mathrm{ib}}^b+C_b^n({\▿}^b+w_a^b)-(2{\mathrm{\ω}}_{\mathrm{ie}}^{n^{\′}}+{\mathrm{\ω}}_{\mathrm{en}}^{n^{\′}})\×\mathrm{\δv}-(2\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ie}}^n+\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{en}}^n)\×v+\mathrm{\δg}$

其中，为加速度计测量值；为载体系到导航系的方向余弦矩阵，其每个元素表示为C_ij(i，j=1，2，3)；为陀螺仪的常值漂移误差，

为其零均值高斯白噪声；

为地球自转角速度在计算导航系的取值，为地理系相对地球系的转动角速度在计算导航系的取值，

和分别为

和的计算误差；δv为速度误差，δgⁿ为重力加速度计算误差。

(3)建立SINS在动基座条件下的非线性滤波方程：

考虑经纬度误差、水平速度误差、失准角、水平加速度计零偏和陀螺仪常值漂移，则取12维状态向量：

其中δλ，

分别为经度误差和纬度误差；δv_x，δv_y分别为东向速度误差和北向速度误差；φ_x，φ_y，φ_z分别为东向、北向及方位失准角；

分别为x，y轴向的加速度计零偏；ε_x，ε_y，ε_z分别为x,y，z轴向的陀螺常值漂移。

将位置误差方程、速度误差方程和姿态误差方程展开，可以得到如下SINS动基座大方位失准角情况下的状态方程：

$\stackrel{\·}{X}=F\left(X\right)+\mathrm{GW}$

式中W为系统噪声向量，

$G=\left[\begin{array}{cccc}{0}_{2\×2}& 0_{2\×2}& 0_{2\×3}& 0_{2\×5}\\ 0_{2\×2}& G_1& 0_{2\×3}& 0_{2\×5}\\ 0_{3\×2}& 0_{3\×2}& G_2& 0_{3\×5}\\ 0_{5\×2}& 0_{5\×2}& 0_{5\×3}& 0_{5\×5}\end{array}\right],$ $G_1=\left[\begin{array}{cc}{C}_{11}& C_{12}\\ C_{21}& C_{22}\end{array}\right],$ $G_2=\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].$

其中R_M，R_N分别为地球子午面半径和卯酉面半径，

为当地地理纬度，C_ij(i，j=1，2，3)为方向余弦矩阵的相应元素。

以SINS与多普勒计程仪(Doppler Velocity Log，DVL)的速度之差为观测量，建立系统量测方程：

Z=HX+V

其中Z为系统的量测向量，H为量测矩阵，V为量测噪声。

利用CKF对动基座SINS的非线性状态进行估计，估计出失准角。

(4)利用步骤(3)中估计出的失准角来修正系统的捷联初始姿态矩阵，可以得到精确的捷联初始姿态矩阵，从而完成动基座下的精对准过程。

本发明的方法具有如下优点：

对准过程中，应用了动基座情况下的速度误差方程，从而解决了舰船在航行过程中海况不佳时的初始对准问题；姿态误差方程和速度误差方程均采用大方位失准角情况下的非线性形式，更加准确地描述了SINS的误差传播特性；应用新的非线性滤波方法CKF，提高了滤波精度和滤波收敛速度，从而解决了动基座SINS非线性系统初始对准的滤波问题。所以本发明可以大幅提高动基座SINS大方位失准角情况下初始对准的精度，从而提到导航系统的精度。

附图说明

图1为本发明的流程图；

图2为利用Matlab仿真得到的水平失准角估计误差曲线图；

图3为利用Matlab仿真得到的方位失准角估计误差曲线图；

图4为海上试验中舰船的航行速度；

图5为海上试验中所得姿态误差角。

具体实施方式

以下结合具体实施例，对本发明进行详细说明。

结合图1，下面举例对本发明做更详细的描述：

(1)利用GPS信息确定载体的初始位置信息，光纤陀螺捷联惯导系统充分预热后采集光纤陀螺仪和石英挠性加速度计输出的数据。

根据加速度计的输出与重力加速度的关系以及陀螺仪输出与地球旋转角速率的关系初步确定此时的姿态信息(纵摇角θ，横摇角γ和航向角ψ)，完成捷联惯导系统的粗对准，初步确定载体的姿态信息

$C_b^{n^{\′}}=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

(2)建立动基座SINS大方位失准角下的非线性误差模型

SINS非线性姿态误差方程为：

$\stackrel{\·}{\mathrm{\φ}}=(I-{\left(C_n^{n^{\′}}\right)}^T){\mathrm{\ω}}_{\mathrm{in}}^{n^{\′}}+\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{in}}^n-C_b^{n^{\′}}({\mathrm{\ϵ}}^b+w_g^b)---\left(1\right)$

其中φ＝[φ_x φ_y φ_z]^T为失准角，表示计算导航坐标系n′与导航坐标系n之间的转动角度，n系到n′系的方向余弦矩阵用

表示；

为载体系b系到计算导航坐标系n′系的方向余弦矩阵；

为导航系n系相对于惯性系i系的转动角速度在计算导航系n′系上的取值，

为

的计算误差；ε^b为陀螺仪的常值漂移误差，

为其零均值高斯白噪声。

$C_n^{n^{\′}}=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z-\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z+\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z& -\sin {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_x\\ -\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_x\\ \sin {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+\cos {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-\cos {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_x\end{array}\right]$

当水平失准角为小角度且方位失准角为大角度时，可化为：

$C_n^{n^{\′}}=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -\sin {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_z& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z& {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z& 1\end{array}\right]$

动基座下速度误差方程为：

$\mathrm{\δ}\stackrel{\·}{v}=[I-{\left(C_n^{m^{\′}}\right)}^T]C_b^{n^{\′}}{f}_{\mathrm{ib}}^b+C_b^n({\▿}^b+w_a^b)-(2{\mathrm{\ω}}_{\mathrm{ie}}^{n^{\′}}+{\mathrm{\ω}}_{\mathrm{en}}^{n^{\′}})\×\mathrm{\δv}-(2\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ie}}^n+2\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{en}}^n)\×v+\mathrm{\δg}---\left(2\right)$

其中，为加速度计测量值；为载体系到导航系的方向余弦矩阵，其每个元素表示为C_ij(i，j＝1，2，3)；

为陀螺仪的常值漂移误差，

为其零均值高斯白噪声；

为地球自转角速度在计算导航系的取值，

为地理系相对地球系的转动角速度在计算导航系的取值，

和

分别为

和

的计算误差；δv为速度误差，δgⁿ为重力加速度计算误差。

位置误差方程由经纬度误差组成，为：

其中，R_M、R_N分别为地球子午面半径和卯酉面半径，

为当地纬度值，v_x、v_y分别为东向和北向速度。

(3)建立SINS在动基座条件下的非线性滤波方程

考虑经纬度误差

、水平速度误差(δv_x，δv_y)、欧拉平台失准角(α_x，α_y，α_z)、水平加速度计零偏

和陀螺仪常值漂移(ε_x，ε_y，ε_z)，则取12维状态向量：

系统噪声向量为：

W＝[0_1×2 w_ax w_ay w_gx w_gy w_gz 0_1×5]^T

将(2)中的位置误差方程、姿态误差方程和速度误差方程展开，可以得到如下动基座SINS的非线性状态方程：

$\stackrel{\·}{X}=F\left(X\right)+\mathrm{GW}---\left(4\right)$

其中F(·)可以由式(1)～(3)展开得到，如下：

其中R_M，R_N分别为地球子午面半径和卯酉面半径，

为当地地理纬度，C_ij(i，j＝1，2，3)为方向余弦矩阵

的相应元素。

$G=\left[\begin{array}{cccc}{0}_{2\×2}& 0_{2\×2}& 0_{2\×3}& 0_{2\×5}\\ 0_{2\×2}& G_1& 0_{2\×3}& 0_{2\×5}\\ 0_{3\×2}& 0_{3\×2}& G_2& 0_{3\×5}\\ 0_{5\×2}& 0_{5\×2}& 0_{5\×3}& 0_{5\×5}\end{array}\right],$

$G_1=\left[\begin{array}{cc}{C}_{11}& C_{12}\\ C_{21}& C_{22}\end{array}\right],G_2=\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].$

以SINS与DVL的速度之差为观测量，建立量测方程：

Z=HX+V   (5)

其中Z为系统的量测向量，V为量测噪声，量测矩阵H为

H=[0_2×2 1_2×2 0_2×8]

以CKF滤波方程进行滤波估计，估计出欧拉平台失准角。

(4)利用步骤(3)中估计出的失准角来修正系统的捷联初始姿态矩阵，可以得到精确的捷联初始姿态矩阵

即从而完成动基座下的精对准过程。

对本发明有益的说明：

(1)Matlab仿真实验验证：

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

水面舰船SINS的三轴摇摆运动模型为：

$\left\{\begin{array}{c}\mathrm{\ψ}={\mathrm{\ψ}}_m\sin ({\mathrm{\ω}}_{\mathrm{\ψ}}t+{\mathrm{\ψ}}_0)+{\mathrm{\ψ}}_k\\ \mathrm{\θ}={\mathrm{\θ}}_m\sin ({\mathrm{\ω}}_{\mathrm{\θ}}t+{\mathrm{\θ}}_0)+{\mathrm{\θ}}_k\\ \mathrm{\γ}={\mathrm{\γ}}_m\sin ({\mathrm{\ω}}_{\mathrm{\γ}}t+{\mathrm{\γ}}_0)+{\mathrm{\γ}}_k\end{array}\right.$

其中为θ，γ，ψ分别为舰船的纵摇角、横摇角和航向角；摇摆幅值为θ_m=10°，γ_m=8°，ψ_m=6°；摇摆周期为T_θ=10s，T_γ=8s，T_ψ=6s；初始相角为θ₀=γ₀=ψ₀=0°；初始姿态角为θ_k=γ_k=0°，ψ_k=30°。

水面舰船的东向和北向初始速度为v_x=v_y=5m/s，舰船在0～30s内作匀加速直线运动，东向和北向加速度为α_x=α_y=0.2m/s²，之后舰船作匀速直线运动。

舰船的初始位置为：北纬45.7796°，东经126.6705°；

赤道半径：R_e=63783930m；

地球自转角速度：ω_ie=7.2921158×10^-5rad/s；

地球重力加速度：g₀=9.78049m/s²；

初始失准角：α_x=α_y=1°，α_x=10°；

陀螺仪常值漂移：ε=0.01°/h；

陀螺仪白噪声误差：w_g=0.001°/h；

加速度计零偏： $\▿={10}^{-4}{g}_{0}m/s^2;$

加速度计白噪声：w_a=10^-5g₀m/s²；

常数：π=3.1415926；

仿真时间：T=900s；

采样频率：Hn=0.1；

(2)海上试验验证：

利用实验空自研光纤陀螺在上海某港口进行海上试验验证，舰船的初始经纬度分别为：经度121.562422°，纬度31.28078°。以船载多普勒计程仪提供的速度为准确速度，舰船的航行速度如图4所示；以船载PHINS提供的姿态为标准姿态，利用本发明所述的方法对本次海上试验进行估计。

利用本发明所述的方法对失准角进行估计，如图1为仿真实验水平失准角估计误差；如图2为仿真实验方位失准角估计误差；图5为海上试验所得的姿态误差角。从图1、图2和图5可以看出，在大方位失准角动基座条件下，利用本发明可以获得很高的对准精度，而且收敛速度很快，从而有效的解决了大方位失准角条件下SINS动基座初始对准高精度与快速性的问题。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (1)

1.一种动基座SINS大方位失准角条件下初始对准方法，其特征在于，包括以下步骤：

(1)利用GPS信息确定载体的初始位置信息，根据光纤陀螺捷联惯导系统预热后采集光纤陀螺仪和石英挠性加速度计的输出数据，完成捷联惯导系统的粗对准，确定惯导系统初始姿态信息，所述惯导系统初始姿态信息包括纵摇角θ，横摇角γ和航向角ψ；

(2)建立动基座SINS大方位失准角下的非线性误差模型；

SINS非线性姿态误差方程为：

$\stackrel{\·}{\mathrm{\φ}}=(I-{\left(C_n^{n^{\′}}\right)}^T){\mathrm{\ω}}_{\mathrm{in}}^{n^{\′}}+\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{in}}^n-C_b^{n^{\′}}({\mathrm{\ϵ}}^b+w_g^b)$

其中φ＝[φ_x φ_y φ_z]^T为失准角，表示计算导航坐标系n′与导航坐标系n之间的转动角度，n系到n′系的方向余弦矩阵用

表示；为载体系b系到计算导航坐标系n′系的方向余弦矩阵；

为导航系n系相对于惯性系i系的转动角速度在计算导航系n′系上的取值，

为

的计算误差；ε^b为陀螺仪的常值漂移误差，为其零均值高斯白噪声；

大方位失准角条件下的有：

$C_n^{n^{\′}}=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -\sin {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_z& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z& {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z& 1\end{array}\right]$

动基座下速度误差方程为：

$\mathrm{\δ}\stackrel{\·}{v}=[I-{\left(C_n^{n^{\′}}\right)}^T]C_b^{n^{\′}}{f}_{\mathrm{ib}}^b+C_b^n({\▿}^b+w_a^b)-(2{\mathrm{\ω}}_{\mathrm{ie}}^{n^{\′}}+{\mathrm{\ω}}_{\mathrm{en}}^{n^{\′}})\×\mathrm{\δv}-(2\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\δ\ω}}_{\mathrm{en}}^n)\×v+\mathrm{\δg}$

其中，

为加速度计测量值；

为载体系到导航系的方向余弦矩阵，其每个元素表示为C_ij(i，j＝1，2，3)；

为陀螺仪的常值漂移误差，为其零均值高斯白噪声;

为地球自转角速度在计算导航系的取值，为地理系相对地球系的转动角速度在计算导航系的取值，

和

分别为

和的计算误差；δv为速度误差，δgⁿ为重力加速度计算误差；

(3)建立SINS在动基座条件下的非线性滤波方程：

考虑经纬度误差、水平速度误差、失准角、水平加速度计零偏和陀螺仪常值漂移，则取12维状态向量：

其中

分别为经度误差和纬度误差；δv_x，δv_y分别为东向速度误差和北向速度误差；φ_x，φ_y，φ_z分别为x，y，z轴向的失准角；分别为x,y轴向的加速度计零偏；ε_x，ε_y，ε_z分别为x，y，z轴向的陀螺常值漂移；

将位置误差方程、速度误差方程和姿态误差方程展开，可以得到如下SINS动基座大方位失准角情况下的状态方程：

$\stackrel{\·}{X}=F\left(X\right)+\mathrm{GW}$

式中W为系统噪声向量，

$G=\left[\begin{array}{cccc}{0}_{2\×2}& 0_{2\×2}& 0_{2\×3}& 0_{2\×5}\\ 0_{2\×2}& G_1& 0_{2\×3}& 0_{2\×5}\\ 0_{3\×2}& 0_{3\×2}& G_2& 0_{3\×5}\\ 0_{5\×2}& 0_{5\×2}& 0_{5\×3}& 0_{5\×5}\end{array}\right],$ $G_1=\left[\begin{array}{cc}{C}_{11}& C_{12}\\ C_{21}& C_{22}\end{array}\right],$ $G_2=\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]$

其中R_M，R_N分别为地球子午面半径和卯酉面半径，

为当地地理纬度，

C_ij(i，j＝1，2，3)为方向余弦矩阵

的相应元素；

以SINS与DVL的速度之差为观测量，建立量测方程：

Z＝HX+V

其中Z为系统的量测向量，H为量测矩阵，V为量测噪声；

利用CKF对动基座SINS的非线性状态进行估计，估计出失准角；

(4)利用步骤(3)中估计出的失准角来修正系统的捷联初始姿态矩阵，得到精确的捷联初始姿态矩阵，从而完成动基座下的精对准过程。

Images