# Nonlinear Trajectory Tracking Controller for Underwater Vehicles with Shifted Center of Mass Model
## 2 Description of Problem  
### Underactuated Marine Vehicle Model  
An underactuated marine vehicle and the frame coordinates are presented in Figure 1.  
The position and orientation variables vector is \(\eta=[x,y,z,\theta,\psi]^{T}\), whereas the kinematic and dynamic models are [2]  
\[\dot{\eta}=J(\eta)\
u, \tag{1}\] \[M\dot{\psi}+C(\
u)\
u+D(\
u)\
u+g(\eta)=\tau+\tau_{D}, \tag{2}\]  
where \(\
u=[u,v,w,q,r]^{T}\) means the vector of linear and angular velocities (taking into account surge, sway, heave, pitch, and yaw velocity). The vector \(\tau=[\tau_{u},0,0,\tau_{q},\tau_{r}]^{T}\) means the set of control input signals. The symbol \(\tau_{D}\) means the vector of environmental disturbances together with unknown dynamics (because, in the dynamic equation, the matrix \(M\) must be symmetric). Other matrices and vectors are \((c\theta,c\psi,s\theta,s\psi\) are abbreviations of \(cos\theta,\cos\psi,\sin\theta,\sin\psi)\)  
Figure 1: Underactuated underwater vehicle model and frame coordinates. \[J(\eta)=\left[ \begin{array}{cccc}c\phi c\psi&-s\psi&s\theta c\psi&0&0\\ c\theta s\psi&c\psi&s\theta s\psi&0&0\\ -s\theta&0&c\theta&0&0\\ 0&0&0&1&0\\ 0&0&0&0&1/c\theta\end{array} \right],\ \ \ \ \ M=\left[ \begin{array}{cccc}m_{11}&0&0&m_{14}&0\\ 0&m_{22}&0&0&m_{25}\\ 0&0&m_{33}&m_{34}&0\\ m_{14}&0&m_{34}&m_{44}&0\\ 0&m_{25}&0&0&m_{55}\end{array} \right],\] \[C(\
u)=\left[ \begin{array}{cccc}0&0&0&c_{14}&c_{15}\\ 0&0&0&c_{24}&c_{25}\\ 0&0&0&c_{34}&0\\ -c_{14}&-c_{24}&-c_{34}&0&0\\ -c_{15}&-c_{25}&0&0&0\end{array} \right],\ \ \ \ D(\
u)=\left[ \begin{array}{cccc}d_{11}&0&0&d_{14}&0\\ 0&d_{22}&0&0&d_{25}\\ 0&0&d_{33}&d_{34}&0\\ d_{41}&0&d_{43}&d_{44}&0\\ 0&d_{52}&0&0&d_{55}\end{array} \right], \tag{3}\]  
and \(g(\eta)=[0,0,0,g_{4},0]^{T}\) (for neutrally buoyant vehicle \(W=B\) with \(g_{4}=z_{g}W\delta+x_{g}Wc\theta\)). Moreover, \(J(\eta)\) denotes the transformation matrix, \(M\) denotes the mass matrix, including the added masses, \(C(\
u)\) denotes the Coriolis and centripetal forces matrix, and \(D(\
u)\) is the hydrodynamic damping matrix. The disturbances are included in vector \(\tau_{D}=[\tau_{D\
u},\tau_{D\
u},\tau_{D\
u},\tau_{D\sigma},\tau_{D\sigma}]^{T}\). Elements of the matrices are defined in Appendix A. Noting that \(C^{\circ}(\
u)=C(\
u)\
u\) and \(D^{\circ}(\
u)=D(\
u)\
u\), one obtains  
\[M\dot{\psi}+C^{\circ}(\
u)+D^{\circ}(\
u)+g(\eta)=\tau+\tau_{D}, \tag{4}\]  
where \(C^{\circ}(\
u)=[c_{1}^{\circ},c_{2}^{\circ},c_{3}^{\circ},c_{4}^{\circ},c_{5} ^{\circ}]^{T}\) and \(D^{\circ}(\
u)=[d_{1}^{\circ},d_{2}^{\circ},d_{3}^{\circ},d_{4}^{\circ},d_{5} ^{\circ}]^{T}\). Equation (4) can be rewritten in more compact form:  
\[M\dot{\psi}=F(\
u,\eta)+\tau+\tau_{D}, \tag{5}\]  
where \(F(\
u,\eta)=-[C^{\circ}(\
u)+D^{\circ}(\
u)+g(\eta)]\). The paper considers the effect of the center of mass shift on the trajectory tracking task. For this reason, all disturbances due to parameter inaccuracies and environmental influences are collected in an additional component that reflects them. The inertia matrix, in contrast, includes displacements so that their effect on system dynamics during vehicle movement can be estimated. Equation (4) represents a description of the dynamics that ensure that this condition is met. One of the possible decomposition methods used in this paper is given in [31]. The application of the equations expressed in IQVs for fully actuated marine vehicles can be found, e.g., in [32]. After decomposition of the matrix \(M\), one obtains a diagonal matrix \(\mathbb{N}=\hat{Y}^{T}M\hat{Y}\) (\(\hat{Y}\) contains nominal parameters). This also means that \(M=\hat{Y}^{-T}\mathbb{N}\hat{Y}^{-1}\) (both matrices are positive definite). All nonlinearities resulting from their omission from the real matrix \(\mathbb{Y}\) occur in the modified perturbation function \(f=\tau_{D}+\Delta Y\), where \(f=[f_{u},f_{v},f_{w},f_{q},f_{r}]^{T}\). Furthermore, the nominal value of the matrix \(\hat{\mathbb{N}}\) is \(\hat{\mathbb{N}}=\hat{Y}^{T}M\hat{Y}\).