# Linear Parameter-Varying Model Predictive Control of AUV for Docking Scenarios
## 3 AUV Control System
### Linear Parameter-Varying Model Predictive Control  
The MPC iteratively solves a finite-horizon, optimal control problem using a prediction model of the controlled system and an estimation of the current state. When the cost function is quadratic and constraints are affine, the LPV-MPC optimization problem to be solved at each time-step can be summarized as:  
\[\min_{u}\sum_{i=1}^{N_{p-1}}||Q(y_{AUV}(k+i|k)-\tau_{SP}(k))||_{2}^{2}+\sum_ {j=1}^{N_{u}}||R(\Delta\tau_{V}(k+j|k))||_{2}^{2} \tag{12}\]  
\[+||P(y_{AUV}(k+N_{p}|k)-\tau_{SP}(k)||_{2}^{2}\]  
\[s.t.\ x_{AUV}(k+i+1|k)=A_{d}(k)x(k+i|k)+B_{d}(k)\tau_{V}(k+i)+d_{d}(k) \tag{13}\]  
\[y_{AUV}(k+i|k)=C(k)x_{AUV}(k+i|k) \tag{14}\]  
\[x_{AUV}(k|k)=\hat{x}_{AUV}(k) \tag{15}\]  
\[\tau_{V}(k+i+1)=\tau_{V}(k+i)\ \ \ \ \ \ \ for\ i>N_{u} \tag{16}\]  
\[\tau_{V}(k+i+1)=\tau_{V}(k+i)+\Delta\tau_{V}(k+i+1)\ \ \ \ for\ i\leq N_{u} \tag{17}\]  
\[\Delta u_{V}(k+i)\in D \tag{18}\]  
\[\tau_{V}(k+i)\in U \tag{19}\]\[y_{AUV}(k+i)\in Y \tag{20}\] \[+||P(y_{AUV}(k+N_{p}|k)-\tau_{SP}(k)||_{2}^{2}\]0  
where \(P\), \(Q\) and \(R\) the matrices for weighting the predicted output error, input rate and final output value (terminal weight), respectively, \(y_{AUV}(k+i|k)\) is the output predicted at the \(i\)-th prediction step, \(r_{SP}(k)\) is the reference output considered constant over the prediction/control horizon, \(\Delta\tau_{V}(k+j|k)\) and \(\tau_{V}(k+j|k)\) are the predicted control input-rate and input-magnitude, respectively, \(N_{p}\) is the prediction horizon and \(N_{u}\) is the control horizon, \(D\), \(U\) and \(Y\) are the convex sets on input rate, input and controlled output, respectively. A standard LPV-MPC problem involves the assumption that the prediction model can be assumed constant over the prediction horizon, so that the model should match the real system for the first prediction step. Because the control action computed by MPC is mapped into the actuators control signals space by TA, there would be a mismatch between virtual forces and moments computed by MPC and real control forces and moments driving the AUV. This approximation introduces some sub-optimality in the solution, but enables the computational effort to be reduced, particularly with respect to more expensive nonlinear optimal control policies (e.g., nonlinear MPC). However, the controller performance can degrade because of the iterative introduction of an error in the prediction. To compensate for this problem, it is possible to iteratively evaluate the prediction of the plant model at least for the first prediction step. The prediction error \(e_{p}(k)\) can be iteratively computed by using the previous control effort so that,  
\[+||P(y_{AUV}(k+N_{p}|k)-\tau_{SP}(k)||_{2}^{2}\]1  
where \(\hat{x}_{AUV}(k)\) is the state-estimate (provided by the Kalman filter), \(\hat{x}_{AUV}(k-1)\) is the state-estimate at the previous time step, \(\tau_{V}(k-1)\) is the last computed control signal, \(d_{d}(k-1)\) is the last computed disturbance signal and \(A_{d}(k-1),\ B_{d}(k-1)\) are the matrices describing the model at the previous time instant. The prediction error, computed as in Equation (22), can be introduced in the prediction model by updating the disturbance vector \(d_{d}(k)\) iteratively, so that at each time instant it will be replaced by:  
\[+||P(y_{AUV}(k+N_{p}|k)-\tau_{SP}(k)||_{2}^{2}\]2  
with \(d_{d}(k)\) the original disturbance vector. To reduce the computational complexity of the controller, the LPV-MPC problem can be represented as a condensed Quadratic Programming (QP) problem that can be implemented and solved on an embedded platform:  
\[+||P(y_{AUV}(k+N_{p}|k)-\tau_{SP}(k)||_{2}^{2}\]3 \[+||P(y_{AUV}(k+N_{p}|k)-\tau_{SP}(k)||_{2}^{2}\]4  
where \(z\) is the vector of optimization variables, \(\gamma_{k}\) is the vector of MPC parameters belonging to a given set of interest, \(H_{k}\) is a symmetric and positive definite matrix, \(F_{k}\) is the linear cost-term matrix and \(G_{k},h_{k}\) and \(w_{k}\) are the terms defining constraints on input and output.