## 4 Tracking Control Based on EKF
### Predictive Tracking Control  
Briefly, at each time step \(k\), MPC computes a future control sequence over the prediction horizon \(N_{\rm p}\) based on the current system state. The predicted system trajectory is then driven close to the given reference. Only the first element of the control sequence is applied to the system. At the next time step \(k+1\), this process is repeated over the shifted prediction horizon with newly measured system states. MPC is based on solving optimization problems online repetitively and mostly numerically, which naturally and explicitly handles system constraints. Therefore, MPC is especially suitable to systems of which analytical control laws are difficult to derive, and suitable to systems with constraints. For the AUV tracking problem, system constraints Equations (4)-(6) are imposed. We next propose the MPC approach to solve the AUV tracking problem with the corrected information from EKF or the predicted information from dead-reckoning. Consider at time step \(k\), given the current estimated \(\bar{\mathbf{x}}(k)\) when the acoustic estimated state \(\bar{\mathbf{x}}(k)\) is not available, future predicted trajectories \(x(i|k)\) over \(N_{\rm p}\) are then made following Equation (22). Moreover, given a reference path determined by an origin \((x_{\rm o},y_{\rm o})\) and a destination \((x_{\rm d},y_{\rm d})\) in the inertial coordinate system, a constant reference speed \(u_{\rm d}\) is set to parameterize path. The reference trajectory over a future prediction horizon \(N_{\rm p}\) at \(k\) is calculated as  
\[\mathbf{y}_{\rm r}(i|k)=\left[ \begin{array}{c}x_{\rm o}\\ y_{\rm o}\\ \psi_{\rm d}\end{array} \right]+u_{\rm d}T_{\rm S}^{\rm c}(k+i)\left[ \begin{array} []{c}\cos(\psi_{\rm d})\\ \sin(\psi_{\rm d})\\ 0\end{array} \right], \tag{31}\]  
where \(\psi_{\rm d}=\arctan((y_{\rm d}-y_{\rm 0})/(x_{\rm d}-x_{\rm 0}))\). The tracking problem is then represented by solving a constrained finite horizon linear programming problem  
\[\min_{\mathbf{u}(i|k)}||\bar{\mathbf{y}}(N_{\rm p}|k)-\mathbf{y}_{\rm r}(N_{\rm p}|k)||_{ \bar{\mathbf{Q}}_{f}}^{2}+\sum_{i=0}^{N_{\rm p}-1}||\bar{\mathbf{y}}(i|k)-\mathbf{y}_{\rm r }(i|k)||_{\bar{\mathbf{Q}}}^{2}+||\mathbf{u}(i|k)||_{\bar{\mathbf{R}}}^{2}, \tag{32}\]subject to Equation (22) and  
\[x(0|k)=\hat{\mathbf{x}}(k)\text{ or }\bar{\mathbf{x}}(k), \tag{33}\] \[u_{\min}\leqslant u(i|k)\leqslant u_{\max},i=0,1,\cdots,N_{\text{p}},\] (34) \[\xi_{\min}\leqslant\xi(i|k)\leqslant\xi_{\max},i=0,1,\cdots,N_{ \text{p}}-1,\] (35) \[\delta_{\min}\leqslant\delta(i|k)\leqslant\delta_{\max},i=0,1, \cdots,N_{\text{p}}-1, \tag{36}\]  
where \(\hat{\mathbf{y}}(i|k)=\mathbf{C}\mathbf{\xi}(i|k)\). The cost function Equation (32) aims to minimize the tracking error between the predicted state trajectory and the reference trajectory with optimal control efforts. The three terms are the terminal tracking error cost, the stage tracking error cost, and the control input cost, respectively; \(\mathbf{Q}_{f}\), \(\mathbf{Q}\) and \(\mathbf{R}\) are positive definite weighting matrices for the respective cost terms and can be adjusted to set priority between convergence speed and control efforts. Due to the constraints on system states and control inputs, the optimization problem does not have an analytical solution, and needs to be solved numerically repetitively online with newly \"measured\" system states and time-varying prediction models. However, due to the successive linearization, the above optimization problem becomes convex and can be solved efficiently. Overall, the predictive tracking control algorithm with EKF and acoustic localization information or dead-reckoning can be summarized as Algorithm 1.