一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法

摘要

一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法，该方法有四大步骤：一、利用已经对准好的主惯导系统信息，完成子惯导系统的粗对准及导航信息的初始化；二、主、子惯导系统分别进行导航解算，主惯导的速度、姿态信息传输到子惯导的导航计算机；三、在子惯导的导航计算机中，基于常用的速度/姿态匹配模式推导滤波器观测模型，建立含时间延迟的观测方程，进行标准离散卡尔曼滤迭代解算；四、滤波结束后，得到时间延迟的估计值及经过补偿过后的其他状态量的估计值。本发明通过滤波方法可以准确估计时间延迟误差，同时能有效抑制时间延迟对传递对准及惯性器件误差估计精度影响。

说明书

技术领域

本发明涉及一种惯导系统传递对准基准信息时间延迟的估计与补偿方法，尤其涉及一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法。属于惯性技术领域。

背景技术

在传递对准过程中，子惯导将主惯导信息作为基准信息进行初始对准。子惯导得到的基准信息会因为主子惯导时钟不同步、数据的传输处理等因素而存在时间延迟。当时间延迟存在时，子惯导不进行时间延迟的估计与补偿，直接利用接收到的信息会影响对准的精度。因此实现对时间延迟的估计与补偿，会提升传递对准的精度。

使用卡尔曼滤波方法，可以有效的对时间延迟进行估计与补偿，但现有的方法没有在提升传递对准精度的同时，实现抑制时间延迟对惯性器件误差估计精度的影响。

发明内容

本发明基于常用的速度/姿态匹配模式推导滤波器观测模型，建立含时间延迟模型的观测方程，提出一种传递对准时间延迟的滤波估计与补偿方法。目的在于准确估计时间延迟误差，同时有效抑制时间延迟对传递对准精度及惯性器件误差估计精度的影响。

技术方案：本发明一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法，该方法具体步骤如下：

步骤一：利用已经对准好的主惯导系统信息，完成子惯导系统的粗对准及导航信息的初始化；

步骤二：主、子惯导系统分别进行导航解算，主惯导的速度、姿态信息传输到子惯导的导航计算机；

步骤三：在子惯导的导航计算机中，基于常用的速度/姿态匹配模式推导滤波器观测模型，建立含时间延迟的观测方程，进行标准离散卡尔曼滤迭代解算；取游动自由方位系为子惯导的导航坐标系，所采用子惯导误差模型为：

$>\left(\begin{array}{c}\delta {\stackrel{·}{v}}_x=2{\Omega }_z{\mathrm{\delta v}}_y-({\rho }_y+2{\Omega }_y){\mathrm{\delta v}}_z-f_z{\psi }_y+f_y{\psi }_z+{▿}_x\\ \delta {\stackrel{·}{v}}_y=-2{\Omega }_z{\mathrm{\delta v}}_x+({\rho }_x+2{\Omega }_x){\mathrm{\delta v}}_z+f_z{\psi }_x-f_x{\psi }_z+{▿}_y\\ \delta {\stackrel{·}{v}}_z=({\omega }_y+{\Omega }_y){\mathrm{\delta v}}_x-({\omega }_x+{\Omega }_x){\mathrm{\delta v}}_y-f_y{\psi }_x+f_x{\psi }_y+{▿}_z\\ {\stackrel{·}{\psi }}_x={\Omega }_z{\psi }_y-{\omega }_y{\psi }_z-{ϵ}_x\\ {\stackrel{·}{\psi }}_y=-{\Omega }_z{\psi }_y+{\omega }_x{\psi }_z-{ϵ}_y\\ {\stackrel{·}{\psi }}_z={\omega }_y{\psi }_x-{\omega }_x{\psi }_y-{ϵ}_z\\ {\stackrel{·}{\eta }}_x=-\frac{1}{{\tau }_{\eta x}}\\ {\stackrel{·}{\eta }}_y=-\frac{1}{{\tau }_{\eta y}}\\ {\stackrel{·}{\eta }}_z=-\frac{1}{{\tau }_{\eta z}}\\ \stackrel{·}{\kappa }=-\frac{1}{{\tau }_{\kappa }}\end{array}\right)$>

其中δv_x、δv_y、δv_z为速度误差，ρ_x、ρ_y、ρ_z为载体运动角速率矢量分量，Ω_x、Ω_y、Ω_z为地球自转角速率矢量分量,ω表示ρ+Ω；▽_x、▽_y、▽_z为加速度计偏置，f为导航坐标系下载体感受的比力矢量；ψ_x、ψ_y、ψ_z为姿态误差，ε_x、ε_y、ε_z为陀螺漂移；η_x、η_y、η_z为主惯导相对子惯导的安装角误差在主惯导坐标系下的投影分量，τ_ηx、τ_ηy、τ_ηz分别为三轴向相对安装角误差的相关时间；κ为主惯导相对子惯导的延迟时间误差，τ_κ为延迟时间误差的相关时间。

子惯导器件误差模型：

陀螺的误差模型为

$>{\left(\begin{array}{c}{ϵ}_x\\ {ϵ}_y\\ {ϵ}_z\end{array}\right)}^n=C_b^n{\left(\begin{array}{c}{ϵ}_x\\ {ϵ}_y\\ {ϵ}_z\end{array}\right)}^b=\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)(\left(\begin{array}{c}{B}_x\\ B_y\\ B_z\end{array}\right)+\left(\begin{array}{ccc}{S}_{gx}& 0& 0\\ 0& S_{gy}& 0\\ 0& 0& S_{gz}\end{array}\right)\left(\begin{array}{c}{\omega }_{ib}^x\\ {\omega }_{ib}^y\\ {\omega }_{ib}^z\end{array}\right))$>

B_x、B_y、B_z为陀螺的零位误差项；S_gx、S_gy、S_gz为陀螺的标度误差项。C_ij(i,j＝1,2,3)为系统子惯导捷联矩阵中的第i行j列元素。

加速度计的误差模型为

$>{\left(\begin{array}{c}{▿}_x\\ {▿}_y\\ {▿}_z\end{array}\right)}^n=C_b^n{\left(\begin{array}{c}{▿}_x\\ {▿}_y\\ {▿}_z\end{array}\right)}^b=\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)(\left(\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right)+\left(\begin{array}{ccc}{S}_{ax}& 0& 0\\ 0& S_{ax}& 0\\ 0& 0& S_{ax}\end{array}\right)\left(\begin{array}{c}{f}_b^x\\ f_b^y\\ f_b^z\end{array}\right))$>

其中A_x、A_y、A_z为加速度计的零位误差项；S_ax、S_ay、S_az为加速度计的标度误差项。

所选系统状态变量为：

X(t)＝[δv_xδv_yδv_zψ_xψ_yψ_zη_xη_yη_zκB_xB_yB_zS_gxS_gyS_gzA_xA_yA_zS_axS_ayS_az]^T对应的系统状态方程为：

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

其中

$>F=\left(\begin{array}{cccccccc}{F}_{11}& F_{12}& 0_{3\times 3}& 0_{3\times 1}& 0_{3\times 3}& 0_{3\times 3}& F_{13}& F_{14}\\ 0_{3\times 3}& F_{21}& 0_{3\times 3}& 0_{3\times 1}& F_{22}& F_{23}& 0_{3\times 3}& 0_{3\times 3}\\ & & & 0_{16\times 22}& & & & \end{array}\right)$>

$>G=\left(\begin{array}{ccccc}{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}& 0_{3\times 12}\\ 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}& 0_{3\times 12}\\ 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 1}& 0_{3\times 12}\\ 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& I_{1\times 1}& 0_{1\times 12}\\ & & 0_{12\times 22}& & \end{array}\right)$>

$>W={\left(\begin{array}{ccccccccccc}{w}_{ax}& w_{ay}& w_{az}& w_{ϵx}& w_{ϵy}& w_{ϵz}& -\frac{1}{{\tau }_{\eta x}}& -\frac{1}{{\tau }_{\eta x}}& -\frac{1}{{\tau }_{\eta x}}& -\frac{1}{{\tau }_{\kappa }}& 0_{1\times 12}\end{array}\right)}^T$>

w_ax,w_ay,w_az为加速度计零偏随机白噪声，w_εx,w_εy,w_εz为陀螺仪漂移随机白噪声。

式中

$>\left(\begin{array}{cc}{F}_{11}=\left(\begin{array}{ccc}0& 2{\Omega }_z& -({\rho }_y+2{\Omega }_y)\\ -2{\Omega }_z& 0& {\rho }_x+2{\Omega }_x\\ {\rho }_y+2{\Omega }_y& -({\rho }_x+2{\Omega }_x)& 0\end{array}\right)& F_{12}=\left(\begin{array}{ccc}0& -f_z& f_y\\ f_z& 0& -f_x\\ -f_y& f_x& 0\end{array}\right)\end{array}\right)$>

$>\left(\begin{array}{cc}{F}_{13}=C_b^n=\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)& F_{14}=\left(\begin{array}{ccc}{f}_x& 0& 0\\ 0& f_y& 0\\ 0& 0& f_z\end{array}\right)\end{array}\right)$>

$>F_{21}=\left(\begin{array}{ccc}0& {\Omega }_z& -{\omega }_y\\ -{\Omega }_z& 0& {\omega }_x\\ {\omega }_y& -{\omega }_x& 0\end{array}\right)$>

$>F_{23}=\left(\begin{array}{ccc}{\omega }_{ibx}^n& 0& 0\\ 0& {\omega }_{iby}^n& 0\\ 0& 0& {\omega }_{ibz}^n\end{array}\right)$>

系统观测量：

Z＝[δv_xδv_yδv_zM₂₃-M₁₃M₁₂]^T

其中子惯导t时刻速度为此时主惯导速度采样有时间延迟κ，速度为主、子惯导速度之差

$>\delta V=V_s^t-V_m^{t-\kappa }\approx \delta v+\kappa ·f$>

子惯导t时刻捷联矩阵为导航坐标系相对于惯导本体坐标系的角速率在导航坐标系下的投影为此时主惯导姿态采样有时间延迟κ，捷联矩阵为取二者之乘积

$>M={\left({\bar{C}}_b^n\right)}_s^t·{\left(C_{b_1}^n\right)}_m^{\prime t-\kappa }\approx [I-\left(\left(\psi +C_{b_1}^n\eta +{\omega }_{bn}^n·\kappa \right)\times \right)]$>

式中$>(\psi \times )=\left(\begin{array}{ccc}0& -{\psi }_z& {\psi }_y\\ {\psi }_z& 0& -{\psi }_x\\ -{\psi }_y& {\psi }_x& 0\end{array}\right),$>其他类推。

观测方程为

Z＝HX+V

其中

$>H=\left(\begin{array}{ccccc}{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& f& 0_{3\times 12}\\ 0_{3\times 3}& I_{3\times 3}& C_{b_1}^n& {\omega }_{bn}^n& 0_{3\times 12}\end{array}\right)$>

时间延迟量测值计算方法

考虑到闭环卡尔曼滤波可以减少模型线性化误差的影响、准确估计系统状态变量，将上述时间延迟量测值计算过程进行改进。设子惯导t时刻时间延迟估计值为相对安装角误差估计值为

1)速度量测值

2)姿态量测值

步骤四：滤波结束后，得到时间延迟的估计值及经过补偿过后的其他状态量的估计值。

优点及功效：本发明一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法，该方法的优点是：本发明通过滤波方法可以准确估计时间延迟误差，同时能有效抑制时间延迟对传递对准及惯性器件误差估计精度的影响。和没有时间延迟误差时相比，应用本发明滤波方法后传递对准精度以及器件主要零位、标度误差项估计精度变化可忽略，可以有效抑制基准信息时延对传递对准性能的影响。

附图说明

图1为本发明流程框图。

具体实施方式

见图1，本发明一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法，该方法具体步骤如下：

步骤一：利用已经对准好的主惯导系统信息，完成子惯导系统的粗对准及导航信息的初始化；

步骤二：主、子惯导系统分别进行导航解算，主惯导的速度、姿态信息传输到子惯导的导航计算机；

步骤三：在子惯导的导航计算机中，基于常用的速度/姿态匹配模式推导滤波器观测模型，建立含时间延迟模型的观测方程，进行标准离散卡尔曼滤迭代结算，具体包括如下五个步骤：

(1)子惯导误差方程的建立；(2)传递对准系统状态方程的建立；(3)建立考虑时间延迟的速度加姿态匹配方法的量测方程；(4)时间延迟量测值计算方法；(5)卡尔曼滤迭代解算。

(1)子惯导误差方程的建立。取游动自由方位系为子惯导的导航坐标系，可得子惯导的误差方程为

1)速度误差方程

$>\left(\begin{array}{c}\delta {\stackrel{·}{v}}_x=2{\Omega }_z\delta v_y-({\rho }_y+2{\Omega }_y)\delta v_z-f_z{\psi }_y+f_y{\psi }_z+{▿}_x\\ \delta {\stackrel{·}{v}}_y=-2{\Omega }_z{\mathrm{\delta v}}_x+({\rho }_x+2{\Omega }_x){\mathrm{\delta v}}_z+f_z{\psi }_x-f_x{\psi }_z+{▿}_y\\ \delta {\stackrel{·}{v}}_z=({\omega }_y+{\Omega }_y){\mathrm{\delta v}}_x-({\omega }_x+{\Omega }_x){\mathrm{\delta v}}_y-f_y{\psi }_x+f_x{\psi }_y+{▿}_z\end{array}\right)$>

其中δv_x、δv_y、δv_z为速度误差，ρ_x、ρ_y、ρ_z为载体运动角速率矢量分量，Ω_x、Ω_y、Ω_z为地球自转角速率矢量分量,ω表示ρ+Ω；▽_x、▽_y、▽_z为加速度计偏置，f为导航坐标系下载体感受的比力矢量；

2)姿态误差方程

$>\left(\begin{array}{c}{\stackrel{·}{\psi }}_x={\Omega }_z{\psi }_y-{\omega }_y{\psi }_z-{ϵ}_x\\ {\stackrel{·}{\psi }}_y=-{\Omega }_z{\psi }_y+{\omega }_x{\psi }_z-{ϵ}_y\\ {\stackrel{·}{\psi }}_z={\omega }_y{\psi }_x-{\omega }_x{\psi }_y-{ϵ}_z\end{array}\right)$>

ψ_x、ψ_y、ψ_z为姿态误差，ε_x、ε_y、ε_z为陀螺漂移；

3)相对安装角误差方程

$>\left(\begin{array}{c}{\stackrel{·}{\eta }}_x=-\frac{1}{{\tau }_{\eta x}}\\ {\stackrel{·}{\eta }}_y=-\frac{1}{{\tau }_{\eta y}}\\ {\stackrel{·}{\eta }}_z=-\frac{1}{{\tau }_{\eta z}}\end{array}\right)$>

η_x、η_y、η_z为主惯导相对子惯导的安装角误差在主惯导坐标系下的投影分量，τ_ηx、τ_ηy、τ_ηz分别为三轴向相对安装角误差的相关时间。

4)延迟时间误差方程

$>\stackrel{·}{\kappa }=-\frac{1}{{\tau }_{\kappa }}$>

κ为主惯导相对子惯导的延迟时间误差，τ_κ为延迟时间误差的相关时间。

5)陀螺、加速度计误差方程

一般情形下精确标定后的弹载惯组的安装误差变化较为缓慢，但零位误差、标度误差不能忽略。陀螺的误差模型为：

$>{\left(\begin{array}{c}{ϵ}_x\\ {ϵ}_y\\ {ϵ}_z\end{array}\right)}^n=C_b^n{\left(\begin{array}{c}{ϵ}_x\\ {ϵ}_y\\ {ϵ}_z\end{array}\right)}^b=\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)(\left(\begin{array}{c}{B}_x\\ B_y\\ B_z\end{array}\right)+\left(\begin{array}{ccc}{S}_{gx}& 0& 0\\ 0& S_{gy}& 0\\ 0& 0& S_{gz}\end{array}\right)\left(\begin{array}{c}{\omega }_{ib}^x\\ {\omega }_{ib}^y\\ {\omega }_{ib}^z\end{array}\right))$>

B_x、B_y、B_z为陀螺的零位误差项；S_gx、S_gy、S_gz为陀螺的标度误差项。C_ij(i,j＝1,2,3)为系统子惯导捷联矩阵中的第i行j列元素。

加速度计的误差模型为：

$>{\left(\begin{array}{c}{▿}_x\\ {▿}_y\\ {▿}_z\end{array}\right)}^n=C_b^n{\left(\begin{array}{c}{▿}_x\\ {▿}_y\\ {▿}_z\end{array}\right)}^b=\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)(\left(\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right)+\left(\begin{array}{ccc}{S}_{ax}& 0& 0\\ 0& S_{ax}& 0\\ 0& 0& S_{ax}\end{array}\right)\left(\begin{array}{c}{f}_b^x\\ f_b^y\\ f_b^z\end{array}\right))$>

其中A_x、A_y、A_z为加速度计的零位误差项；S_ax、S_ay、S_az为加速度计的标度误差项。

(2)传递对准系统状态方程的建立。综合子惯导的误差方程，得到传递对准系统状态方程，状态变量为

X(t)＝[δv_xδv_yδv_zψ_xψ_yψ_zη_xη_yη_zκB_xB_yB_zS_gxS_gyS_gzA_xA_yA_zS_axS_ayS_az]^T

对应的系统状态方程为：

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

其中

$>F=\left(\begin{array}{cccccccc}{F}_{11}& F_{12}& 0_{3\times 3}& 0_{3\times 1}& 0_{3\times 3}& 0_{3\times 3}& F_{13}& F_{14}\\ 0_{3\times 3}& F_{21}& 0_{3\times 3}& 0_{3\times 1}& F_{22}& F_{23}& 0_{3\times 3}& 0_{3\times 3}\\ & & & 0_{16\times 22}& & & & \end{array}\right)$>

$>G=\left(\begin{array}{ccccc}{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}& 0_{3\times 12}\\ 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}& 0_{3\times 12}\\ 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 1}& 0_{3\times 12}\\ 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& I_{1\times 1}& 0_{1\times 12}\\ & & 0_{12\times 22}& & \end{array}\right)$>

$>W={\left(\begin{array}{ccccccccccc}{w}_{ax}& w_{ay}& w_{az}& w_{ϵx}& w_{ϵy}& w_{ϵz}& -\frac{1}{{\tau }_{\eta x}}& -\frac{1}{{\tau }_{\eta x}}& -\frac{1}{{\tau }_{\eta x}}& -\frac{1}{{\tau }_{\kappa }}& 0_{1\times 12}\end{array}\right)}^T$>

w_ax,w_ay,w_az为加速度计零偏随机白噪声，w_εx,w_εy,w_εz为陀螺仪漂移随机白噪声。

式中

$>\left(\begin{array}{cc}{F}_{11}=\left(\begin{array}{ccc}0& 2{\Omega }_z& -({\rho }_y+2{\Omega }_y)\\ -2{\Omega }_z& 0& {\rho }_x+2{\Omega }_x\\ {\rho }_y+2{\Omega }_y& -({\rho }_x+2{\Omega }_x)& 0\end{array}\right)& F_{12}=\left(\begin{array}{ccc}0& -f_z& f_y\\ f_z& 0& -f_x\\ -f_y& f_x& 0\end{array}\right)\end{array}\right)$>

$>\left(\begin{array}{cc}{F}_{13}=C_b^n=\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)& F_{14}=\left(\begin{array}{ccc}{f}_x& 0& 0\\ 0& f_y& 0\\ 0& 0& f_z\end{array}\right)\end{array}\right)$>

$>\left(\begin{array}{cc}{F}_{21}=\left(\begin{array}{ccc}0& {\Omega }_z& -{\omega }_y\\ -{\Omega }_z& 0& {\omega }_x\\ {\omega }_y& -{\omega }_x& 0\end{array}\right)& F_{22}=C_b^n=\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)\end{array}\right)$>

$>F_{23}=\left(\begin{array}{ccc}{\omega }_{ibx}^n& 0& 0\\ 0& {\omega }_{iby}^n& 0\\ 0& 0& {\omega }_{ibz}^n\end{array}\right)$>

(3)建立考虑时间延迟的速度加姿态匹配方法的量测方程。

子惯导系统在时刻t得到主惯导系统在时刻(t-κ)的数据，速度和姿态量测方程分别为：

1)速度量测方程

子惯导t时刻速度为此时主惯导速度采样有时间延迟κ，速度为主、子惯导速度之差

$>\delta V=V_s^t-V_m^{t-\kappa }\approx \delta v+\kappa ·f$>

传递对准速度量测模型为

$>\left(\begin{array}{c}{Z}_1\left(t\right)=\delta V=H_{1}X\left(t\right)+{\xi }_1\left(t\right)\\ =\left[{\begin{array}{cccc}{I}_{3\times 3}& 0_{3\times 6}& f& 0\end{array}}_{3\times 12}\right]X\left(t\right)+{\xi }_1\left(t\right)\end{array}\right)$>

Z₁(t)为速度观测矢量，ξ₁(t)为速度量测噪声，I为单位阵。

2)姿态量测方程

子惯导t时刻捷联矩阵为导航坐标系相对于惯导本体坐标系的角速率在导航坐标系下的投影为此时主惯导姿态采样有时间延迟κ，捷联矩阵为取二者之乘积

$>M\approx [I-\left(\left(\psi +C_{b_1}^n\eta +{\omega }_{bn}^n·\kappa \right)\times \right)]$>

式中$>(\psi \times )=\left(\begin{array}{ccc}0& -{\psi }_z& {\psi }_y\\ {\psi }_z& 0& -{\psi }_x\\ -{\psi }_y& {\psi }_x& 0\end{array}\right),$>其他类推。

传递对准姿态量测模型为

$>\left(\begin{array}{c}{Z}_2\left(t\right)={\left(\begin{array}{ccc}{M}_{23}& -M_{13}& M_{12}\end{array}\right)}^T\\ =H_{2}X\left(t\right)+{\xi }_2\left(t\right)\\ =\left(\begin{array}{ccccc}{0}_{3\times 3}& I_{3\times 3}& C_{b_1}^n& {\omega }_{bn}^n& 0_{0\times 12}\end{array}\right)X\left(t\right)+{\xi }_2\left(t\right)\end{array}\right)$>

上标T表示矩阵转置，Z₂(t)为姿态观测矢量，ξ₂(t)为姿态量测噪声。

(4)时间延迟量测值计算方法。

考虑到闭环卡尔曼滤波可以减少模型线性化误差的影响、准确估计系统状态变量，将上述时间延迟量测值计算过程进行改进。设子惯导t时刻时间延迟估计值为相对安装角误差估计值为

3)速度量测值

4)姿态量测值

(5)依据所建系统状态方程和观测方程，结合主、子惯导信息进行卡尔曼滤波迭代解算。

步骤四：滤波结束后，得到时间延迟的估计值及经过补偿过后的其他状态量的估计值。