## 2 Preliminaries  
### Kinematics and Kinetics  
Modeling a marine vehicle involves the study of statics and dynamics. The 6 DOF motion of a marine vehicle is analyzed by defining two coordinate frames, as illustrated in Figure 1. \(O_{b}X_{b}Y_{b}Z_{b}\) is fixed to the vehicle and is called the body-fixed frame. \(OXYZ\) is fixed to the earth and is called the earth-fixed frame. The origin of the body-fixed frame is the center of gravity (CG). The center of buoyancy locates at CG. The notations of the frames used in this paper are [1]  
\[\eta=[\eta_{1}^{\top},\eta_{2}^{\top}]^{\top};\eta_{1}=[x,y,z]^{ \top};\eta_{2}=[\phi,\theta,\psi]^{\top} \tag{1}\] \[\
u=[\
u_{1}^{\top},\
u_{2}^{\top}]^{\top};\
u_{1}=[u,v,w]^{\top}; \
u_{2}=[p,q,r]^{\top} \tag{2}\]  
here \(\eta\) denotes the position and orientation of the vehicle and \(\
u\) denotes the linear and angular velocity of the vehicle. The rigid body kinematics of the vehicle are given by [1]  
\[\eta_{1}=J_{1}(\eta_{2})\
u_{1} \tag{3}\] \[\eta_{2}=J_{2}(\eta_{2})\
u_{2} \tag{4}\]  
in which  
\[J_{1}(\eta_{2})= \begin{bmatrix}c\psi\theta&-s\psi c\phi+c\psi s\theta s\phi&s \psi s\phi+c\psi c\phi s\theta\\ s\psi c\theta&c\psi c\phi+s\phi s\theta s\psi&-c\psi s\phi+s\theta s\psi c\phi\\ -s\theta&c\theta s\phi&c\theta c\phi\end{bmatrix}  \tag{5}\]  
\[J_{2}(\eta_{2})= \begin{bmatrix}1&s\phi t\theta&c\phi t\theta\\ 0&c\phi&-s\phi\\ 0&s\phi/c\theta&c\phi/c\theta\end{bmatrix}  \tag{6}\]  
where \(s\cdot=sin(\cdot),c\cdot=cos(\cdot),t\cdot=tan(\cdot)\). The mathematical model of the 6 DOF rigid body dynamics is  
\[M_{RB}\dot{\
u}+C_{RB}(\
u)\
u=\tau_{RB} \tag{7}\]  
where  
\[\tau_{RB}=\tau_{H}+\tau_{P} \tag{8}\]  
\(\tau_{H}\) denotes the hydrodynamic forces and moments, and \(\tau_{P}\) denotes the propulsion forces and moments. \(\tau_{H}\) can be calculated as  
\[\tau_{H}=-M_{A}\dot{\
u}-C_{A}(\
u)\
u-D(\
u)\
u \tag{9}\]  
For underwater vehicles, if the movement is at low speed, it can be assumed that the vehicle performs a non-coupled motion. For simplicity, \(M_{A}\) and \(D(\
u)\) have a diagonal structure with only linear damping terms on the diagonal  
\[M_{A}=-diag\{X_{u},Y_{\phi},Z_{\psi},K_{\psi},M_{\psi}\} \tag{10}\]The Coriolis terms of added mass are  
\[C_{A}(v)= \begin{bmatrix}0&0&0&0&-Z_{vb}w&Y_{\phi}v\\ 0&0&0&Z_{vb}w&0&-X_{\mu}u\\ 0&0&0&Y_{\phi}v&X_{\mu}u&0\\ 0&-Z_{vb}w&Y_{\phi}v&0&-N_{\tau}r&M_{q}q\\ Z_{vb}w&0&-X_{\mu}u&N_{\tau}r&0&-K_{\rho}p\\ Y_{\phi}v&X_{\mu}u&0&-M_{q}q&K_{\rho}p&0\end{bmatrix}  \tag{11}\]  
The damping terms are  
\[D(v)=-diag\{X_{u},Y_{\
u},Z_{\
u},K_{\rho},M_{q},N_{\tau}\} \tag{12}\]  
For the rigid body, the inertia matrix \(M_{RB}\) is  
\[M_{RB}= \begin{bmatrix}mI_{3\times 3}&0\\ 0&I_{0}\end{bmatrix}  \tag{13}\]  
where  
\[I_{0}= \begin{bmatrix}I_{x}&-I_{xy}&-I_{xz}\\ -I_{yx}&I_{y}&-I_{yz}\\ -I_{zx}&-I_{zy}&I_{z}\end{bmatrix}  \tag{14}\]  
The Coriolis and centripetal terms are  
\[C_{RB}(v)= \begin{bmatrix}0_{3\times 3}&-mS(v_{1})\\ -mS(v_{1})&-S(I_{0}v_{2})\end{bmatrix}  \tag{15}\]  
**Remark 1**. : _The skew-symmetric matrix \(S(\lambda)\) is defined as_  
\[S(\lambda)= \begin{bmatrix}0&-\lambda_{3}&\lambda_{2}\\ \lambda_{3}&0&-\lambda_{1}\\ -\lambda_{2}&\lambda_{1}&0\end{bmatrix}  \tag{16}\]  
_where \(\lambda=[\lambda_{1},\lambda_{2},\lambda_{3}]^{\top}\). \(S\) satisfies \(x^{\top}S(v_{2})x\equiv 0,x,v_{2}\in\mathbb{R}^{3}\)._  
Based on the mechanical structure of the vehicle, the propulsion forces \(f\) and moments \(\tau\) acting on the vehicle are [26, 27, 28]  
\[f= \begin{bmatrix}0\\ f_{3}sina\\ -f_{1}-f_{2}-f_{3}cosa\end{bmatrix}  \tag{17}\]  
\[\tau= \begin{bmatrix}l_{1}(f_{1}-f_{2})\\ l_{2}(f_{1}+f2)-l_{3}f_{3}cosa-\tau_{3}sina\\ -l_{3}f_{3}sina+\tau_{1}-\tau_{2}+\tau_{3}cosa\end{bmatrix}  \tag{18}\]  
where the force and torque generated by each rotor is  
\[\tau_{i} = k_{\tau}\omega^{2} \tag{19}\] \[f_{i} = k_{f}\omega^{2} \tag{20}\]The total moments (18) can be considered as the sum of the following two terms  
\[\tau_{m}=