CN105021212A - 一种初始方位信息辅助下潜航器快速传递对准方法 - Google Patents

Abstract

本发明针对现有传递对准方法在保证潜航器惯导对准精度的条件下，不能实现移动基准与潜航器快速分离和一次性多潜航器布放的问题，公开了一种初始方位信息辅助下潜航器快速传递对准方法。本发明包括以下四步：1、初始信息装订，信息装订完毕后潜航器与移动基准即分离；2、潜航器姿态更新方程、速度更新方程、位置更新方程确定；3、卡尔曼滤波状态方程与观测方程确定；4、卡尔曼滤波并对潜航器姿态、速度误差修正，完成传递对准。本发明实现了移动基准与潜航器的快速分离，同时能够保证潜航器惯导对准精度，且能实现一次性多潜航器布放。

Description

一种初始方位信息辅助下潜航器快速传递对准方法

技术领域

本发明涉及一种传递对准方法，特别是一种初始方位信息辅助下潜航器快速传递对准方法。

背景技术

潜航器遂行作战任务之前，由舰船将其带入指定地点，然后将其投放；之后，潜航器利用其自身惯性导航设备在多普勒测速仪辅助下进行自主导航。潜航器布放前其惯性导航***的初始对准，可以选择采用组合导航初始对准的方式进行自对准，也可以以舰船惯导***或移动基准惯导***为主惯导***进行传递对准。

采用组合导航自对准方式时不需要舰船惯导***提供数据，但动基座环境下自对准通常需要较长时间才能完成初始对准过程，难以达到快速对准的要求。为了满足快速对准的需求，目前国内外多采取了利用主惯导或移动基准惯导***(MINS)为信息源辅助子惯导***(SINS)进行传递对准，这就需要主惯导***实时与子惯导***进行一段时间的测量参数匹配或者导航参数匹配，这种传递对准方法存在的问题是：不能实现主惯导或移动基准惯导与潜航器的快速分离，影响潜航器的快速布放，战场环境下，不仅影响潜航器作战效能的发挥，还会对携带潜航器进入战场环境的移动基准载体的生存能力构成威胁，并且按传统传递对准方式逐一进行传递对准，难以实现短时间同时布放多个潜航器的任务。因此，寻找一种能实现移动基准惯导与潜航器惯导快速分离，同时能够保证潜航器惯导对准精度，且能实现一次性多潜航器布放的传递对准方式具有重要意义。

发明内容

本发明针对现有传递对准方法在保证潜航器惯导对准精度的条件下，不能实现移动基准惯导与潜航器惯导的快速分离，且不能实现一次性多潜航器布放的问题，提出了一种初始方位信息辅助下潜航器快速传递对准方法。

该方法只需移动基准惯导在初始对准开始时刻提供潜航器惯导***方位信息及概略的水平姿态信息后，潜航器随即布放入水，其惯导***在多普勒测速仪的速度信息辅助下估计惯性系下的初始水平姿态误差和当前的速度误差，进行组合导航自对准。由于水平姿态误差和速度误差可通过卡尔曼滤波快速收敛，可实现移动基准惯导与潜航器惯导快速分离，且能满足短时间投放较多数量潜航器的快速对准要求。

为实现本发明所采取的技术解决方案是：

一种初始方位信息辅助下潜航器快速传递对准方法，包含以下步骤：

步骤一：移动基准惯导(MINS)对每一个潜航器惯导(SINS)装订初始位置(包括初始经度λ₀、初始纬度L₀)、初始速度初始姿态信息(包括准确的方位角存在误差的俯仰角与横滚角)，装订完成后，潜航器与移动基准分离；

其中，t₀表示对准初始时刻，表示潜航器相对于地球的速度在地理坐标系n系下的投影，地理坐标系n系定义为北向-东向-地向(N-E-D)，地球坐标系e系的原点位于地心，e_z轴沿地球自转轴方向，e_x轴在赤道平面内指向格林威治子午线，e_y轴与另外两轴构成右手正交坐标系；

表示惯性坐标系系到惯性坐标系系的方向余弦矩阵，系由对准初始时刻载体坐标系b系与惯性空间固联得到(相对惯性空间固定不动)，系由对准初始时刻n系与惯性空间固联得到(相对惯性空间固定不动)，载体坐标系b系轴向沿载体横滚轴-俯仰轴-偏航轴(前-右-下)；

步骤二：潜航器惯导(SINS)建立姿态更新方程、速度更新方程、位置更新方程分别如下：

姿态更新方程： $C_b^n=C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}---\left(1\right)$

其中，

n₀系定义为对准初始时刻的地理坐标系，相对地球表面固定不动，不随导航***在地球表面运动而运动；

为对准初始时刻导航系n₀系到当前时刻导航系n系的方向余弦矩阵；

为惯性坐标系系到对准初始时刻导航系n₀系的方向余弦矩阵；

为惯性坐标系系到惯性坐标系系的方向余弦矩阵；

为载体坐标系b系到惯性坐标系系的方向余弦矩阵；

速度更新方程： ${\mathrm{\ν}}_e^n\left(t_k\right)={\mathrm{\ν}}_e^n\left(t_{k-1}\right)+{\mathrm{\Δv}}_a+{\mathrm{\Δv}}_b+{\mathrm{\Δv}}_c---\left(2\right)$

其中，

${\mathrm{\Δv}}_a=C_{n_0}^n\left(t_{k-1}\right)C_{i_{n_0}}^{n_0}\left(t_{k-1}\right)C_{i_{b_0}}^{i_{n_0}}\left(t_{k-1}\right){\∫}_{t_{k-1}}^{t_k}{C}_b^{i_{b_0}}{f}^{b}dt$

${\mathrm{\Δv}}_b=\mathrm{\Δ}t\left(\right(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v_e^n\left(t_{k-1}\right))$

${\mathrm{\Δv}}_c={\mathrm{\Δtg}}_l^n$

t_k-1、t_k分别表示第k-1、k个潜航器速度更新时刻，为对应的速度，f^b为加表比力输出，为t_k-1时刻相应的方向余弦矩阵，Δt＝t_k-t_k-1为惯导更新时间间隔，为地理系下的地球自转角速度，为地理系下的转移角速度，为地理系下的重力；

位置更新方程： $\begin{array}{c}{L}_k=L_{k-1}+\mathrm{\Δ}t\·v_n/(R_N+h)\\ {\mathrm{\λ}}_k={\mathrm{\λ}}_{k-1}+\mathrm{\Δ}t\·v_e/\[(R_N+h){\mathrm{cosL}}_{k-1}\]\end{array}---\left(3\right)$

潜航器经纬度通过航迹推算进行更新，其中，

L_k-1、L_k分别表示k-1、k时刻潜航器的纬度，λ_k-1、λ_k分别表示k-1、k时刻潜航器的经度，v_n、v_e表示潜航器的北向、东向速度，R_N表示子午面曲率半径，h表示高度；

步骤三：构建卡尔曼滤波状态方程与观测方程如下：

选取横滚角误差δθ₀、俯仰角误差δγ₀、北向速度误差δv_n、东向速度误差δv_e为***状态，即

x＝[δθ₀ δγ₀ δv_n δv_e]^T       (4)

构建***状态方程如下，

$\stackrel{\·}{x}=Fx+Gw---\left(5\right)$

其中，

上标T表示矩阵或向量的转置，x表示***状态，表示状态微分，F表示状态转移矩阵，G表示***噪声矩阵； $w={\left[\begin{array}{cc}{\mathrm{\δ\ω}}_{i_b}^b& {\mathrm{\δf}}^b\end{array}\right]}^T,$ 为陀螺输出噪声，δf^b为加表输出噪声；

取1s更新一次的多普勒速度为观测量，构建观测方程如下，

z＝Hx+v         (6)

其中，

多普勒坐标系d系轴向沿多普勒测速仪的横滚轴-俯仰轴-偏航轴(前-右-下)；

z为观测向量，H为观测矩阵，v为测量噪声；

步骤四：根据卡尔曼滤波状态方程和观测方程进行滤波，对横滚角误差、俯仰角误差、北向速度误差、东向速度误差进行估计，对潜航器的姿态、速度进行修正，完成传递对准；

其中，在步骤二中所述的姿态更新方程按如下方法更新：

$C_{n_0}^n\≈I_{3\×3}+\left[\begin{array}{ccc}0& -(\mathrm{\λ}-{\mathrm{\λ}}_0+{\mathrm{\ω}}_{ie}t)\sin t& L_0-L\\ (\mathrm{\λ}-{\mathrm{\λ}}_0{\mathrm{\ω}}_{ie}t)\sin L& 0& -(\mathrm{\λ}-{\mathrm{\λ}}_0+{\mathrm{\ω}}_{ie}t)\cos L\\ L-L_0& (\mathrm{\λ}-{\mathrm{\λ}}_0+{\mathrm{\ω}}_{ie}t)\cos L& 0\end{array}\right]---\left(7\right)$

其中，I_3×3表示三维单位矩阵，ω_ie为地球自转角速度，t为初始时刻到当前时刻的解算时间，λ₀、L₀为潜航器装订的初始经度、纬度，λ、L为当前时刻经度、纬度；

可由潜航器初始经纬度和t确定，将按如下分解，

$C_{i_{n_0}}^{n_0}=C_e^{n_0}{C}_{i_{e_0}}^e{C}_{i_{n_0}}^{i_{e_0}}---\left(8\right)$

其中，

$\begin{array}{c}{C}_e^{n_0}=\left[\begin{array}{ccc}-{\mathrm{sinL}}_0{\mathrm{cos\λ}}_0& -{\mathrm{sinL}}_0{\mathrm{sin\λ}}_0& {\mathrm{cosL}}_0\\ {\mathrm{cosL}}_0{\mathrm{cos\λ}}_0& {\mathrm{cosL}}_0{\mathrm{sin\λ}}_0& {\mathrm{sin\λ}}_0\\ -{\mathrm{sin\λ}}_0& {\mathrm{cos\λ}}_0& 0\end{array}\right]\\ C_{i_{e_0}}^e=\left[\begin{array}{ccc}\cos \left({\mathrm{\ω}}_{ie}t\right)& \sin \left({\mathrm{\ω}}_{ie}t\right)& 0\\ -\sin \left({\mathrm{\ω}}_{ie}t\right)& \cos \left({\mathrm{\ω}}_{ie}t\right)& 0\\ 0& 0& 1\end{array}\right],C_{i_{n_0}}^{i_{e_0}}={\left(C_e^{n_0}\right)}^T\end{array}---\left(9\right)$

i_e0系由对准初始时刻e系与惯性空间固联得到(相对惯性空间固定不动)；

为地球坐标系e系到对准初始时刻导航系n₀系的方向余弦矩阵；

为惯性坐标系系到地球坐标系e系的方向余弦矩阵；

为惯性坐标系系到惯性坐标系系的方向余弦矩阵；

利用陀螺输出数据求解，传递对准完成时间内，忽略陀螺的零偏、随机游走等误差，按如下微分方程和初值进行姿态更新求解，即

${\stackrel{\·}{C}}_b^{i_{b_0}}=C_b^{i_{b_0}}\[{\mathrm{\ω}}_{ib}^b\×\],C_b^{i_{b_0}}\left(0\right)=I_{3\×3}---\left(10\right)$

其中，为载体角速度，为的初值；

在步骤三中所述的卡尔曼滤波状态方程按如下方法构建：

1)水平欧拉角误差微分方程构建如下

水平欧拉角误差为常值，其微分为零，即

${\left[\begin{array}{cc}\mathrm{\δ}{\stackrel{\·}{\mathrm{\θ}}}_0& \mathrm{\δ}{\stackrel{\·}{\mathrm{\γ}}}_0\end{array}\right]}^T=0_{2\×1}---\left(11\right)$

2)速度误差微分方程构建如下

①构建估算的姿态矩阵失准角与水平欧拉角误差δθ₀、δγ₀间的关系

其中，φ为估算的姿态矩阵失准角，

进一步，将(13)代入的表达式(12)并化简可得，

②构建包含水平欧拉角误差δθ₀、δγ₀的速度误差微分方程

潜航器相对于地球真实速度在n系下的速度微分方程为，

${\stackrel{\·}{\mathrm{\ν}}}_e^n=C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{f}^b+(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×{\mathrm{\ν}}_e^n+g_l^n---\left(15\right)$

估算速度微分方程为，

${\stackrel{\·}{\tilde{v}}}_e^n=C_{n_0}^n{C}_{i_{n_0}}^{n_0}{\tilde{C}}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{\tilde{f}}^b+(2{\mathrm{\ω}}_{ie}^n+{\widetilde{\mathrm{\ω}}}_{en}^n)\×{\tilde{v}}_e^n+g_l^n---\left(16\right)$

(16)式与(15)式相减得速度误差微分方程，

$\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_e^n={\stackrel{\·}{\tilde{v}}}_e^n-{\stackrel{\·}{v}}_e^n\\ =(C_{n_0}^n{C}_{i_{n_0}}^{n_0}{\tilde{C}}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{\tilde{f}}^b-C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{f}^b)-\[(2{\mathrm{\ω}}_{ie}^n+{\widetilde{\mathrm{\ω}}}_{\mathrm{en}}^n)\×{\tilde{v}}_e^n-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v_e^n\]\\ =M_1\mathrm{\φ}+M_2{\mathrm{\δv}}_e^n+M_3{\mathrm{\δf}}^b\end{array}---\left(17\right)$

$M_1=-C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}\[C_b^{i_{b_0}}{f}^b\×\],M_2=-\left(\right(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×),M_3=C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}$

其中，为有误差的比力输出，为估算速度，为速度误差，为估算的转移角速度；

将(14)式代入(17)式，得到包含水平欧拉角误差δθ₀、δγ₀的北向速度误差δv_n、东向速度误差δv_e微分方程如下，

$\begin{array}{c}{\left[\begin{array}{cc}\mathrm{\δ}{\stackrel{\·}{v}}_n& \mathrm{\δ}{\stackrel{\·}{v}}_e\end{array}\right]}^T\\ ={\[{\begin{array}{cc}{\stackrel{\·}{\tilde{v}}}_n& \stackrel{\·}{\tilde{v}}\end{array}}_e\]}^T-{\left[\begin{array}{cc}{\stackrel{\·}{v}}_n& {\stackrel{\·}{v}}_e\end{array}\right]}^T\\ =\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]M_{1}M{\left[\begin{array}{cc}{\mathrm{\δ\θ}}_0& {\mathrm{\δ\γ}}_0\end{array}\right]}^T+\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]M_2{\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]}^T{\left[\begin{array}{cc}{\mathrm{\δv}}_n& {\mathrm{\δv}}_e\end{array}\right]}^T\\ +\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]M_3{\mathrm{\δf}}^b\\ =M_4{\left[\begin{array}{cc}{\mathrm{\δ\θ}}_0& {\mathrm{\δ\γ}}_0\end{array}\right]}^T+M_5{\left[\begin{array}{cc}{\mathrm{\δv}}_n& {\mathrm{\δv}}_e\end{array}\right]}^T+M_6{\mathrm{\δf}}^b\end{array}---\left(18\right)$

其中，δv_n为北向速度误差、δv_e为东向速度误差，为估算的北向速度，为估算的东向速度，v_n为真实北向速度，v_e为真实东向速度；

M₄＝[I_2×2 0_2×1]M₁M,M₅＝[I_2×2 0_2×1]M₂[I_2×2 0_2×1]^T,M₆＝[I_2×2 0_2×1]M₃；

3)由1)、2)步中水平欧拉角误差微分方程、速度误差微分方程得到卡尔曼滤波状态方程中状态转移矩阵及***噪声矩阵相应为

$F=\left[\begin{array}{cc}{0}_{2\×2}& 0_{2\×2}\\ M_4& M_5\end{array}\right],G=\left[\begin{array}{cc}{0}_{2\×3}& 0_{2\×3}\\ 0_{2\×3}& M_6\end{array}\right]$

在步骤三中所述的卡尔曼滤波观测方程按如下方法构建：

1)构建地理系n系下含有误差的多普勒测速输出与姿态矩阵失准角φ的关系

${\widetilde{\mathrm{\ν}}}_{ed}^n=C_{n_0}^n{C}_{i_{n_0}}^{n_0}\[I-\mathrm{\φ}\×\]C_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{C}_d^b(v_{ed}^d+{\mathrm{\δv}}_{ed}^d)---\left(19\right)$

其中，为多普勒测速仪测得的潜航器速度在n系下投影，为多普勒测速仪测得的潜航器速度在d系下的投影，多普勒测速仪测速误差在d系下的投影；

2)构建速度误差观测量z

潜航器惯导解算速度将潜航器惯导解算速度的北向速度东向速度与多普勒测速仪输出中的北向速度东向速度作差得到速度误差观测量，

$\begin{array}{c}z={\left[\begin{array}{cc}{\tilde{v}}_n& {\tilde{v}}_e\end{array}\right]}^T-{\left[\begin{array}{cc}{\tilde{v}}_{nDVL}& {\tilde{v}}_{eDVL}\end{array}\right]}^T\\ =\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]({\tilde{v}}_e^n-{\tilde{v}}_{ed}^n)\\ =\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right](v_e^n+{\mathrm{\δv}}_e^n-C_{n_0}^n{C}_{i_{n_0}}^{n_0}\[I-\mathrm{\φ}\×\]C_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{C}_d^b(v_{ed}^d+{\mathrm{\δv}}_{ed}^d))\\ =\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]({\mathrm{\δv}}_e^n+C_{n_0}^n{C}_{i_{n_0}}^{n_0}\[C_b^{i_{b_0}}{C}_d^b{v}_{ed}^d\×\]\mathrm{\φ}-C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{C}_d^b{\mathrm{\δv}}_{ed}^d)\\ ={\left[\begin{array}{cc}{\mathrm{\δv}}_n& {\mathrm{\δv}}_e\end{array}\right]}^T+M_7{\left[\begin{array}{cc}{\mathrm{\δ\θ}}_0& {\mathrm{\δ\γ}}_0\end{array}\right]}^T+v\end{array}---\left(20\right)$

其中， $M_7=\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]C_{n_0}^n{C}_{i_{n_0}}^{n_0}\[C_b^{i_{b_0}}{C}_d^b{v}_{ed}^d\×\]M,v=-\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{C}_d^b{\mathrm{\δv}}_{ed}^d;$

3)确定观测方程z＝Hx+v中观测矩阵为

H＝[M₇ I_2×2]。

与现有技术相比，本发明的有益效果是：

1)移动基准惯导(MINS)对每一个潜航器惯导(SINS)装订完成初始位置、初始速度、初始姿态信息后，潜航器与移动基准即可分离，不需要主/子惯导继续传送信息，传送信息少，改变了目前传递对准需要主/子惯导实时传送一段时间的信息才能完成对准的现状，实现了分离的快速性。

2)移动基准惯导(MINS)可同时对多个潜航器惯导(SINS)进行信息装订，同时布放多个潜航器，改变了目前传递对准只能实现主惯导与每一个子惯导一一进行传递对准的现状。

3)该传递对准方法对惯性坐标系下的常值水平欧拉角误差进行估计，估计精度高，缩短了对准时间。

附图说明

图1为本发明方法的流程示意图；

图2为无初始方位角信息辅助下传统对准过程中方位角误差曲线；

图3为初始方位角信息辅助下传递对准过程中方位角误差曲线。

具体实施方式

下面结合附图对本发明中的方法作进一步详细说明。

步骤一：移动基准惯导(MINS)对每一个潜航器惯导(SINS)装订初始位置(包括初始经度λ₀、初始纬度L₀)、初始速度初始姿态信息(包括准确的方位角存在误差的俯仰角与横滚角)，装订完成后，潜航器与移动基准分离；

其中，t₀表示对准初始时刻，表示潜航器相对于地球的速度在地理坐标系n系下的投影，地理坐标系n系定义为北向-东向-地向(N-E-D)，地球坐标系e系的原点位于地心，e_z轴沿地球自转轴方向，e_x轴在赤道平面内指向格林威治子午线，e_y轴与另外两轴构成右手正交坐标系；

表示惯性坐标系系到惯性坐标系系的方向余弦矩阵，系由对准初始时刻载体坐标系b系与惯性空间固联得到(相对惯性空间固定不动)，系由对准初始时刻n系与惯性空间固联得到(相对惯性空间固定不动)，载体坐标系b系轴向沿载体横滚轴-俯仰轴-偏航轴(前-右-下)；

步骤二：潜航器惯导(SINS)建立姿态更新方程、速度更新方程、位置更新方程分别如下：

姿态更新方程： $C_b^n=C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}---\left(21\right)$

其中，

n₀系定义为对准初始时刻的地理坐标系，相对地球表面固定不动，不随导航***在地球表面运动而运动；

为对准初始时刻导航系n₀系到当前时刻导航系n系的方向余弦矩阵；

为惯性坐标系系到对准初始时刻导航系n₀系的方向余弦矩阵；

为惯性坐标系系到惯性坐标系系的方向余弦矩阵；

为载体坐标系b系到惯性坐标系系的方向余弦矩阵；

速度更新方程： ${\mathrm{\ν}}_e^n\left(t_k\right)={\mathrm{\ν}}_e^n\left(t_{k-1}\right)+{\mathrm{\Δv}}_a+{\mathrm{\Δv}}_b+{\mathrm{\Δv}}_c---\left(22\right)$

其中，

$\begin{array}{c}{\mathrm{\Δv}}_a=C_{n_0}^n\left(t_{k-1}\right)C_{i_{n_0}}^{n_0}\left(t_{k-1}\right)C_{i_{b_0}}^{i_{n_0}}\left(t_{k-1}\right){\∫}_{t_{k-1}}^{t_k}{C}_b^{i_{b_0}}{f}^{b}dt\\ {\mathrm{\Δv}}_b=\mathrm{\Δ}t\left(\right(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v_e^n\left(t_{k-1}\right))\\ {\mathrm{\Δv}}_c={\mathrm{\Δtg}}_l^n\end{array}$

t_k-1、t_k分别表示第k-1、k个潜航器速度更新时刻，为对应的速度，f^b为加表比力输出，为t_k-1时刻相应的方向余弦矩阵，Δt＝t_k-t_k-1为惯导更新时间间隔，为地理系下的地球自转角速度，为地理系下的转移角速度，为地理系下的重力；

位置更新方程： $\begin{array}{c}{L}_k=L_{k-1}+\mathrm{\Δ}t\·v_n/(R_N+h)\\ {\mathrm{\λ}}_k={\mathrm{\λ}}_{k-1}+\mathrm{\Δ}t\·v_e/\[(R_N+h){\mathrm{cosL}}_{k-1}\]\end{array}---\left(23\right)$

潜航器经纬度通过航迹推算进行更新，其中，

L_k-1、L_k分别表示k-1、k时刻潜航器的纬度，λ_k-1、λ_k分别表示k-1、k时刻潜航器的经度，v_n、v_e表示潜航器的北向、东向速度，R_N表示子午面曲率半径，h表示高度；

步骤三：构建卡尔曼滤波状态方程与观测方程如下：

选取横滚角误差δθ₀、俯仰角误差δγ₀、北向速度误差δv_n、东向速度误差δv_e为***状态，即

x＝[δθ₀ δγ₀ δv_n δv_e]^T      (24)

构建***状态方程如下，

$\stackrel{\·}{x}=Fx+Gw---\left(25\right)$

其中，上标T表示矩阵或向量的转置，x表示***状态，表示状态微分，F表示状态转移矩阵，G表示***噪声矩阵； $w={\left[\begin{array}{cc}{\mathrm{\δ\ω}}_{i_b}^b& {\mathrm{\δf}}^b\end{array}\right]}^T,$ 为陀螺输出噪声，δf^b为加表输出噪声；

***状态方程包括水平欧拉角误差微分方程和速度误差微分方程，分别构建如下：

1)水平欧拉角误差微分方程

水平欧拉角误差为常值，其微分为零，即

${\left[\begin{array}{cc}\mathrm{\δ}{\stackrel{\·}{\mathrm{\θ}}}_0& \mathrm{\δ}{\stackrel{\·}{\mathrm{\γ}}}_0\end{array}\right]}^T=0_{2\×1}---\left(26\right)$

2)速度误差微分方程

①构建估算的姿态矩阵失准角与水平欧拉角误差δθ₀、δγ₀间的关系

其中，φ为估算的姿态矩阵失准角，

进一步，将(28)代入的表达式(27)并化简可得，

②构建包含水平欧拉角误差δθ₀、δγ₀的速度误差微分方程

潜航器相对于地球真实速度在n系下的速度微分方程为，

${\stackrel{\·}{\mathrm{\ν}}}_e^n=C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{f}^b+(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×{\mathrm{\ν}}_e^n+g_l^n---\left(30\right)$

估算速度微分方程为，

${\stackrel{\·}{\tilde{v}}}_e^n=C_{n_0}^n{C}_{i_{n_0}}^{n_0}{\tilde{C}}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{\tilde{f}}^b+(2{\mathrm{\ω}}_{ie}^n+{\widetilde{\mathrm{\ω}}}_{en}^n)\×{\widetilde{\mathrm{\ν}}}_e^n+g_l^n---\left(31\right)$

(31)式与(30)式相减得速度误差微分方程，

$\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_e^n={\stackrel{\·}{\tilde{v}}}_e^n-{\stackrel{\·}{v}}_e^n\\ =(C_{n_0}^n{C}_{i_{n_0}}^{n_0}{\tilde{C}}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{\tilde{f}}^b-C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{f}^b)-\[(2{\mathrm{\ω}}_{ie}^n+{\widetilde{\mathrm{\ω}}}_{\mathrm{en}}^n)\×{\tilde{v}}_e^n-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v_e^n\]\\ =M_1\mathrm{\φ}+M_2{\mathrm{\δv}}_e^n+M_3{\mathrm{\δf}}^b\end{array}---\left(32\right)$

$M_1=-C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}\[C_b^{i_{b_0}}{f}^b\×\],M_2=-\left(\right(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×),M_3=C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}$

其中，为有误差的比力输出，为估算速度，为速度误差，为估算的转移角速度；

将(29)式代入(32)式，得到包含水平欧拉角误差δθ₀、δγ₀的北向速度误差δv_n、东向速度误差δv_e微分方程如下，

$\begin{array}{c}{\left[\begin{array}{cc}\mathrm{\δ}{\stackrel{\·}{v}}_n& \mathrm{\δ}{\stackrel{\·}{v}}_e\end{array}\right]}^T\\ ={\[{\begin{array}{cc}{\stackrel{\·}{\tilde{v}}}_n& \stackrel{\·}{\tilde{v}}\end{array}}_e\]}^T-{\left[\begin{array}{cc}{\stackrel{\·}{v}}_n& {\stackrel{\·}{v}}_e\end{array}\right]}^T\\ =\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]M_{1}M{\left[\begin{array}{cc}{\mathrm{\δ\θ}}_0& {\mathrm{\δ\γ}}_0\end{array}\right]}^T+\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]M_2{\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]}^T{\left[\begin{array}{cc}{\mathrm{\δv}}_n& {\mathrm{\δv}}_e\end{array}\right]}^T\\ +\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]M_3{\mathrm{\δf}}^b\\ =M_4{\left[\begin{array}{cc}{\mathrm{\δ\θ}}_0& {\mathrm{\δ\γ}}_0\end{array}\right]}^T+M_5{\left[\begin{array}{cc}{\mathrm{\δv}}_n& {\mathrm{\δv}}_e\end{array}\right]}^T+M_6{\mathrm{\δf}}^b\end{array}---\left(33\right)$

其中，δv_n为北向速度误差、δv_e为东向速度误差，为估算的北向速度，为估算的东向速度，v_n为真实北向速度，v_e为真实东向速度；

M₄＝[I_2×2 0_2×1]M₁M,M₅＝[I_2×2 0_2×1]M₂[I_2×2 0_2×1]^T,M₆＝[I_2×2 0_2×1]M₃；

3)由1)、2)步中水平欧拉角误差微分方程、速度误差微分方程得到卡尔曼滤波状态方程中状态转移矩阵及***噪声矩阵相应为

$F=\left[\begin{array}{cc}{0}_{2\×2}& 0_{2\×2}\\ M_4& M_5\end{array}\right],G=\left[\begin{array}{cc}{0}_{2\×3}& 0_{2\×3}\\ 0_{2\×3}& M_6\end{array}\right]$

取1s更新一次的多普勒速度为观测量，构建观测方程如下，

z＝Hx+v         (34)

其中，

多普勒坐标系d系轴向沿多普勒测速仪的横滚轴-俯仰轴-偏航轴(前-右-下)；

z为观测向量，H为观测矩阵，v为测量噪声；

观测方程包括观测量与观测矩阵的确定，分别确定如下：

1)构建地理系n系下含有误差的多普勒测速输出与姿态矩阵失准角φ的关系

${\widetilde{\mathrm{\ν}}}_{ed}^n=C_{n_0}^n{C}_{i_{n_0}}^{n_0}\[I-\mathrm{\φ}\×\]C_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{C}_d^b({\mathrm{\ν}}_{ed}^d+{\mathrm{\δ\ν}}_{ed}^d)---\left(35\right)$

其中，为多普勒测速仪测得的潜航器速度在n系下投影，为多普勒测速仪测得的潜航器速度在d系下的投影，多普勒测速仪测速误差在d系下的投影；

2)构建速度误差观测量z

潜航器惯导解算速度将潜航器惯导解算速度的北向速度东向速度与多普勒测速仪输出中的北向速度东向速度作差得到速度误差观测量，

$\begin{array}{c}z={\left[\begin{array}{cc}{\tilde{v}}_n& {\tilde{v}}_e\end{array}\right]}^T-{\left[\begin{array}{cc}{\tilde{v}}_{nDVL}& {\tilde{v}}_{eDVL}\end{array}\right]}^T\\ =\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]({\tilde{v}}_e^n-{\tilde{v}}_{ed}^n)\\ =\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right](v_e^n+{\mathrm{\δv}}_e^n-C_{n_0}^n{C}_{i_{n_0}}^{n_0}\[I-\mathrm{\φ}\×\]C_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{C}_d^b(v_{ed}^d+{\mathrm{\δv}}_{ed}^d))\\ =\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]({\mathrm{\δv}}_e^n+C_{n_0}^n{C}_{i_{n_0}}^{n_0}\[C_b^{i_{b_0}}{C}_d^b{v}_{ed}^d\×\]\mathrm{\φ}-C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{n_0}}^{i_{b_0}}{C}_b^{i_{b_0}}{C}_d^b{\mathrm{\δv}}_{ed}^d)\\ ={\left[\begin{array}{cc}{\mathrm{\δv}}_n& {\mathrm{\δv}}_e\end{array}\right]}^T+M_7{\left[\begin{array}{cc}{\mathrm{\δ\θ}}_0& {\mathrm{\δ\γ}}_0\end{array}\right]}^T+v\end{array}---\left(36\right)$

其中， $M_7=\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]C_{n_0}^n{C}_{i_{n_0}}^{n_0}\[C_b^{i_{b_0}}{C}_d^b{v}_{ed}^d\×\]M,v=-\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{j_{b_0}}{C}_d^b{\mathrm{\δv}}_{ed}^d;$

3)确定观测方程z＝Hx+v中观测矩阵为

H＝[M₇ I_2×2]；

步骤四：根据卡尔曼滤波状态方程和观测方程进行滤波，对横滚角误差、俯仰角误差、北向速度误差、东向速度误差进行估计，对潜航器的姿态、速度进行修正，完成传递对准。

下面对本发明方法的效果进行检验。

使用水下潜航器数据对本发明中的方法进行验证，实验中主惯导***传递给子惯导的姿态信息为初始基准值为潜航器姿态基准值为利用原始数据进行10min的INS/GPS组合导航解算得到。潜航器惯导***为激光陀螺捷联惯导***，陀螺精度为0.01°/h，加表精度为5×10^-5g。实验截取潜航器运行前10min数据进行方法验证。

对比图2对应的无初始方位角信息辅助下传统对准过程中方位角误差曲线(以多普勒测速仪速度输出值为观测量，潜航器在无初始方位信息下自对准)、图3对应的初始方位角信息辅助下传递对准过程中方位角误差曲线，可以发现图2中方位角误差在600s时才收敛，图3中采用初始方位角信息辅助下的传递对准方案，方位角误差到50s时就收敛到1密位，对准时间与精度都得到明显改善。

本方法利用移动基准惯导的方位角信息确定子惯导***的方位角信息，缩短了与子惯导***信息传输时间，使得子惯导***可以实现与主惯导***分离的快速性，在此基础上，可以实现短时间投放大量潜航器的实际需求，同时可以保证子惯导***在短时间内实现对准。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应当视为落入本发明的保护范围。

Claims (2)

1.一种初始方位信息辅助下潜航器快速传递对准方法，其特征在于包含以下步骤：

步骤一：移动基准惯导(MINS)对每一个潜航器惯导(SINS)装订初始位置(包括初始经度λ₀、初始纬度L₀)、初始速度初始姿态信息(包括准确的方位角存在误差的俯仰角与横滚角)，装订完成后，潜航器与移动基准分离；

其中，t₀表示对准初始时刻，表示潜航器相对于地球的速度在地理坐标系n系下的投影，地理坐标系n系定义为北向-东向-地向(N-E-D)，地球坐标系e系的原点位于地心，e_z轴沿地球自转轴方向，e_x轴在赤道平面内指向格林威治子午线，e_y轴与另外两轴构成右手正交坐标系；

表示惯性坐标系系到惯性坐标系系的方向余弦矩阵，系由对准初始时刻载体坐标系b系与惯性空间固联得到(相对惯性空间固定不动)，系由对准初始时刻n系与惯性空间固联得到(相对惯性空间固定不动)，载体坐标系b系轴向沿载体横滚轴-俯仰轴-偏航轴(前-右-下)；

步骤二：潜航器惯导(SINS)建立姿态更新方程、速度更新方程、位置更新方程分别如下：

姿态更新方程： $C_b^n=C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}---\left(1\right)$

其中，

n₀系定义为对准初始时刻的地理坐标系，相对地球表面固定不动，不随导航***在地球表面运动而运动；

为对准初始时刻导航系n₀系到当前时刻导航系n系的方向余弦矩阵；

为惯性坐标系系到对准初始时刻导航系n₀系的方向余弦矩阵；

为惯性坐标系系到惯性坐标系系的方向余弦矩阵；

为载体坐标系b系到惯性坐标系系的方向余弦矩阵；

速度更新方程： $v_e^n\left(t_k\right)=v_e^n\left(t_{k-1}\right)+{\mathrm{\Δv}}_a+{\mathrm{\Δv}}_b+{\mathrm{\Δv}}_c---\left(2\right)$

其中，

${\mathrm{\Δv}}_a=C_{n_0}^n\left(t_{k-1}\right)C_{i_{n_0}}^{n_0}\left(t_{k-1}\right)C_{i_{b_0}}^{i_{n_0}}\left(t_{k-1}\right){\∫}_{t_{k-1}}^{t_k}{C}_b^{i_{b_0}}{f}^{b}dt$

${\mathrm{\Δv}}_b=\mathrm{\Δ}t\left(\right(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v_e^n(t_{k-1}\left)\right)$

${\mathrm{\Δv}}_c={\mathrm{\Δtg}}_l^n$

t_k-1、t_k分别表示第k-1、k个潜航器速度更新时刻，为对应的速度，f^b为加表比力输出，为t_k-1时刻相应的方向余弦矩阵，Δt＝t_k-t_k-1为惯导更新时间间隔，为地理系下的地球自转角速度，为地理系下的转移角速度，为地理系下的重力；

位置更新方程： $\begin{array}{c}{L}_k=L_{k-1}+\mathrm{\Δ}t\·v_n/(R_N+h)\\ {\mathrm{\λ}}_k={\mathrm{\λ}}_{k-1}+\mathrm{\Δ}t\·v_e/\[(R_N+h){\mathrm{cosL}}_{k-1}\]\end{array}---\left(3\right)$

潜航器经纬度通过航迹推算进行更新，其中，

L_k-1、L_k分别表示k-1、k时刻潜航器的纬度，λ_k-1、λ_k分别表示k-1、k时刻潜航器的经度，v_n、v_e表示潜航器的北向、东向速度，R_N表示子午面曲率半径，h表示高度；

步骤三：构建卡尔曼滤波状态方程与观测方程如下：

选取横滚角误差δθ₀、俯仰角误差δγ₀、北向速度误差δv_n、东向速度误差δv_e为***状态，即

x＝[δθ₀ δγ₀ δv_n δv_e]^T   (4)

构建***状态方程如下，

$\stackrel{\·}{x}=Fx+Gw---\left(5\right)$

其中，

上标T表示矩阵或向量的转置，x表示***状态，表示状态微分，F表示状态转移矩阵，G表示***噪声矩阵； $w={\left[\begin{array}{cc}{\mathrm{\δ\ω}}_{i_b}^b& {\mathrm{\δf}}^b\end{array}\right]}^T,$ 为陀螺输出噪声，δf^b为加表输出噪声；

取1s更新一次的多普勒速度为观测量，构建观测方程如下，

z＝Hx+v   (6)

其中，

多普勒坐标系d系轴向沿多普勒测速仪的横滚轴-俯仰轴-偏航轴(前-右-下)；

z为观测向量，H为观测矩阵，v为测量噪声；

步骤四：根据卡尔曼滤波状态方程和观测方程进行滤波，对横滚角误差、俯仰角误差、北向速度误差、东向速度误差进行估计，对潜航器的姿态、速度进行修正，完成传递对准。

2.根据权利要求1所述的一种初始方位信息辅助下潜航器快速传递对准方法，其特征在于：

在步骤二中所述的姿态更新方程按如下方法更新：

$C_{n_0}^n\≈I_{3\×3}+\left[\begin{array}{ccc}0& -(\mathrm{\λ}-{\mathrm{\λ}}_0+{\mathrm{\ω}}_{ie}t)\sin L& L_0-L\\ (\mathrm{\λ}-{\mathrm{\λ}}_0+{\mathrm{\ω}}_{ie}t)\sin L& 0& -(\mathrm{\λ}-{\mathrm{\λ}}_0+{\mathrm{\ω}}_{ie}t)\cos L\\ L-L_0& (\mathrm{\λ}-{\mathrm{\λ}}_0+{\mathrm{\ω}}_{ie}t)\cos L& 0\end{array}\right]---\left(7\right)$

其中，I_3×3表示三维单位矩阵，ω_ie为地球自转角速度，t为初始时刻到当前时刻的解算时间，λ₀、L₀为潜航器装订的初始经度、纬度，λ、L为当前时刻经度、纬度；

可由潜航器初始经纬度和t确定，将按如下分解，

$C_{i_{n_0}}^{n_0}=C_e^{n_0}{C}_{i_{e_0}}^e{C}_{i_{n_0}}^{i_{e_0}}---\left(8\right)$

其中，

$\begin{array}{c}{C}_e^{n_0}=\left[\begin{array}{ccc}-{\mathrm{sinL}}_0{\mathrm{cos\λ}}_0& -{\mathrm{sinL}}_0{\mathrm{sin\λ}}_0& {\mathrm{cosL}}_0\\ {\mathrm{cosL}}_0{\mathrm{cos\λ}}_0& {\mathrm{cosL}}_0{\mathrm{sin\λ}}_0& {\mathrm{sin\λ}}_0\\ -{\mathrm{sin\λ}}_0& {\mathrm{cos\λ}}_0& 0\end{array}\right]\\ C_{i_{e_0}}^e=\left[\begin{array}{ccc}\cos \left({\mathrm{\ω}}_{ie}t\right)& \sin \left({\mathrm{\ω}}_{ie}t\right)& 0\\ -\sin \left({\mathrm{\ω}}_{ie}t\right)& \cos \left({\mathrm{\ω}}_{ie}t\right)& 0\\ 0& 0& 1\end{array}\right],C_{i_{n_0}}^{i_{e_0}}={\left(C_e^{n_0}\right)}^T\end{array}---\left(9\right)$

系由对准初始时刻e系与惯性空间固联得到(相对惯性空间固定不动)；

为地球坐标系e系到对准初始时刻导航系n₀系的方向余弦矩阵；

为惯性坐标系系到地球坐标系e系的方向余弦矩阵；

为惯性坐标系系到惯性坐标系系的方向余弦矩阵；

利用陀螺输出数据求解，传递对准完成时间内，忽略陀螺的零偏、随机游走等误差，按如下微分方程和初值进行姿态更新求解，即

${\stackrel{\·}{C}}_b^{i_{b_0}}=C_b^{i_{b_0}}\[{\mathrm{\ω}}_{ib}^b\×\],C_b^{i_{b_0}}\left(0\right)=I_{3\×3}---\left(10\right)$

其中，为载体角速度，为的初值；

在步骤三中所述的卡尔曼滤波状态方程按如下方法构建：

1)水平欧拉角误差微分方程构建如下

水平欧拉角误差为常值，其微分为零，即

${\left[\begin{array}{cc}\mathrm{\δ}{\stackrel{\·}{\mathrm{\θ}}}_0& \mathrm{\δ}{\stackrel{\·}{\mathrm{\γ}}}_0\end{array}\right]}^T=0_{2\×1}---\left(11\right)$

2)速度误差微分方程构建如下

$\begin{array}{c}{\left[\begin{array}{cc}\mathrm{\δ}{\stackrel{\·}{v}}_n& \mathrm{\δ}{\stackrel{\·}{v}}_e\end{array}\right]}^T\\ ={\left[\begin{array}{cc}{\stackrel{\·}{\tilde{v}}}_n& {\stackrel{\·}{\tilde{v}}}_e\end{array}\right]}^T-{\left[\begin{array}{cc}{\stackrel{\·}{v}}_n& {\stackrel{\·}{v}}_e\end{array}\right]}^T\\ =\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]M_{1}M{\left[\begin{array}{cc}{\mathrm{\δ\θ}}_0& {\mathrm{\δ\γ}}_0\end{array}\right]}^T+\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]M_2{\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]}^T{\left[\begin{array}{cc}{\mathrm{\δv}}_n& {\mathrm{\δv}}_e\end{array}\right]}^T\\ +\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]M_3{\mathrm{\δf}}^b\\ =M_4{\left[\begin{array}{cc}{\mathrm{\δ\θ}}_0& {\mathrm{\δ\γ}}_0\end{array}\right]}^T+M_5{\left[\begin{array}{cc}{\mathrm{\δv}}_n& {\mathrm{\δv}}_e\end{array}\right]}^T+M_6{\mathrm{\δf}}^b\end{array}---\left(12\right)$

其中，为估算的北向速度，为估算的东向速度，v_n为真实北向速度，v_e为真实东向速度，

$M_1=-C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}\[C_b^{i_{b_0}}{f}^b\×\],M_2=-\left(\right(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×),M_3=C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}};$

$M_4=\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]M_{1}M,M_5=\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]M_2{\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]}^T,M_6=\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]M_3$

3)由1)、2)步中水平欧拉角误差微分方程、速度误差微分方程得到卡尔曼滤波状态方程中状态转移矩阵及***噪声矩阵相应为

$F=\left[\begin{array}{cc}{0}_{2\×2}& 0_{2\×2}\\ M_4& M_5\end{array}\right],G=\left[\begin{array}{cc}{0}_{2\×3}& 0_{2\×3}\\ 0_{2\×3}& M_6\end{array}\right]$

在步骤三中所述的卡尔曼滤波观测方程按如下方法构建：

1)构建地理系n系下含有误差的多普勒测速输出与姿态矩阵失准角φ的关系

${\tilde{v}}_{ed}^n=C_{n_0}^n{C}_{i_{n_0}}^{n_0}\[I-\mathrm{\φ}\×\]C_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{C}_d^b(v_{ed}^d+{\mathrm{\δv}}_{ed}^d)---\left(13\right)$

其中，为多普勒测速仪测得的潜航器速度在n系下投影，为多普勒测速仪测得的潜航器速度在d系下的投影，δv_e ^d _d多普勒测速仪测速误差在d系下的投影；

2)构建速度误差观测量z

潜航器惯导解算速度为将潜航器惯导解算速度的北向速度东向速度与多普勒测速仪输出中的北向速度东向速度作差得到速度误差观测量，

$\begin{array}{c}z={\left[\begin{array}{cc}{\tilde{v}}_n& {\tilde{v}}_e\end{array}\right]}^T-{\left[\begin{array}{cc}{\tilde{v}}_{nDVL}& {\tilde{v}}_{eDVL}\end{array}\right]}^T\\ =\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]\left({\tilde{v}}_e^n-{\tilde{v}}_{ed}^n\right)\\ =\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]\left(v_e^n+{\mathrm{\δv}}_e^n-C_{n_0}^n{C}_{i_{n_0}}^{n_0}\[I-\mathrm{\φ}\×\]C_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{C}_d^b\left(v_{ed}^d+{\mathrm{\δv}}_{ed}^d\right)\right)\\ =\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]\left({\mathrm{\δv}}_e^n+C_{n_0}^n{C}_{i_{n_0}}^{n_0}\[C_b^{i_{b_0}}{C}_d^b{v}_{ed}^d\×\]\mathrm{\φ}-C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{C}_d^b{\mathrm{\δv}}_{ed}^d\right)\\ ={\left[\begin{array}{cc}{\mathrm{\δv}}_n& {\mathrm{\δv}}_e\end{array}\right]}^T+M_7{\left[\begin{array}{cc}{\mathrm{\δ\θ}}_0& {\mathrm{\δ\γ}}_0\end{array}\right]}^T+v\end{array}---\left(14\right)$

其中， $M_7=\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]C_{n_0}^n{C}_{i_{n_0}}^{n_0}\[C_b^{i_{b_0}}{C}_d^b{v}_{ed}^d\×\]M,v=\left[\begin{array}{cc}{I}_{2\×2}& 0_{2\×1}\end{array}\right]C_{n_0}^n{C}_{i_{n_0}}^{n_0}{C}_{i_{b_0}}^{i_{n_0}}{C}_b^{i_{b_0}}{C}_d^b{\mathrm{\δv}}_{ed}^d;$

3)确定观测方程z＝Hx+v中观测矩阵为

H＝[M₇ I_2×2]。