航迹推算基本原理和算法应用

航迹推算是根据罗经和计程仪所指示的航向、航程，以及船舶操纵要素和风流要素等在不借助外界导航物标的条件下，求取航迹和船位的方法。

根据具体场景，所估计的真实模型可假设为线性模型和非线性模型。

线性模型

线性化的UUV运动系统的状态空间模型，

位置微分方程：

记地心坐标系为 i 系；地球坐标系为 e 系；“东北天”地理坐标系为 n 系；“右前上”载体坐标系为 b 系。

$$P_k=P_{k-1}+T_{s}V_{k-1}$$

$$V_{k}^{n}=V_{k-1}^{n}+T_s[C_{bk-1}^{n}f_{k}^{b}-(2w_{iek-1}^n+w_{enk-1}^n)\times V_{k-1}^{n}+g^{n}]$$

其中$P_k=[L_k \quad \lambda_k \quad h_k ]^T$分别为维度、经度和高度；

$V_{k}=[V_{Nk}/(R_M+h_{k}) \quad V_{Ek}secL_{k}/(R_N+h_{k}) \quad V_{Uk} ]^T$；

$V^n=[V_{N} \quad V_{U} \quad V_{E} ]^T$分别为北天东速度；

$C_{b}^{n}$为惯导姿态矩阵。

误差方程：

$$ \dot{X(t)}=F(t)X(t)+w(t) $$

$$ Z(t)=H(t)X(t)+v(t) $$

其中$X(t)$为状态变量，$F(t)$为系统矩阵，$H(t)$为量测矩阵，$w(t)$为系统噪声向量，$v(t)$为量测噪声向量

取15维状态变量为：

$$X=\begin{bmatrix} \delta{V_n} & \delta{V_u}& \delta{V_e}& \delta{L}& \delta{h}& \delta{\lambda}& \phi_h& \phi_u& \phi_e& \nabla_x& \nabla_y& \nabla_z& \varepsilon_x& \varepsilon_y& \varepsilon_z \end{bmatrix}^T$$

其中，$\delta{V_n}、\delta{V_u}、\delta{V_e}$分别为系统北天东方向的速度误差；

$\delta{L}、\delta{h}、\delta{\lambda}$分别为系统纬度、高度、经度误差；

$\phi_h、\phi_u、\phi_e$为北天东方向的失准角；

$\nabla_x、\nabla_y、\nabla_z$分别为xyz方向的加速度零偏；

$\varepsilon_x、\varepsilon_y、\varepsilon_z$分别为xyz方向的陀螺漂移。

量测量：

$$Z=\begin{bmatrix} V_n-V_n^{base} & V_u-V_u^{base}& V_u-V_u^{base} \end{bmatrix}$$

其中，$V_n、V_u、V_e$分别为惯导解算得到的北天东的速度；

$V_n^{base}、V_u^{base}、V_e^{base}$为北天东的速度基准。

量测矩阵：

$$H=\begin{bmatrix} I_{33}& O_{33}& O_{33}& O_{33}& O_{3*3}\end{bmatrix}$$

系统矩阵:

$$F=\begin{bmatrix} O_{33}&I_{33}& O_{33}& O_{33}& O_{33} \ O_{33}& O_{33}& S_{t}& C_{b}^{n}& O_{33} \ O_{33}& O_{33}& O_{33}& O_{33}& -C_{b}^{n} \ O_{33}& O_{33}& O_{33}& O_{33}& O_{33} \ O_{33}& O_{33}& O_{33}& O_{33}& O_{33} \end{bmatrix}$$