CN104236586B - 基于量测失准角的动基座传递对准方法 - Google Patents

Abstract

本发明提供一种基于量测失准角的动基座传递对准方法，包括以下步骤：步骤1、子惯导系统利用主惯导系统采用固定频率发送的导航参数完成粗对准，前述导航参数包括速度、姿态和位置信息；步骤2、采用“量测失准角+速度”匹配方式建立系统状态方程、系统观测方程；步骤3、子惯导系统利用主惯导以固定频率发送给子惯导系统的速度与姿态信息构造观测量；以及步骤4、采用卡尔曼滤波进行迭代解算，估算子惯导系统的姿态失准角及速度、位置误差的状态估算值，并据此对子惯导系统的导航信息进行修正，完成传递对准。

Description

基于量测失准角的动基座传递对准方法

技术领域

本发明属于惯性导航技术领域，是一种基于量测失准角的动基座传递对准方法。

背景技术

传递对准是将子惯导输出的导航信息与主惯导输出的相应导航信息相比较，并以此对子惯导姿态误差角进行最优估计的一种方法。它是解决机载武器和任务传感器在空中动基座条件下初始对准问题的主要方法。

目前，在失准角为小角度条件下的快速传递对准技术，在国内，一般采用“速度+姿态”匹配方式，在快速性和精确性方面都能满足需求。但在某些特殊的姿态角时，使部分量测矩阵为0，导致某些失准角误差难以估计。在国外文献中，使用主、子惯导姿态矩阵的乘积结果作为量测量的方法得到广泛的使用。量测失准角匹配法是在文献《Rapid transferalignment for tactical weapon applications》中提出的量测失准角的基础上发展和推导的。

在《西北工业大学学报》第25卷第5期中，文献《传递对准中一种新的姿态匹配算法》中，推导了平台失准角，量测失准角和实际失准角三者间的关系，并采用该方法进行仿真，精度和快速性均能满足要求。但不能满足大失准角的情况。

发明内容

本发明目的在于提供一种可用于大失准角条件下的基于量测失准角的动基座传递对准方法，能够满足快速性与精确性要求。

本发明的上述目的通过独立权利要求的技术特征实现，从属权利要求以另选或有利的方式发展独立权利要求的技术特征。

为达成上述目的，本发明所采用的的技术方案如下：

一种基于量测失准角的动基座传递对准方法，包括以下步骤：

步骤1、子惯导系统利用主惯导系统采用固定频率发送的导航参数完成粗对准，前述导航参数包括速度、姿态和位置信息；

步骤2、采用“量测失准角+速度”匹配方式建立系统状态方程、系统观测方程；

步骤3、子惯导系统利用主惯导以固定频率发送给子惯导系统的速度与姿态信息构造观测量；以及

步骤4、采用卡尔曼滤波进行迭代解算，估算子惯导系统的姿态失准角及速度、位置误差的状态估算值，并据此对子惯导系统的导航信息进行修正，完成传递对准。

由以上本发明的技术方案可知，本发明的有益效果在于：

1、相对于传统的传递对准算法，本发明的方法具有形式简单、易于理解，可大大减小计算量的优点。

2、本发明所提出的对准方法适用于大失准角条件，对主惯导姿态进行补偿构造观测量后，可在线性模型下直接实现。

附图说明

图1为本发明一实施方式基于量测失准角的动基座传递对准方法的实现流程图。

图2、3、4分别为利用图1实施例的方法实现的经过卡尔曼滤波对准后子惯导的姿态角估计误差曲线示意图，其中：图2为x轴姿态角估计误差曲线，图3为y轴姿态角估计误差曲线，图4为z轴姿态角估计误差曲线。

具体实施方式

为了更了解本发明的技术内容，特举具体实施例并配合所附图式说明如下。

如图1所示，根据本发明的较优实施例，一种基于量测失准角的动基座传递对准方法，应用于由主惯导系统和子惯导系统组成的惯性导航系统，前述主惯导系统和子惯导系统均为捷联式惯性导航系统，该方法的实现包括以下步骤：

步骤1、子惯导系统利用主惯导系统采用固定频率发送的导航参数完成粗对准，前述导航参数包括速度、姿态和位置信息；

步骤2、采用“量测失准角+速度”匹配方式建立系统状态方程、系统观测方程；

步骤3、子惯导系统利用主惯导以固定频率发送给子惯导系统的速度与姿态信息构造观测量；以及

步骤4、采用卡尔曼滤波进行迭代解算，估算子惯导系统的姿态失准角及速度、位置误差的状态估算值，并据此对子惯导系统的导航信息进行修正，完成传递对准。

下面结合附图1-4所示，详细说明上述各步骤的具体实施。

在前述步骤1中，子惯导系统利用主惯导的导航参数完成粗对准，即初始化过程。

在前述步骤2中，采用“量测失准角+速度”匹配方式建立系统状态方程、系统观测方程，具体如下：

步骤2-1、根据捷联惯导系统的力学编排建立惯导误差模型，得到系统的误差方程，包括姿态误差方程、速度误差方程及惯性器件误差方程；

步骤2-2、根据惯导误差模型及系统误差方程建立捷联惯导系统的状态方程。

作为可选的方式，前述步骤2-1中，姿态误差方程、速度误差方程及惯性器件误差方程建立如下：

$\mathrm{\δ}{\stackrel{\·}{V}}^n=f^n\×{\mathrm{\φ}}^n-2(\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\δ\ω}}_{\mathrm{en}}^n)\×V^n-2({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×{\mathrm{\δV}}^n+{\▿}^n$

$\stackrel{\·}{\mathrm{\φ}}=-{\mathrm{\ω}}_{\mathrm{in}}^n\×\mathrm{\φ}-C_b^n\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ib}}^b+\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{in}}^n$

$\stackrel{\·}{\mathrm{\ϵ}}=0$

$\stackrel{\·}{\▿}=0$

$\stackrel{\·}{\mathrm{\λ}}=0$

式中，φⁿ为子惯导系统的失准角， 为地球自传角速率在导航坐标下的投影，为导航系相对地球系的旋转角速率，为子惯导的姿态矩阵，δV＝[δV_e δV_n δV_u]为子惯导系统的东、北、天方向的速度误差， $V^n=\left[\begin{array}{ccc}{V}_e^n& V_n^n& V_u^n\end{array}\right]$ 分别为主惯导系统的东、北、天方向的速度，w_ie为地球转动角速率， $f^n=\left[\begin{array}{ccc}{f}_e^n& f_n^n& f_u^n\end{array}\right]$ 是主惯导系统加速度计测量到的比力通过坐标转换后得到的导航系下东、北、天三个方向上的比力分量，ε＝[ε_xε_y ε_z]为子惯导陀螺在体坐标系下的随机常值漂移，▽＝[▽_x ▽_y ▽_z]为子惯导加速度计在体坐标系下的随机常值偏置，λ＝[λ_x λ_y λ_z]为子惯导系统相对主惯导系统的安装误差角在体坐标系下的分量；

作为可选的方式，前述步骤2-2中，捷联惯导系统的状态方程建立如下：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}$

式中X为系统状态矢量，F为系统状态转移矩阵，G为系统噪声驱动矩阵，W为系统噪声矢量，具体如下：

1)X为系统状态矢量：

X＝[φ δV ε ▽ λ]_15×1

2)F为系统状态转移矩阵：

系统状态转移矩阵中矩阵块F₁与F₂如下所示：

$F_1=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)& 0& -\frac{V_N}{R_M+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}& \frac{V_N}{R_M+h}& 0\\ 0& -f_U& f_N\\ f_U& 0& -f_E\\ -f_N& f_E& 0\end{array}\right]$

$F_2=\left[\begin{array}{ccc}0& -\frac{1}{R_M+h}& 0\\ \frac{1}{R_N+h}& 0& 0\\ \frac{\tan L}{R_N+h}& 0& 0\\ 0& {2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L& -({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ -({2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)& 0& \frac{V_N}{R_M+h}\\ {2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}& \frac{V_N}{R_M+h}& 0\end{array}\right]$

其中，R_m为子午圈曲率半径；R_n为卯酉圈曲率半径；h为载体当前的高度；L为载体当前所处位置的纬度。

在前述步骤2中，捷联惯导系统的观测方程建立如下：

Z＝HX+V

式中，为观测量，为量测失准角在x、y、z轴上的分量；V是量测噪声且为均值为零的高斯白噪声，其协方差为E[VV^T]＝R，R为量测噪声方差阵；H为系统的观测矩阵：

$H={\left[\begin{array}{ccccc}{-C}_n^m& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]}_{6\×15}$

其中：I为单位矩阵，为主惯导系统姿态矩阵的转置。

结合图1所示，在前述步骤3中，子惯导系统利用主惯导以固定频率发送给子惯导系统的速度与姿态信息构造观测量。

设矩阵表示s′系相对主惯导坐标系的姿态矩阵，即主惯导坐标系旋转角度λ'后即得到s′系。角度λ'为子惯导系统相对主惯导系统的安装误差角的大概值，可通过子惯导自对准或其他仪器粗略测出，通过补偿这个角度λ'，可将大失准角问题转换为子惯导体坐标系与s′系间的小失准角问题。如此，子惯导系统利用主惯导系统发送速度与姿态信息构造观测量即可转化为构造小失准角问题的观测量：

式中，为主、子惯导系统体坐标系间的转换矩阵，为子惯导系统姿态矩阵的转置，为主惯导系统姿态矩阵， ×为×乘运算符；

推导公式如下：

对上式右边展开，可得

$\begin{array}{c}{C}_m^{s^{\′}}=(I-\mathrm{\λ}\×)(I+C_n^m\mathrm{\φ}\×C_m^n)\\ =I+C_n^m(\mathrm{\φ}\×)C_m^n-(\mathrm{\λ}\×)+(\mathrm{\λ}\×)C_n^m(\mathrm{\φ}\×)C_m^n\end{array}$

根据反对称矩阵的性质有并带入上式，可得：

去掉反对称形式得：即由此可得到观测量如前述的观测量观测量δV_e,δV_n,δV_u由主惯导的速度减去子惯导的速度得到，即 $\left(\begin{array}{c}{\mathrm{\δV}}_e\\ {\mathrm{\δV}}_n\\ {\mathrm{\δV}}_u\end{array}\right)=\left(\begin{array}{c}{V}_{\mathrm{me}}^n\\ V_{\mathrm{mn}}^n\\ V_{\mathrm{mu}}^n\end{array}\right)-\left(\begin{array}{c}{V}_{\mathrm{se}}^n\\ V_{\mathrm{sn}}^n\\ V_{\mathrm{su}}^n\end{array}\right),$ 其中， $V_m^n=\left[\begin{array}{ccc}{V}_{\mathrm{me}}^n& V_{\mathrm{mn}}^n& V_{\mathrm{mu}}^n\end{array}\right]$ 为主惯导的东北天速度， $V_s^n=\left[\begin{array}{ccc}{V}_{\mathrm{se}}^n& V_{\mathrm{sn}}^n& V_{\mathrm{su}}^n\end{array}\right]$ 为子惯导的东北天速度，由此可得到观测方程中的全部的观测量(由此可得前述观测方程)。

结合图1，在步骤4中，根据前述步骤建立的系统状态方程、系统观测方程以及系统的观测量进行卡尔曼滤波迭代解算，估计子惯导系统的姿态失准角及速度、位置误差的状态估算值，并对子惯导系统的导航参数进行补偿，完成传递对准。

前述系统状态方程、系统观测方程离散化后可得：

X_k＝Φ_k,k-1X_k-1+Γ_k-1W_k-1

Z_k＝H_kX_k+V_k

其中，X_k表示被估计状态，Φ_k,k-1为t_k-1时刻的一步转移矩阵，Γ_k-1为k-1时刻系统噪声驱动阵，W_k为系统激励噪声序列，Z_k为观测量，H_k为量测阵，V_k为量测噪声序列。

根据适用于离散系统的卡尔曼滤波器基本方程的卡尔曼滤波算法具体如下：

状态一步预测方程：

X_k/k-1＝φ_k,k-1X_k-1

其中，X_K/K-1为k-1时刻到k时刻的系统状态一步预测值，X_k-1为k-1时刻系统状态估计值，φ_k,k-1为k-1时刻到k时刻的系统状态转移矩阵；

一步预测均方误差方程：

$P_{k/k-1}={\mathrm{\φ}}_{k,k-1}{P}_{k-1}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

其中，P_k/k-1为k-1时刻到k时刻的系统状态协方差阵，P_k-1为k-1时刻的系统状态协方差阵，Q_k-1为k-1时刻系统噪声矩阵；

最优滤波增益方程：

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

其中，K_k为k时刻系统增益矩阵，H_k为k时刻系统量测矩阵，R_k为k时刻系统量测噪声矩阵；

状态估计方程：

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)

估计均方误差方程：

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_{k-1}^T$

其中，Q_k是系统过程噪声方差阵，P_k为均方误差阵。

联立前述状态一步预测方程、一步预测均方误差方程、最优滤波增益方程、状态估计方程和估计均方误差方程，采用卡尔曼滤波算法进行状态估计，估计出状态矢量X＝[φδV ε ▽ λ]_15×1的状态估计值。

然后，利用状态矢量X＝[φ δV ε ▽ λ]_15×1中的姿态失准角φ的估计值和速度误差δV的估计值对子惯导系统的导航信息进行修正，从而完成传递对准。

为了进一步对上述实施例的方法进行说明，充分展现该方法的可靠性与精确性，进行了以下数字仿真试验。

(1)仿真初始条件及参数设置

11)载体初始速度为东向速度0m/s、北向速度100m/s、天向速度0m/s；载体初始位置：纬度为30°，经度为110°，高度为1000m；载体初始姿态为：俯仰角为0°，滚转角为0°，偏航角为0°。

12)子惯导加速度计常值偏置设为5mg；子惯导陀螺常值漂移设为10(deg/h)；子惯导在三个轴向相对于主惯导的安装角设为10°、20°、30°。

13)仿真总时间为50s，滤波周期设为0.1s，采取加速+爬升机动模拟飞机起飞完成传递对准。

(2)仿真试验结果及分析

图2-4给出了经过卡尔曼滤波对准后子惯导的姿态角(x、y、z轴)估计误差曲线。由图可以看出子惯导系统的三个轴向的姿态角估计误差均能在10s内快速收敛到0.1°以内。并且在对准过程中只需简单的机动即可完成对准。综上所述，本方法在大失准角条件下可以有效地、准确地对子惯导姿态失准角进行估计与补偿，满足传递对准的高精度和快速性要求。

虽然本发明已以较佳实施例揭露如上，然其并非用以限定本发明。本发明所属技术领域中具有通常知识者，在不脱离本发明的精神和范围内，当可作各种的更动与润饰。因此，本发明的保护范围当视权利要求书所界定者为准。

Claims (3)

1.一种基于量测失准角的动基座传递对准方法，应用于由主惯导系统和子惯导系统组成的惯性导航系统，前述主惯导系统和子惯导系统均为捷联式惯性导航系统，其特征在于，该方法的实现包括以下步骤：

步骤1、子惯导系统利用主惯导系统采用固定频率发送的导航参数完成粗对准，前述导航参数包括速度、姿态和位置信息；

步骤2、采用“量测失准角+速度”匹配方式建立系统状态方程、系统观测方程；

步骤3、子惯导系统利用主惯导以固定频率发送给子惯导系统的速度与姿态信息构造观测量；以及

步骤4、采用卡尔曼滤波进行迭代解算，估算子惯导系统的姿态失准角及速度、位置误差的状态估算值，并据此对子惯导系统的导航信息进行修正，完成传递对准；

其中，在前述步骤2中，采用“量测失准角+速度”匹配方式建立系统状态方程、系统观测方程，具体如下：

步骤2-1、系统姿态误差方程、速度误差方程及惯性器件误差方程建立如下：

$\mathrm{\δ}{\stackrel{\·}{V}}^n=f^n\×{\mathrm{\φ}}^n-2({\mathrm{\δ\ω}}_{ie}^n+{\mathrm{\δ\ω}}_{en}^n)\×V^n-2({\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×{\mathrm{\δV}}^n+{\▿}^n$

$\stackrel{\·}{\mathrm{\φ}}=-{\mathrm{\ω}}_{in}^n\×\mathrm{\φ}-C_b^n{\mathrm{\δ\ω}}_{ib}^b+{\mathrm{\δ\ω}}_{in}^n$

$\stackrel{\·}{\mathrm{\ϵ}}=0$

$\stackrel{\·}{\▿}=0$

$\stackrel{\·}{\mathrm{\λ}}=0$

式中，φⁿ为子惯导系统的失准角， 为地球自传角速率在导航坐标下的投影，为导航系相对地球系的旋转角速率，为子惯导的姿态矩阵，δV＝[δV_e δV_n δV_u]为子惯导系统的东、北、天方向的速度误差，分别为主惯导系统的东、北、天方向的速度，w_ie为地球转动角速率，是主惯导系统加速度计测量到的比力通过坐标转换后得到的导航系下东、北、天三个方向上的比力分量，ε＝[ε_x ε_y ε_z]为子惯导陀螺在体坐标系下的随机常值漂移，为子惯导加速度计在体坐标系下的随机常值偏置，λ＝[λ_x λ_y λ_z]为子惯导系统相对主惯导系统的安装误差角在体坐标系下的分量；

分别表示对对应参数的微分；

前述的系统状态方程建立如下：

$\stackrel{\·}{X}=FX+GW$

式中，X为系统状态矢量，F为系统状态转移矩阵，G为系统噪声驱动矩阵，W为系统噪声矢量，且：

1)X为系统状态矢量：

$X={\left[\begin{array}{ccccc}\mathrm{\φ}& \mathrm{\δ}V& \mathrm{\ϵ}& \▿& \mathrm{\λ}\end{array}\right]}_{15\×1}$

2)F为系统状态转移矩阵：

式中，矩阵块F₁与F₂表达如下：

$F_1=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R_N+h}\tan L& -({\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_N+h})\\ -({\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R_N+h}\tan L)& 0& -\frac{V_N}{R_M+h}\\ {\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_N+h}& \frac{V_N}{R_M+h}& 0\\ 0& -f_U& f_N\\ f_U& 0& -f_E\\ -f_N& f_E& 0\end{array}\right]$

$F_2=\left[\begin{array}{ccc}0& -\frac{1}{R_M+h}& 0\\ \frac{1}{R_N+h}& 0& 0\\ \frac{\tan L}{R_N+h}& 0& 0\\ 0& 2{\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R_N+h}\tan L& -(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_N+h})\\ -(2{\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R_N+h}\tan L)& 0& -\frac{V_N}{R_M+h}\\ 2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_N+h}& \frac{V_N}{R_M+h}& 0\end{array}\right]$

其中，R_m为子午圈曲率半径；R_n为卯酉圈曲率半径；h为载体当前的高度；L为载体当前所处位置的纬度；

步骤2-2、系统观测方程建立如下：

Z＝HX+V

式中，为观测量，为量测失准角在x、y、z轴上的分量；V是量测噪声且为均值为零的高斯白噪声，其协方差为E[VV^T]＝R，R为量测噪声方差阵；H为系统的观测矩阵：

$H={\left[\begin{array}{ccccc}-C_n^m& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]}_{6\×15}$

其中：I为单位矩阵，为主惯导系统姿态矩阵的转置。

2.根据权利要求1所述的基于量测失准角的动基座传递对准方法，其特征在于，所述步骤3中，按照下述关系构造观测量Z，

且

由此可得：

由上式可得到观测量Z中的且观测量δV_e,δV_n,δV_u由主惯导的速度减去子惯导的速度得到，即其中，为主惯导的东北天速度，为子惯导的东北天速度，由此可得到前述系统观测方程中的全部的观测量。

3.根据权利要求2所述的基于量测失准角的动基座传递对准方法，其特征在于，所述步骤4的实现包括：

将前述系统状态方程、系统观测方程离散化后得到：

X_k＝Φ_k,k-1X_k-1+Γ_k-1W_k-1

Z_k＝H_kX_k+V_k

其中，X_k表示被估计状态，Φ_k,k-1为t_k-1时刻的一步转移矩阵，Γ_k-1为k-1时刻系统噪声驱动阵，W_k为系统激励噪声序列，Z_k为观测量，H_k为量测阵，V_k为量测噪声序列；

根据适用于离散系统的卡尔曼滤波器基本方程的卡尔曼滤波算法具体如下：

状态一步预测方程：

X_k/k-1＝φ_k,k-1X_k-1

其中，X_K/K-1为k-1时刻到k时刻的系统状态一步预测值，X_k-1为k-1时刻系统状态估计值，φ_k,k-1为k-1时刻到k时刻的系统状态转移矩阵；

一步预测均方误差方程：

$P_{k/k-1}={\mathrm{\φ}}_{k,k-1}{P}_{k-1}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

其中，P_k/k-1为k-1时刻到k时刻的系统状态协方差阵，P_k-1为k-1时刻的系统状态协方差阵，Q_k-1为k-1时刻系统噪声矩阵；

最优滤波增益方程：

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

其中，K_k为k时刻系统增益矩阵，H_k为k时刻系统量测矩阵，R_k为k时刻系统量测噪声矩阵；

状态估计方程：

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)

估计均方误差方程：

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_{k-1}^T$

其中，Q_k是系统过程噪声方差阵，P_k为均方误差阵；

联立前述状态一步预测方程、一步预测均方误差方程、最优滤波增益方程、状态估计方程和估计均方误差方程，采用卡尔曼滤波算法进行状态估计，估计出状态矢量的状态估计值；

然后，利用状态矢量中的姿态失准角φ的估计值和速度误差δV的估计值对子惯导系统的导航信息进行修正，从而完成传递对准。