捷联惯导系统学习7.4(车载惯性/里程仪组合导航 )

航位推算算法（Dead Reckoning，DR）

利用位姿、航向、行驶里程信息推算载车相对于起始点的位置。
里程仪（odometer）:输出一般是载车在一小段内行驶的路程增量，为了方便分析，假设里程仪输出的是瞬时速度。轮式车辆往往分为转向轮和非转向轮，非转向轮始终与车体正前方一致，这里假设里程仪测量的是非转向轮的信号。
理想情况下：载车正常形式，前进取正，倒车取负，且车轮紧贴地面无打滑、滑行、弹跳。设车体坐标系为m系，$o{y}_m:在和载车车轮相接处的地平面内,o{z}_m：垂直与地平面内向上为正;0x_m指向右方$
里程仪的输出在车体坐标系m可以表示为：
$$v_D^m=\left[\begin{array}{ccc}0& v_D& 0\end{array}\right]$$
捷联惯导(IMU)固定在车体上，假设IMU坐标系（b系）与车体坐标系（m系）重合，通过IMU中3轴陀螺仪可以实时计算载车的姿态矩阵，记为$C_b^n$,可以利用$C_b^n$和$v_D^n$求得$c_D^n$在导航坐标系下的里程仪速度输出，即：
$$v_D^n=C_b^n{v}_D^n$$

1. 利用$v_D^n$求得航位
   $$\begin{gathered} L_D:航位推算的地理纬度 \\ {\lambda }_D:航位推算的经度 \\ h:航位推算的高度 \end{gathered}$$

   $$\begin{gathered} R_{MhD}=R_{Mh}+h_D:R_{Mh}航位推算的地理位置计算的子午圈 \\ {R}_{NhD}=R_{Nh}+h_D:R_{Nh}航位推算的地理位置计算的卯酉圈 \end{gathered}$$
   $v_D^n={\left[\begin{array}{ccc}{v}_{DE}& v_{ND}& v_{DU}\end{array}\right]}^T$
   $$\begin{gathered} {\dot{L}}_D=\frac{v_{DN}}{R_{MhD}} \\ \dot{\lambda }=\frac{v_{DE}\sec L_D}{R_{NhD}} \\ {\dot{h}}_D=v_{DU} \end{gathered}$$

2. 可将上式简写为：
   $P_D={\left[\begin{array}{ccc}{L}_D& {\lambda }_D& h_D\end{array}\right]}^T$
   $$\begin{gathered} M_{pvD}=\left[\begin{array}{ccc}0& 1/R_{MhD}& 0\\ sec{L}_D/R_{NhD}& 0& 0\\ 0& 0& 1\end{array}\right] \\ {\dot{P}}_D=M_{pv}{v}_D^n \end{gathered}$$

3. 令航位推算纬度代替捷联惯导解算纬度,里程仪计算速度代替捷联惯导解算速度即：$L=L_D,v^n=v_D^n$得到航位推算姿态更新算法方程如下：
   $w_{ib}^b:陀螺仪输出角速度信息$
   $$\begin{gathered} w_{in}^n=w_{ie}^n+w_{en}^n: \\ {w}_{ie}^n={\left[\begin{array}{ccc}0& w_{ie}cos{L}_D& w_{ie}sin{L}_D\end{array}\right]}^T \\ {w}_{en}^n={\left[\begin{array}{ccc}-\frac{v_{DN}}{R_{MhD}}& \frac{v_{DE}}{R_{NhD}}& \frac{v_{DE}tan{L}_D}{R_{NhD}}\end{array}\right]}^T \end{gathered}$$
   $$\begin{gathered} {\dot{C}}_b^n=C_b^n(w_{ib}^b\times )-(w_{in}^n\times )C_b^n \\ {w}_{in}^n=w_{ie}^n+w_{en}^n \end{gathered}$$

4. 得到航位推算算法：
   $$\{\begin{array}{ll}{\dot{P}}_D=M_{pv}{v}_D^n& \\ {\dot{C}}_b^n=C_b^n(w_{ib}^b\times )-(w_{in}^n\times )C_b^n& w_{in}^n=w_{ie}^n+w_{en}^n\end{array}$$

数值更新算法：

1. 如果里程仪在$T_j=t_j-t_{j-1}$内的路程增量为$\Delta S_j$,当时间足够短时，可以认为载车在着小段时间内时沿直线行驶，可以得到路程增量在m系的投影为：
   $$\Delta S_j^n={\left[\begin{array}{ccc}0& \Delta S_j& 0\end{array}\right]}^T$$

2. 类似可以得到:
   $C_{b(j-1)是t_{j-1}}^n时刻的载车姿态矩阵$
   $$\Delta S_j^m=C_{b(j-1)}^n\Delta S_j^m$$

3. 将数据离散化得到航位推算位置更新算法如下：
   $\Delta S_j^n={\left[\begin{array}{ccc}\Delta S_E(j)& \Delta S_N(j)& \Delta S_U(j)\end{array}\right]}^T,分别为东、北、天向位移分量$
   $$\begin{gathered} L_{D(j)}=L_{D(j-1)}+\frac{T_j{v}_{DN(j)}}{R_{MhD(j-1)}}=L_{D(j-1)}+\frac{\Delta S_{N(j)}}{R_{MhD(j-1)}} \\ {\lambda }_{D(j)}={\lambda }_{D(j-1)}+\frac{T_j{v}_{DE(j)}sec{L}_{D(j-1)}}{R_{NhD(j-1)}}={\lambda }_{D(j-1)}+\frac{\Delta S_{E(j)}sec{L}_{D(j-1)}}{R_{NhD(j-1)}} \\ {h}_{D(j)}=H_{D(j-1)}+T_j{v}_{DU}=h_{D(j-1)}+\Delta S_{U(j)} \end{gathered}$$

4. 航位推算的子态阵更新算法如下：
   ${\varphi }_{ib(j)}^b:由陀螺仪输出计算的等效旋转矢量$
   $$\begin{gathered} C_{b(j)}^n=C_{n(j-1)}^{n(j)}{C}_{b(j-1)}^n{C}_{b(j)}^{(b(j-1))} \\ {C}_{b(j)}^{(b(j-1))}=M_{RV}({\varphi }_{ib(j)}^b),C_{n(j-1)}^{n(j)}=M_{RV}^T({\varphi }_{in(j)}^n) \end{gathered}$$
   $$\begin{gathered} {\varphi }_{in(j)}^n=T_j(w_{ie(j)}^n+w_{en(j)}^n)=T_j\left[\begin{array}{c}0\\ w_{ie}cos{L}_{D(j)}\\ w_{ie}sin{L}_{D(j)}\end{array}\right]+T_j\left[\begin{array}{c}-v_{DN(j)}/R_{MhD(j)}\\ v_{DE(j)}/R_{NhD(j)}\\ v_{DE(j)}tan{L}_{D(j)}/R_{NhD(j)}\end{array}\right] \\ =T_j\left[\begin{array}{c}0\\ w_{ie}cos{L}_{D(j)}\\ w_{ie}sin{L}_{D(j)}\end{array}\right]+\left[\begin{array}{c}-\Delta S_{DN(j)}/R_{MhD(j)}\\ \Delta S_{DE(j)}/R_{NhD(j)}\\ \Delta S_{DE(j)}{v}_{DE(j)}tan{L}_{D(j)}/R_{NhD(j)}\end{array}\right] \end{gathered}$$

航位推算算法误差分析

1. 惯导坐标系(b系)与车体坐标系(m系)不平行存在误差，俯仰偏角${\alpha }_{\theta }$、方位偏角${\alpha }_{\varphi }$、滚动偏角${\alpha }_r$,令偏差角矢量$\alpha ={\left[\begin{array}{ccc}{\alpha }_{\theta }& {\alpha }_r& {\alpha }_{\varphi }\end{array}\right]}^T$

2. 即b系与m系的变换矩阵为：
   $$c_b^m=I+(\alpha \times )=\left[\begin{array}{ccc}1& -{\alpha }_{\varphi }& {\alpha }_r\\ {\alpha }_{\varphi }& 1& -{\alpha }_{\theta }\\ -{\alpha }_r& {\alpha }_{\theta }& 1\end{array}\right]$$

3. 还存在刻度系数误差$\delta K_D$，得到实际速度大小${\tilde{v}}_D$与理论速度大小$v_D$存在如下关系:
   $${\tilde{v}}_D=(1+\delta K_D)v_D$$
   向量形式：
   $${\tilde{v}}_D^m=(1+\delta K_D)v_D^m$$

4. 在导航坐标系下的实际速度输出为：
   ${\varphi }_D:航位推算的失准角误差$
   $$\begin{gathered} {\tilde{v}}_D^n={\tilde{C}}_b^n(C_b^m{)}^T{\tilde{v}}_D^m=(I-{\varphi }_D\times )C_b^n(I-\alpha \times )(1+\delta K_D)v_D^m \\ \approx v_D^n-({\varphi }_D\times )C_b^n{v}_D^m-C_b^n(\alpha \times )v_D^m+C_b^n\delta K_D{v}_D^m \\ =v_D^n+v_D^n\times {\varphi }_D+C_b^n(v_D^m\times )\alpha +C_b^n{v}_D^m\delta K_D \end{gathered}$$

5. 令$C_b^n=C_{ij}(i,j=1,2,3);v_D^m={\left[\begin{array}{ccc}0& v_D& 0\end{array}\right]}^T$,可以简化得到：
   $M_{vkD}=v_D\left[\begin{array}{ccc}-C_{13}& C_{12}& C_{11}\\ -C_{23}& C_{22}& C_{21}\\ -C_{33}& C_{32}& C_{31}\end{array}\right]$
   $K_D=\left[\begin{array}{c}{\alpha }_{\theta }\\ \delta K_D\\ {\alpha }_{\varphi }\end{array}\right]$
   $$\begin{gathered} {\tilde{v}}_D^n=v_D^n+v_D^n\times {\varphi }_D+v_D\left[\begin{array}{ccc}-C_{13}& C_{12}& C_{11}\\ -C_{23}& C_{22}& C_{21}\\ -C_{33}& C_{32}& C_{31}\end{array}\right]\left[\begin{array}{c}{\alpha }_{\theta }\\ \delta K_D\\ {\alpha }_{\varphi }\end{array}\right] \\ =v_D^n+v_D^n\times {\varphi }_D+M_{vkD}{K}_D \end{gathered}$$

6. 里程仪速度误差方程：
   $$\delta v_D^n={\tilde{v}}_D^n-v_D^n=v_D^n\times {\varphi }_D+M_{vk}{K}_D$$

7. 航位推算位置方程下：
   $M_{pvD}=\left[\begin{array}{ccc}0& 1/R_{MhD}& 0\\ sec{L}_D/R_{NhD}& 0& 0\\ 0& 0& 1\end{array}\right]$
   $M_{ppD}=\left[\begin{array}{ccc}0& 0& -v_{DN}/R_{MhD}^2\\ v_{DE}sec{L}_{D}tan{L}_D/R_{NhD}& 0& -v_{DE}secL/R_{NhD}^2\\ 0& 0& 0\end{array}\right]$
   $$\delta {\dot{P}}_D=M_{pvD}\delta v_D^n+M_{ppD}\delta P_D$$

8. 得到航位推算误差方程为：
   $$\begin{gathered} \delta {\dot{p}}_D=M_{pvD}(v_D^n\times {\varphi }_D+M_{vk}{K}_D)+M_{ppD}\delta P_D=M_{paD}{\varphi }_D+M_{pkD}{K}_D+M_{ppD}\delta P_D \\ {M}_{paD}=M_{pvD}(v_D^n\times );M_{pkD}=M_{pvD}{M}_{vkD} \end{gathered}$$

9. 航位推算姿态误差方程为：
   $$\begin{gathered} {\dot{\varphi }}_D=M_{aaD}^{\prime }{\varphi }_D+M_{avD}(v_D^n\times {\varphi }_D+M_{vkD}{K}_D)+M_{apD}\delta p_D-C_b^n{\xi }^b \\ =M_{aaD}{\varphi }_D+M_{akD}{K}_D+M_{apD}\delta P_D-C_b^n{\xi }^b \\ {M}_{aaD}=M_{aaD}^{\prime }+M_{avD}(v_D^n\times ),M_{akD}=M_{avD}{M}_{vkD} \\ {M}_{aaD}^{\prime }=(\left[\begin{array}{c}0\\ w_{ie}cos{L}_D\\ w_{ie}sin{L}_D\end{array}\right]+\left[\begin{array}{c}-v_{DN}/R_{MhD}\\ v_{DE}/R_{NhD}\\ v_{DE}tan{L}_D/R_{NhD}\end{array}\right])\times ,M_{avD}=\left[\begin{array}{ccc}0& -1/R_{MhD}& 0\\ 1/R_{NhD}& 0& 0\\ tan{L}_D/R_{NhD}& 0& 0\end{array}\right] \\ {M}_{apD}=\left[\begin{array}{ccc}0& 0& 0\\ -w_{ie}sin{L}_D& 0& 0\\ w_{ie}cos{L}_D+v_{DE}se{c}^2{L}_D/R_N& 0& 0\end{array}\right] \end{gathered}$$

10. 总结：在初始误差$\delta P_D$不大时，安装偏差${\alpha }_{\theta },{\alpha }_{\varphi }$里程刻度系误差$\delta K_D$陀螺仪漂移${\xi }^b$是主要误差来源

轨迹相似性原理

1. 在载车行驶过程中，一般姿态角都比较小，近似有：
   $u_U={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T$
   $$C_b^n\approx \left[\begin{array}{ccc}cos\varphi & -sin\varphi & 0\\ sin\varphi & cos\varphi & 0\\ 0& 0& 1\end{array}\right]$$
   $v_D^n\times u_U=(c_b^n{v}_D^m)\times u_U\approx \left[\begin{array}{ccc}cos\varphi & -sin\varphi & 0\\ sin\varphi & cos\varphi & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}0\\ v_D\\ 0\end{array}\right]\times \left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=v_D\left[\begin{array}{c}cos\varphi \\ sin\varphi \\ 0\end{array}\right]$

2. 矢量误差为$${\varphi }_D={\left[\begin{array}{ccc}{\varphi }_{DE}& {\varphi }_{DN}& {\varphi }_{DU}\end{array}\right]}^T$$

3. 近似认为${\varphi }_{DE}\approx {\varphi }_{DN}\approx 0$,可得：
   $$\begin{gathered} {\tilde{v}}_D^n\approx v_D^n+v_D^n\times \left[\begin{array}{c}0\\ 0\\ {\varphi }_{DU}\end{array}\right]+v_D\left[\begin{array}{ccc}0& 0& cos\varphi \\ 0& 0& sin\varphi \\ -1& 0& 0\end{array}\right]\left[\begin{array}{c}{\alpha }_{\theta }\\ {\alpha }_r\\ {\alpha }_{\varphi }\end{array}\right]+v_D^n\delta K_D \\ =v_D^n+{\varphi }_{DU}{v}_D^n\times u_U+a_{\varphi }{v}_D\times u_U-a_{\theta }{v}_D{u}_U+v_D^n\theta K_D \\ =[I-({\varphi }_{DU}+a_{\varphi })u_U\times ]v_D^n+\delta K_D{v}_D^n-a_{\theta }{v}_D{u}_U \\ \approx (1+\delta K_D)[I-({\varphi }_{DU}+{\alpha }_{\varphi })u_U\times ]v_D^n-{\alpha }_{\theta }{v}_D{u}_U \end{gathered}$$

4. 假设${\alpha }_{\theta },{\varphi }_{DU}+{\alpha }_{\varphi },\delta K_D$,均为常值小量，对上式等式两端进行积分：
   $$\begin{gathered} S_D^n={\int }_0^T{v}_D^{n}dt \\ {\tilde{S}}_D^n={\int }_0^T{\tilde{v}}_D^{n}dt \\ {S}_D={\int }_0^T{v}_{D}dt \end{gathered}$$
   $${\tilde{S}}_D^n=(1+\delta K_D)[I-({\varphi }_{DU}+{\alpha }_{\varphi })u_U\times ]S_D^n-{\alpha }_{\theta }{S}_D{u}_U$$

5. 将上式分解为水平和垂直两部分：
   ${\tilde{S}}_D^n={\left[\begin{array}{ccc}{\tilde{S}}_{DE}& {\tilde{S}}_{DN}& {\tilde{S}}_{DU}\end{array}\right]}^T$
   $S_D^n={\left[\begin{array}{ccc}{S}_{DE}& S_{DN}& S_{DU}\end{array}\right]}^T$
   ${\tilde{S}}_{HD}^n={\left[\begin{array}{ccc}{\tilde{S}}_{DE}& {\tilde{S}}_{DN}& 0\end{array}\right]}^T$
   $S_{DH}^n={\left[\begin{array}{ccc}{S}_{DE}& S_{DN}& 0\end{array}\right]}^T:水平面上的投影$
   $$\begin{gathered} {\tilde{S}}_{DH}^n=(1+\delta K_D)[I-({\varphi }_{DU}+{\alpha }_{\varphi })u_U\times ]S_{DH}^n \\ {\tilde{S}}_{DU}=(1+\delta K_D)S_{DU}-{\alpha }_{\theta }{S}_D \end{gathered}$$

假设载车从某一点A开始沿某一路线，行驶一圈又回到A点，如上图在行驶路线上取一点B，其对应的计算位移为点C，做点D：$AC\perp BD$，真实位移$S_{DH}^n=\overrightarrow{AB}$,计算位移${\tilde{S}}_{DH}^n=\overrightarrow{AC}$,转动角度为${\varphi }_{DU}+{\alpha }_{\varphi }$.
（1）：航位推算轨迹与真实轨迹的相似性：真实位移移动${\varphi }_{DU}+{\alpha }_{\varphi }$角，在扩大$(1+\delta K_D)$倍得到计算位移，${\tilde{S}}_{DH}^n$
（2）：$\overrightarrow{BD}$:垂直方向位移$\overrightarrow{DC}$:水平方向位移
（3）：$\overrightarrow{BC}$：误差向量当载车距离起始点A越远误差越大，当放回原点误差会逐渐消失
（4）：定义高度误差为：$\delta S_{DU}={\tilde{S}}_{DU}-S_{DU}=\delta K_D{S}_{DU}-{\alpha }_{\theta }{S}_D$,当里程仪刻度系数误差较小时$\delta K_D$近似有$\delta S_{DU}\approx -a\theta S_D$

惯性/里程仪组合

在这里插入图片描述

里程仪转向与杆臂矫正
如上图：
$O_1为AB前轮AB中心点$
$O_2为后轮CD中心点$
$\varphi 为前轮偏转角$
$v_D:前轮/转向轮差动式里程仪输出，点O_1的与地面速度$

1. 里程仪输出在车体坐标系（m系）的投影为：
   $$v_D^m={\left[\begin{array}{ccc}{v}_{D}sin\varphi & v_{D}cos\varphi & 0\end{array}\right]}^T$$
   偏转角$\varphi$，右转为正,$-\pi /2<\varphi <\pi /2$
   $r_{\varphi }=\frac{v_D}{\dot{\varphi }}$
   $$\varphi =\angle O_{1}O{O}_2=arcsin\frac{dL}{r_{\varphi }}$$

2. 得到前轮里程仪的输出为：
   $$v_D^m={\left[\begin{array}{ccc}{d}_L\dot{\varphi }& \sqrt{v_D^2-(d_L\dot{\varphi }{)}^2}& 0\end{array}\right]}^T$$
   杆臂误差为：
   $$\delta l_D^b={\left[\begin{array}{ccc}dx& dy& 0\end{array}\right]}^T$$

3. 得到里程计与惯导之间的杆臂误差为：
   $$\delta P_{DL}=P_{INS}-P_D=-M_{pv}{C}_b^n\delta l_D^b$$

惯导/航位推算组合模式

1. 获得捷联惯导解算姿态更新值

2. 使用惯导的姿态矩阵对里程仪测量进行坐标变化，获得导航坐标系下的航位推算速度

3. 此时惯导解算与航位解算有共同的姿态阵，具有相同的失准角误差，将惯导误差与航位推算误差合并，得到状态向量：
   $K_D,{\xi }^b,{▽}^b：随机常值向量$
   $$X={\left[\begin{array}{ccccccc}{\varphi }^T& (\delta v^n{)}^T& (\delta p{)}^T& (\delta p_D{)}^T& ({\xi }^b{)}^T& ({▽}^b{)}^T& K_D^T\end{array}\right]}^T$$

4. 获得里程仪相对于惯组的杆臂，并矫正

5. 以惯导解算位置和航位推算位置之差构造观测量：
   $$Z={\tilde{P}}_{INS}-\delta P_{DL}-{\tilde{P}}_D=\delta P_{INS}-\delta P_D$$
   $$\begin{gathered} \{\begin{array}{l}\dot{X}=FX+G{W}^b\\ Z=HX+V\end{array} \\ st: \\ F=\left[\begin{array}{ccccccc}{M}_{aa}& M_{av}& M_{ap}& 0_{3\times 3}& -C_b^n& 0_{3\times 3}& 0_{3\times 3}\\ M_{va}& M_{vv}& M_{vp}& 0_{3\times 3}& 0_{3\times 3}& C_b^n& 0_{3\times 3}\\ 0_{3\times 3}& M_{pv}& M_{pp}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ M_{paD}& 0_{3\times 3}& M_{pp}& M_{ppD}& 0_{3\times 3}& 0_{3\times 3}& M_{pkD}\\ 0_{9\times 21}& & & & & & \end{array}\right],G=\left[\begin{array}{cc}-C_b^n& 0_{3\times 3}\\ 0_{3\times 3}& C_b^n\\ 0_{15\times 6}& \end{array}\right],W^b=\left[\begin{array}{c}{W}_g^b\\ W_g^b\end{array}\right] \\ H=\left[\begin{array}{cccc}{0}_{3\times 6}& I_{3\times 3}& -I_{3\times 3}& 0_{3\times 9}\end{array}\right] \end{gathered}$$

惯导/航位推算增量组合模式

在路况不好时，会造成里程仪打滑或者滑行，难以满足车辆转弯原理，需要抛弃这种状态不好的数据，只采用状态良好的数据，但从不良状态恢复至良好时，航位推算精度依然会损失，因此使用良好数据时间内的增量与惯导组合，降低误差。
惯导/航位推算增量组合的量测构造方法如下：

1. 惯导解算速度与里程仪解算速度之间误差为：
   $$z={\tilde{v}}_{INS}^n-{\tilde{v}}_D^n=\delta v_{INS}^n-\delta v_D^n\approx -{\tilde{v}}_D^n\times \varphi +\delta v_{INSS}^n-M_{vkD}{K}_D$$

2. 在足够小的时间：$[t_{j-1},t_j]$内，惯导失准角$\varphi$视为常量，对第一步结果进行积分
   $T_j=t_j-t_{j-1}=1s$
   ${\int }_z(t_j)=\widetilde{{\int }_{vINS}}(t_j)-\widetilde{{\int }_{vD}}(t_j)$
   $\widetilde{{\int }_{vD}}(t_j)={\int }_{t_{j-1}}^{t_j}{\tilde{v}}_D^{n}dt$
   ${\int }_M(t_j)={\int }_{t_{j-1}}^{t_j}{M}_{vkD}dt$
   $$\begin{gathered} {\int }_z(t_j)=\widetilde{{\int }_{vINS}}(t_j)-{\widetilde{{\int }_{vD}}}^{t_j}(t_j) \\ =-\widetilde{{\int }_{vD}}(t_j)\times \varphi (t_j)+[\delta v_{INS}^n(t_j)T_j-\frac{\delta v_{INS}^n(t_{j-1})+\delta v_{INS}^n(t_j)}{2}{T}_j]-{\int }_M(t_j)K_D \end{gathered}$$

3. 惯导速度误差为:
   $$\delta {\dot{v}}_{INS}^n\approx f_{sf}^n\times \varphi (t_j)$$
   对等式两边积分可得:
   $$\delta v_{INS}^n(t_j)-{\delta }_{INS}^n(t_{j-1})\approx {\int }_{t_{j-1}}^{t_j}{f}_{sf}^{n}dt\times \varphi (t_j)$$

4. 将第3步求得等式带入第2步，得到：
   $${\int }_z(t_j)=-[\frac{T_j}{2}{\int }_{t_{j-1}}^{t_j}{f}_{sf}^{n}dt+\widetilde{{\int }_{vD}}(t_j)]\times \varphi (t_j)+\delta v_{INS}^n(t_j)T_j-{\int }_M(t_j)K_D$$

5. 构造惯导/航位推算增量方程为：

   $$\begin{gathered} \{\begin{array}{l}\dot{X}=FX+G{W}^b\\ Z(t_j)={\int }_z(t_j)=H(t_j)X(t_j)+V(t_j)\end{array} \\ st: \\ X={\left[\begin{array}{cccccc}{\varphi }^T& (\delta v^n{)}^T& (\delta p{)}^T& ({\xi }^b{)}^T& ({▽}^b{)}^T& K_D^T\end{array}\right]}^T \\ F=\left[\begin{array}{ccccccc}{M}_{aa}& M_{av}& M_{ap}& 0_{3\times 3}& -C_b^n& 0_{3\times 3}& 0_{3\times 3}\\ M_{va}& M_{vv}& M_{vp}& 0_{3\times 3}& 0_{3\times 3}& C_b^n& 0_{3\times 3}\\ 0_{3\times 3}& M_{pv}& M_{pp}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{9\times 21}& & & & & & \end{array}\right],G=\left[\begin{array}{cc}-C_b^n& 0_{3\times 3}\\ 0_{3\times 3}& C_b^n\\ 0_{12\times 6}& \end{array}\right],W^b=\left[\begin{array}{c}{W}_g^b\\ W_g^b\end{array}\right] \\ H(t_j)=\left[\begin{array}{ccc}-[\frac{T_j}{2}{\int }_{t_{j-1}}^{t_j}{f}_{sf}^{n}dt+\widetilde{{\int }_{vD}}(t_j)]\times & T_j{I}_{3\times 3}& 0_{3\times 9}-{\int }_M(t_j)\end{array}\right] \end{gathered}$$