## 5 Simulation Results and Discussions
### Tracking Control with Acoustic Localization
#### 5.1.2 Tracking Control Results without Prediction Biased Errors  
For the MPC controller design, system state feedback is necessary at each time step as in Equation (33). However, due to the uncertainties in system dynamics and measurements, only estimated system states from EKF or predicted system states from dead-reckoning with nominal system dynamics can be utilized. Figure 5 shows that the proposed algorithm can achieve the trajectory tracking goal and control the AUV moving from the initial position to the destination along the path. In Figure 5, five trajectories are plotted in total:  
1. The reference path is determined by connecting the initial position and the destination;
2. The real AUV trajectory is \(\mathbf{x}(t)\) recorded from the simulations by evolving Equation (20) based on Equations (1) and (2) with known uncertain ocean currents. 3. The estimated AUV trajectory \(\mathbf{\hat{x}}(t)\) is the trajectory from the EKF, i.e., from Equation (26). This trajectory is obtained by correcting the dead-reckoning trajectory using the acoustic localization feedback information, i.e., the \"measured\" output, \(\mathbf{\hat{y}}(t)\). 4. The dead-reckoning trajectory \(\mathbf{\hat{x}}(t)\) is calculated from Equation (20) by ignoring the system uncertainties, i.e., Equation (28). 5. The measured outputs \(\mathbf{y}(t)\) are obtained from Equation (18) by adding stochastic uncertainties at the computed results from the TOAs according to Equation (17) at each localization step. Overall, the differences among these trajectories are small. Therefore, a zoom-in around position (300, 300) m is added. The root-mean-square-error for tracking the reference path is 5.59 m which is qualified in applications such as large-area ocean mapping. Note that the estimated trajectory and the measured trajectory are plotted with fewer position points than the dead-reckoning trajectory since the control cycle is faster than the acoustic localization cycle. Figure 4: Errors between the real horizontal distance and the estimated horizontal distance. Figures 6 and 7 show the speed and control input trajectories, respectively. The control inputs, i.e., the propeller thrust and the rudder angle, computed by solving the optimization problem Equation (32) subject to Equations (22), (33)-(36) all satisfy the system constraints. However, for the speed trajectory, the dead-reckoning speed trajectory satisfies the constraints well while small violations of the real and estimated trajectories with respect to the speed limits are observed. This is because both the optimization problem Equation (32) subject to Equations (22), (33)-(36) and the dead-reckoning trajectory use the nominal system dynamics without uncertainties. If all the constraints Equations (22), (33)-(36) are satisfied in the optimization problems, the dead-reckoning trajectory will also satisfy the constraints. However, compared to the dead-reckoning trajectory, the real trajectory further incorporates the uncertainties in ocean currents, and the estimated trajectory further incorporates the uncertainties in measurements. These uncertainties push the dead-reckoning trajectories that are already on the constraint boundary out of the feasible zone. Approaches such as buffer zone design in [31] can be applied to deal with this issue. Note that the differences between the estimated and dead-reckoning speed trajectories are small, and the root-mean-square-root value between these two trajectories is 0.03 m/s. This difference demonstrates the correction capability of the EKF as in Equation (26). Figure 5: AUV tracking control trajectories: real-\(\mathbf{x}(t)\), estimated-\(\mathbf{\xi}(t)\), dead-reckoning-\(\mathbf{\xi}(t)\), and measured-\(\mathbf{y}(t)\). Figure 6: Speed trajectories and constraints. To relieve the potential computational burden of the MPC online optimization problems, successively linearized prediction models Equation (22) are used. Therefore, only linear programming online problems need to be solved.