## 4 Tracking Control Based on EKF
### Successively Linearized Prediction Models  
The highly nonlinear and uncertain AUV dynamics in Equations (1) and (2) render the online optimization based MPC control difficult. The conventional one step linearization approach was proposed for autonomous surface vessels in [30]. The computational time is shown to be much shorter than using the nonlinear prediction models directly. However, the one step linearized model could lead to large linearization errors over the whole prediction horizon and thus degrade the control performance. Therefore, the successive linearization approach proposed in [31] is applied to achieve a trade-off between computational complexity and control performance. The basic idea is to utilize the whole sequence of control inputs from a previous time step and pre-calculate a shifted system trajectory for linearizations over all prediction steps. For completeness while avoiding repetitions, we briefly describe the implemented successive linearization algorithm for the uncertain AUV dynamics. Generalize the AUV dynamics Equations (1) and (2) as:  
\[\hat{x}(t)=f(\mathbf{x}(t),\mathbf{u}(t),\mathbf{b}(t)), \tag{19}\]  
where \(\mathbf{f}:\ \mathbb{R}^{6}\times\mathbb{R}^{2}\times\mathbb{R}^{2}\rightarrow\mathbb{R} ^{6}\) is a nonlinear smooth function; \(\mathbf{x}=[\mathbf{y}^{\mathrm{T}},\mathbf{v}^{\mathrm{T}}]^{\mathrm{T}}\) is the system full state; \(\mathbf{u}=[\xi,\delta]^{\mathrm{T}}\in\mathbb{R}^{2}\) is the control input vector; \(\mathbf{b}\) is defined as before. For numerical simulations, continuous time model Equation (19) is discretized with zero-order-hold assumption as:  
\[\mathbf{x}(k+1)=\mathbf{x}(k)+\int_{kT_{s}^{\mathrm{s}}}^{(k+1)T_{s}^{\mathrm{s}}}f( \mathbf{x}(k),\mathbf{u}(k),\mathbf{b}(k))\mathrm{d}t. \tag{20}\]  
where time step \(k\) relates with the real time \(t\) as \(t=kT_{s}^{\mathrm{s}}\) with control sampling time \(T_{s}^{\mathrm{c}}\). Note that the control sampling time \(T_{s}^{\mathrm{s}}\) is not necessarily the same with the localization sampling time \(T_{s}^{\mathrm{I}}\). At each control time step \(k\), successive linearizations are primarily implemented as:1. Shift the optimal control input sequence from the previous step, \(\mathbf{u}^{*}(i|k-1),i=0,1,\ldots,N_{\mathrm{P}}-1\) with \(N_{\mathrm{P}}\) being the discrete horizon steps, to obtain a seed control input trajectory \(\mathbf{u}^{0}(i|k)\): \[\mathbf{u}^{0}(i|k)=\mathbf{u}^{*}(i|k-1)\] for \(i=0,1,\ldots,N_{\mathrm{P}}-2\) and \[\mathbf{u}^{0}(N_{\mathrm{P}}-1|k)=\mathbf{u}^{*}(N_{\mathrm{P}}-2|k-1).\]
2. Apply \(\mathbf{u}^{0}(i|k)\) to Equation (20) and obtain a seed state trajectory \(\mathbf{x}^{0}(i|k),i=0,1,\ldots,N_{\mathrm{P}}\). The initial state is set as \(\mathbf{x}^{0}(0|k)=\mathbf{x}(k)\) with \(\mathbf{x}(k)\) being the current system state. The seed disturbance values are set as \[\mathbf{b}^{0}(i|k)=\mathbf{\bar{b}}(i|k),\quad i=0,1,\ldots,N_{\mathrm{P}}-1\] (21) where \(\mathbf{\bar{b}}(i|k)\) is constant as \([\bar{V}_{\mathrm{c}},\bar{\beta}_{\mathrm{c}}]\) for simplicity without loss of generality. The proposed algorithm is still applicable and works effectively when time-varying values for \([\bar{V}_{\mathrm{c}},\bar{\beta}_{\mathrm{c}}]\) are available. The EKF that will be introduced in the next subsection is also based on linearizations at the mean values of random disturbances. 3.