## 3 Gaussian-Process-Based MPC
### Cost Function for Control Performance Evaluation  
Building on the foundation established through our understanding of AUV motion dynamics and operational constraints, we now direct our attention to a critical aspect of AUV control: the evaluation of its performance. For the AUV to effectively pursue precise trajectory tracking and adept obstacle avoidance, the formulation of a well-defined cost function becomes paramount in assessing control performance. This cost function, denoted as \(L\), serves as the guiding principle for navigating the intricate challenges inherent to underwater operations. It provides a quantitative means to evaluate the AUV's control performance, ensuring that it not only meets the requirements but excels in executing its designated missions. With the cost function at the forefront, we will explore its formulation and its pivotal role in driving the AUV towards optimal navigation and obstacle avoidance strategies. The cost function, denoted as \(Loss\), is formulated as follows:  
\[\begin{split} L&=\sum\limits_{i=1}^{p}\left(\left( \boldsymbol{x}(k+i|k)-\boldsymbol{x}_{ref}\right)^{\text{T}}\boldsymbol{Q} \Big{(}\boldsymbol{x}(k+i|k)-\boldsymbol{x}_{ref}\Big{)}^{\text{T}}\right.\\ &\quad\quad\left.+\boldsymbol{u}(k+i-1|k)^{\text{T}}\boldsymbol{ R}\boldsymbol{u}(k+i-1|k)+\boldsymbol{\Delta u}^{\text{T}}\boldsymbol{S} \boldsymbol{u}\right)\end{split} \tag{46}\]  
In this formulation, \(\boldsymbol{y}_{ref}\) represents the target trajectory, and \(\boldsymbol{\Delta u}\) denotes the change in the control input \(\boldsymbol{u}\). The matrices \(\boldsymbol{Q}\), \(\boldsymbol{R}\), and \(\boldsymbol{S}\) are configured as diagonal matrices, thereby introducing weighting factors that are crucial to the cost function. In this cost function, the first term primarily controls the distance between the AUV and the target position, while the second and third terms regulate the output force and power change of the AUV to reduce energy consumption and mitigate challenges to actuator performance caused by rapid action changes. These weighting factors determine the relative significance accorded to each term within the function and can be finetuned to align with specific objectives and requirements. The optimization objective is centered on the minimization of this cost function at every sampling instant. For trajectory tracking assignments, \(\boldsymbol{y}_{ref}\) manifests as a time-varying sequence, and the cost function is evaluated using \(\boldsymbol{y}_{ref}(k+i|k)\). By minimizing the cost function, the control system can effectively track the desired trajectory while achieving an optimal performance.  
Through the meticulous process of minimizing this cost function, the control system is empowered to meticulously track the desired trajectory, all while striving for optimal performance. This critical function serves as the linchpin of our approach, offering a quantitative means to evaluate and refine the control strategies employed. It provides the guiding metrics by which the effectiveness of our AUV control methods is measured, allowing us to navigate the intricate interplay between trajectory tracking and obstacle avoidance with precision and confidence. Consequently, the careful formulation of this cost function stands as a cornerstone of our research, ensuring that our AUV's underwater operations achieve the highest standards of performance.