## 3 Gaussian-Process-Based MPC
### MPC for AUV Control
\begin{array}{c}\mathbf{x}(k+1|k)=\mathbf{a}\mathbf{x}(k|k)+\mathbf{b}\mathbf{u}^{\prime}(k|k)\\ \mathbf{x}(k+2|k)=\mathbf{a}\mathbf{x}(k+1|k)+\mathbf{b}\mathbf{u}^{\prime}(k+1|k)\\ =\mathbf{a}^{2}\mathbf{x}(k|k)+\mathbf{a}\mathbf{b}\mathbf{u}^{\prime}(k|k)+\mathbf{b}\mathbf{u}^{\prime}(k +1|k)\\ \vdots\\ \mathbf{x}(k+p|k)=\mathbf{a}^{p}\mathbf{x}(k|k)+\sum\limits_{i=0}^{p-1}\mathbf{a}^{p-1-i}\mathbf{ b}\mathbf{u}^{\prime}(k+i|k)\end{array}  \tag{33}\]  
The derivation process mentioned above can be expressed in a compact matrix form as follows:  
\[\mathbf{X}_{p}(k)=\mathbf{A}(k)\mathbf{x}(k|k)+\mathbf{B}(k)\mathbf{U}(k) \tag{34}\]  
\[\mathbf{Y}_{p}(k)=\mathbf{C}(k)\mathbf{X}_{p}(k) \tag{35}\]  
where  
\[\mathbf{A}(k)= \begin{bmatrix}\mathbf{a}\\ \mathbf{a}^{2}\\ \vdots\\ \mathbf{a}^{p}\end{bmatrix}  \tag{36}\] \[\mathbf{B}(k)= \begin{bmatrix}\mathbf{b}&\mathbf{0}&\cdots&\mathbf{0}\\ \mathbf{a}\mathbf{b}&\mathbf{b}&&\mathbf{0}\\ \vdots&\vdots&&\ddots&\vdots\\ \mathbf{a}^{p-1}\mathbf{b}&\mathbf{a}^{p-2}\mathbf{b}&\cdots&\mathbf{b}\end{bmatrix} \] \[\mathbf{C}(k)= \begin{bmatrix}\mathbf{c}&\mathbf{0}&\cdots&\mathbf{0}\\ \mathbf{0}&\mathbf{c}&\cdots&\mathbf{0}\\ \vdots&\vdots&\ddots&\vdots\\ \mathbf{0}&\mathbf{0}&\cdots&\mathbf{c}\end{bmatrix} \]  
To attain the required acceleration for trajectory tracking tasks with a fully driven AUV, the generalized force \(\mathbf{\tau}(k)\) can be computed using the dynamic inversion method, utilizing the input variables \(\mathbf{x}(k-1)\) and \(\mathbf{u}^{\prime}(k)\). The introduction of \(\mathbf{\tau}(k)\) is necessary to obtain the required control input for the AUV to follow the desired trajectory. \[\mathbf{\tau}(k)=\mathbf{M}\mathbf{u}^{\prime}(k)+\mathbf{C}(\mathbf{v}(k-1))\mathbf{v}(k-1) \tag{37}\] \[+D(\mathbf{v}(k-1))\mathbf{v}(k-1)+\mathbf{g}(\mathbf{\eta}(k-1))\]To conduct obstacle avoidance research on the underactuated AUV horizontal plane model, the following state prediction model is derived by employing the same derivation method as previously mentioned, with the propeller propulsion \(T_{prop}\) and rudder angle \(\delta\) selected as the input \(\mathbf{u}\in\mathbb{R}^{2}\):  
\[\mathbf{x}(k+1)=\mathbf{a}(k)\mathbf{x}(k)+\mathbf{b}(k)\mathbf{u}(k) \tag{38}\]  
\[\mathbf{y}(k)=\mathbf{c}(k)\mathbf{x}(k) \tag{39}\]  
With  
\[\mathbf{a}(k)= \begin{bmatrix}\mathbf{M}^{-1}\mathbf{f}(k)T+\mathbf{F}^{3\times 3}&\mathbf{O}^{3 \times 3}\\ \mathbf{R}(\mathbf{\psi}(k))T&\mathbf{F}^{3\times 3}\end{bmatrix}  \tag{40}\]  
\[\mathbf{b}(k)= \begin{bmatrix}\mathbf{M}^{-1}\mathbf{L}(k)T\\ \mathbf{O}^{3\times 2}\end{bmatrix}  \tag{41}\]  
\[\mathbf{c}(k)= \begin{bmatrix}\mathbf{O}^{3\times 3}&\mathbf{I}^{3\times 3}\end{bmatrix}  \tag{42}\]  
The matrices \(\mathbf{f}\), \(\mathbf{L}\), \(\mathbf{R}\), and \(\mathbf{J}\) are functions of the state of the AUV, which can be found in Section 2. When employing the MPC method to predict \(p\) steps ahead, it is necessary to compute the state of the previous step at each iteration and recalculate these coefficient matrices, resulting in an increased computational burden. When investigating obstacle avoidance motion in three-dimensional space, we extend the horizontal plane model by introducing motion along the \(z\)-axis direction and rotational motion around the \(y\)-axis. This additional degree of freedom allows the AUV to navigate and avoid obstacles in a three-dimensional underwater environment. By employing the same methodology, a state prediction model can also be derived for the obstacle avoidance of underactuated AUVs in three-dimensional space.