# Discrete-Time Position Control for Autonomous Underwater Vehicle under Noisy Conditions
## 3 Discrete-Time Controller Design in Trajectory Tracking
### The Modified Algorithm Based on EnKF  
This subsection introduces the proposed algorithm, relying on a modified EnKF, which involves a TDE scheme canceling the uncertain terms of the state space form. It should be noted that the proposed strategy combining the EnKF with TDE offers overall robustness against large noise levels and great system parameter uncertainties. TDE is used, as it requires only the \(\mathbf{M}_{\mathbf{R}\mathbf{B}}\) parameter to be known. This is important, as the remaining parameters, such as \(\mathbf{C}(\mathbf{v})\), \(\mathbf{D}(\mathbf{v})\), and \(\mathbf{g}(\mathbf{\eta})\), are usually unknown in practical applications; thus, exploiting TDE the proposed method affords broadening the uncertainty bounds of the involved parameters. The motivations for using the EnKF over the Extended Kalman Filter (EKF) counterpart are that the EnKF is a Kalman Filter variant appropriate for large scale-problems as the one investigated in this paper, and is more accurate and efficient than EKF, even for relatively low ensembles, i.e., N \(>\) 50 [32; 33]. According to the separation principle, the optimal control and state estimation problems can be decoupled in some situations. Given that the process and the observation noise are Gaussian, the optimal solution can be separated into a Kalman Filter and a linear quadratic regulator, known as linear-quadratic Gaussian control. In this work, EnKF is a linear variant of the Kalman Filter, and thus, TDE can be considered to be a linear estimation (or optimal control) [35]. Therefore, the separation principle is still available for the designed controller. Hence, for the proposed controller, the state space form becomes the following:  
\[\mathbf{v}(k)= \mathbf{v}(k-1)+\mathbf{M}_{\mathbf{RB}}^{-1}(\hat{\mathbf{H}}(k-1)-\mathbf{H}(k-1))-\hat {\mathbf{v}}(k-1)\] \[+\mathbf{J}^{-1}(\hat{\mathbf{\eta}}_{r}(k)+\mathbf{K}_{P}\hat{\mathbf{\eta}}^{e}( k-1)+\mathbf{K}_{D}\hat{\mathbf{\eta}}^{e}(k-1)) \tag{41}\]  
where \(\mathbf{H}(k)\) contains all the uncertain terms. The process noise is not contained in Equation (41), and therefore, the uncorrelated process noise \(\mathbf{\varsigma}(k)\) and the measurement noise \(\mathbf{\zeta}(k)\) are added into the Equation (41):  
\[\mathbf{v}(k)= \mathbf{v}(k-1)+\mathbf{M}_{\mathbf{RB}}^{-1}(\hat{\mathbf{H}}(k-1)-\mathbf{H}(k-1))- \hat{\mathbf{v}}(k-1)\] \[+\mathbf{J}^{-1}(\hat{\mathbf{\eta}}_{r}(k)+\mathbf{K}_{P}\hat{\mathbf{\eta}}^{e}( k-1)+\mathbf{K}_{D}\hat{\mathbf{\eta}}^{e}(k-1))\] \[+\mathbf{\varsigma}(k-1) \tag{42}\]  
The observation equation is as follows:  
\[\mathbf{Z}(k)=\mathbf{H}_{\mathbf{o}}\mathbf{v}(k)+\mathbf{\zeta}(k) \tag{43}\]  
where \(\mathbf{H}_{\mathbf{o}}\) is the observation operator. The \(\mathbf{\varsigma}(k)\) and \(\mathbf{\zeta}(k)\) are random Gaussian noise with mean 0. Their covariance matrix is \(\mathbf{Q}\) and \(\mathbf{R}\), respectively. The modified algorithm based on the EnKF [24] becomes the following:  
1. Setting the initial estimated velocity \(\hat{\mathbf{v}}(0)=0\). 2. By using the state space form, the N-ensemble for variable state is presented as follows: \[\hat{\mathbf{v}}_{i}^{-}(\hat{\mathbf{\varsigma}})= \hat{\mathbf{\upsilon}}(k-1)+\left(\mathbf{M}_{\mathbf{RB}}^{-1}(\hat{\mathbf{H}} (k-1)-\mathbf{H}(k-1))\right)\] \[+\mathbf{J}^{-1}(\hat{\mathbf{\eta}}_{r}(k)+\mathbf{K}_{P}\hat{\mathbf{\eta}}^{e} (k-1)+\mathbf{K}_{D}\hat{\mathbf{\eta}}^{e}(k-1))\] \[+\mathbf{\varsigma}_{i}(k-1)\] (44) where i counts from 1 to N, N is the number of variable state ensemble members, and \(\mathbf{\varsigma}_{i}(k)\sim\mathbf{N}(0,\mathbf{Q}(k))\)\((i=1,\ldots,N)\) is the ensemble of the process noise. The uncertain terms \(\mathbf{H}(k)\) can be eliminated by using TDE. When \(T\) is set small enough close to zero, \(\hat{\mathbf{H}}(k-1)-\mathbf{H}(k-1)\approx 0\).