## 2 Mathematical Model
### Submarine Motion Equation
\begin{array}{rl}Z_{1}&=\frac{1}{2}\rho L^{4}\Big{[}Z^{\prime}_{pp}pp+Z^{ \prime}_{rr}rr+Z^{\prime}_{rp}rp\Big{]}+\frac{1}{2}\rho L^{2}\Big{[}Z^{\prime} _{wp}vv+Z^{\prime}_{\delta_{r}}u^{2\delta_{\delta}}+Z^{\prime}_{\delta_{b}}u^ {2\delta_{b}}\Big{]}\\ &+\frac{1}{2}\rho L^{3}\Big{[}Z^{\prime}_{pr}vr+Z^{\prime}_{vp}vp\Big{]}+\frac {1}{2}\rho L^{3}\Big{[}Z^{\prime}_{q}u+Z^{\prime}_{|q|\delta_{\delta}}u|q| \delta_{\delta}+Z^{\prime}_{w|q|}u\frac{v}{|v|}\big{[}(v^{2}+w^{2})^{1/2}\Big{]} \big{|}|q|\Big{]}\\ &+\frac{1}{2}\rho L^{2}\Big{[}Z^{\prime}_{q}u^{2}+Z^{\prime}_{w}uw+Z^{\prime} _{w|w|}w\big{[}v(v^{2}+w^{2})^{1/2}\big{]}+Z^{\prime}_{|w|}u|w+Z^{\prime}_{ww} w(v^{2}+w^{2})^{1/2}\Big{]}\\ &-m\big{[}vp-uq+x_{G}rp+y_{G}q-z_{G}(p^{2}+q^{2})\big{]}+(P-B)\cos\theta\cos \varphi\end{array}  \tag{4}\]  
\[ \begin{array}{rl}K_{1}&=\frac{1}{2}\rho L^{5}\Big{[}K^{\prime}_{qr}qr+K^{ \prime}_{pq}pq+K^{\prime}_{p|p|}p|\Big{]}\\ &+\frac{1}{2}\rho L^{4}\Big{[}K^{\prime}_{p}up+K^{\prime}_{ur}ur+K^{\prime}_{ qq}vq+K^{\prime}_{wp}wp+K^{\prime}_{ur}wr\Big{]}\\ &+\frac{1}{2}\rho L^{3}\Big{[}K^{\prime}_{0}u^{2}+K^{\prime}_{q}uv+K^{\prime}_ {\delta_{r}}u^{2\delta_{r}}+K^{\prime}_{v|q|}v\sqrt{v^{2}+w^{2}}+K^{\prime}_{ vu}vw\Big{]}\\ &-\big{(}l_{xx}-l_{yy}\big{)}qr+pql_{xx}-\big{(}r^{2}-q^{2}\big{)}l_{yz}-l_{xy} pr-m\big{[}y_{G}(vp-uq)-z_{G}(ur-wp)\big{]}\\ &+(y_{G}P-y_{B}B)\cos\theta\cos\varphi-(z_{G}P-z_{B}B)\cos\theta\sin\varphi \end{array}  \tag{5}\]  
\[ \begin{array}{rl}M_{1}&=\frac{1}{2}\rho L^{5}\Big{[}M^{\prime}_{pp}pp+M^{ \prime}_{rr}rr+M^{\prime}_{rp}rp+M^{\prime}_{q|q|}q|q|\Big{]}+\frac{1}{2}\rho L ^{3}\Big{[}M^{\prime}_{wp}vv+M^{\prime}_{\delta_{b}}u^{2\delta_{b}}+M^{ \prime}_{\delta_{b}}u^{2\delta_{b}}\Big{]}\\ &+\frac{1}{2}\rho L^{4}\Big{[}M^{\prime}_{pv}vr+M^{\prime}_{vp}vp+M^{\prime} _{q}uq+M^{\prime}_{|q|\delta_{\delta}}u|q|\delta_{\delta}+M^{\prime}_{|w|q|}q \big{[}(v^{2}+w^{2})^{1/2}\big{]}\Big{]}\\ &+\frac{1}{2}\rho L^{3}\Big{[}M^{\prime}_{0}u^{2}+M^{\prime}_{w}uw+M^{\prime}_ {w|w|}w\big{[}(v^{2}+w^{2})^{1/2}\big{]}+M^{\prime}_{|w|}u|w|+M^{\prime}_{ww} w\big{[}(v^{2}+w^{2})^{1/2}\big{]}\Big{]}\\ &-\big{(}l_{xx}-l_{zz}\big{)}rp+l_{xy}qr-\big{(}p^{2}-r^{2}\big{)}l_{xx}-l_{ yz}qp-m\big{[}z_{G}(wq-vr)-x_{G}(vp-uq)\big{]}\\ &-(x_{G}P-x_{B}B)\cos\theta\cos\varphi-(z_{G}P-z_{B}B)\sin\theta\end{array}  \tag{6}\]  
\[ \begin{array}{rl}N_{1}&=\frac{1}{2}\rho L^{5}\Big{[}N^{\prime}_{pq}pq+N^{ \prime}_{qr}qr+N^{\prime}_{r|r|}r|r|\Big{]}+\frac{1}{2}\rho L^{4}\Big{[}N^{ \prime}_{wr}wr+N^{\prime}_{wp}wp+N^{\prime}_{uw}vq\Big{]}\\ &+\frac{1}{2}\rho L^{4}\Big{[}N^{\prime}_{p}up+N^{\prime}_{r}ur++N^{\prime}_ {|r|\delta_{r}}u|r|\delta_{r}+N^{\prime}_{|v|r}r\big{[}(v^{2}+w^{2})^{1/2} \big{]}\Big{]}\\ &+\frac{1}{2}\rho L^{3}\Big{[}N^{\prime}_{0}u^{2}+N^{\prime}_{w}uw+N^{\prime}_ {w|v|v}v(v^{2}+w^{2})^{1/2}+N^{\prime}_{vw}vw+N^{\prime}_{\delta_{r}}u^{2 \delta_{r}}\Big{]}\\ &-\big{(}l_{yy}-l_{xx}\big{)}pq+l_{yz}rp-\big{(}q^{2}-p^{2}\big{)}l_{xy}-l_{ xx}rq-m\big{[}x_{G}(wp+ur)-y_{G}(vr+wq)\big{]}\\ &+(x_{G}P-x_{B}B)\cos\theta\cos\varphi+(y_{G}P-y_{B}B)\sin\theta\end{array}  \tag{7}\]  
This work considers a simplified propeller model for the propulsion system, with negligible consideration given to the effects of load variations during maneuvering. The thrust force \(F_{p}\) is calculated using the following equation:  
\[F_{p}=\rho n^{2}D_{p}^{4}K_{T}(1-t_{p}) \tag{8}\]  
where \(\rho\) represents the fluid density, \(n\) denotes the propeller rotational speed, \(D_{p}\) corresponds to the propeller diameter, \(K_{T}\) represents the thrust coefficient, and \(t_{p}\) is the thrust deduction factor.