Discrete-Time Position Control for Autonomous Underwater Vehicle under Noisy Conditions

This paper deals with the discrete-time position control problem for an autonomous underwater vehicle (AUV) under noisy conditions. Due to underwater noise, the velocity measurements returned by the AUV’s on-board sensors afford low accuracy, downgrading its control quality. Additionally, most of the hydrodynamic parameters of the AUV model are uncertain, further degrading the AUV control accuracy. Based on these findings, a discrete-time control law that improves the position control for the AUV trajectory tracking is presented to reduce the impact of these two factors. The proposed control law extends the Ensemble Kalman Filter and solves the problem of the traditional Ensemble Kalman Filter that underperforms when the hydrodynamic parameters of the AUV model are uncertain. The effectiveness of the proposed discrete-time controller is tested on various simulated scenarios and the results demonstrate that the proposed controller has appealing precision for AUV position tracking under noisy conditions and hydrodynamic parameter uncertainty. The proposed controller outperforms the conventional time-delay controller in root-mean-square error by a percentage range of approximately 72.1–97.4% and requires at least 89.5% less average calculation time than the conventional model predictive control.


Introduction
Autonomous underwater vehicles (AUVs) can perform a variety of deep-sea tasks [1]. These tasks broadly involve marine engineering, military, and marine science fields [2,3]. Specifically, tasks such as the inspection of underwater pipelines, enemy target tracking, and marine environmental data collection can be performed by controlling AUVs [4,5]. However, the acquired data quality is highly dependent on the AUV position accuracy [6]. Thus, improving the positional precision of an AUV has recently become a hot research topic. There are two main challenges in achieving an accurate discrete-time position control of an AUV. The first one involves the uncertain hydrodynamic parameters, such as added mass and drag coefficients, and the second, the real-world processing and measurement noise. Due to the uncertainty of system dynamics, common noise filtering algorithms are usually ineffective, and thus the two aforementioned interacting factors impose a low positional accuracy, affecting the precision of AUV control.
Over the past years, the literature has offered quite a few control methods, addressing the hydrodynamic uncertainties of an AUV. The existing AUV control methods can be divided into two major classes: adaptive control and robust control techniques. Adaptive control techniques can automatically compensate for unexpected changes related to the order of the model, parameters, and input signals. It is worth noting that adaptive control compensates, even in the absence of the confines of uncertainties. Nevertheless, the shortcoming of adaptive control is its high energy consumption, as the uncertain hydrodynamic parameters to be adapted impose great computational complexity. Given the limited energy resources of an AUV, this deficiency poses a disadvantage for AUV applications. Despite this, adaptive control is still used with [7] a focuse on proportional integral derivative (PID) controllers for AUV. In this work, the PID control algorithm, despite being classic, is proven to be quite effective for AUV control. Based on a set of saturation functions for trajectory tracking of AUV, Guerrero et al. [8] propose a nonlinear robust PID controller to cope with system uncertainty. In [9], the authors introduce a conventional back-stepping method that applies to AUV platforms for diving control. Sliding mode controllers (SMCs) [10,11] are designed for AUV control in the presence of uncertain hydrodynamics, with their advantage being their implementation simplicity. Based on a sliding mode control, Yan et al. [12] present a robust adaptive SMC. To track the desired path, Xiang et al. [13] propose a robust fuzzy controller for AUV that considers a massive uncertainty of the dynamics. Ref. [14] describes a novel adaptive autopilot, based on L 1 adaptive control theory, that is robust to the system uncertainties. To achieve trajectory tracking control, Londhe and Patre [15] propose a robust adaptive tracking controller for AUV.
Compared with adaptive control techniques, robust control techniques require fewer computing resources. However, this type of control demands the knowledge of some of the hydrodynamic parameters, which are usually uncertain. To solve this parameter uncertainty problem, Kumar et al. successively design a continuous time-delay control law [16] and a discrete time-delay control law [17]. Ref. [18] presents an enhanced time-delay controller (TDC) for the position control of AUV. The enhanced TDC is computationally simple and robust to unmodeled dynamics and disturbances. These time-delay controllers (TDCs) are designed by employing the time-delay estimator (TDE) that is capable of estimating uncertain hydrodynamic parameters, using time-delayed state derivatives and control inputs [19,20]. The TDCs have two additional advantages, namely, saving computing resources, and having no requirements of prior information about uncertain parameters. However, it should be mentioned that all aforementioned controllers [7][8][9][10][11][12][13][14][15][16][17][18] do not consider any process and measurement noises.
Given that the Global Positioning System (GPS) navigation system does not operate underwater, AUVs estimate their position utilizing acoustic and optical devices that are usually considered to be on-board navigation sensors [21,22]. Both these sensor types feed the positional AUV controller with velocity information from which the traveled distance is derived. However, especially in the deep-sea, sensor measurements are inevitably affected by noise, influencing the quality of the measurement data and ultimately limiting the precision of the AUV controller. Moreover, the mathematical model of AUV imposes some process noise. Thus, the process and measurement noises incommode the design of a motion AUV controller. To deal with these noises, several estimation approaches have been made. Ermayanti et al. [23] present a Fuzzy Kalman Filter (FKF) to estimate the AUV position, while [24] propose an Ensemble Kalman Filter (EnKF). Compared with FKF, EnKF manages a more accurate position estimation, and additionally, the computational complexity of EnKF is less than that of FKF, saving energy for the AUV platforms evaluated here. Fan et al. [25] introduce a comprehensive scheme that combines a Kalman filtertype navigation for both the cross-track and heading control of AUV. However, current estimation methods become invalid if any uncertain hydrodynamic parameters exist. Yan et al. [26] utilize an advanced control strategy, i.e., the model predictive control (MPC), to improve the trajectory tracking stability of an AUV under external disturbances and noise. Yang et al. [27] propose a high aiming precision method for the unmanned combat air vehicle (UCAV) under the MPC framework. These MPC algorithms can bring a satisfactory control precision. However, exploiting MPC imposes a sizable computational complexity and energy consumption during the mission. In [28], the deterministic artificial intelligence (D.A.I.) is used to eliminate the motion trajectory tracking error for unmanned underwater vehicles in continuous time. However, the performance of D.A.I. is severely degraded by the discretization implementation [29].
In practice, AUV is commonly driven by a propulsion system and controlled by an onboard computer. Given that the computer only processes discrete digital signals, the goal is to design a discrete-time controller. Hence, in this work, the discrete-time position control for AUV under noisy conditions is studied and a discrete-time control law for trajectory tracking is proposed. The contributions of this paper are as follows:

1.
A control law, which is a modified algorithm based on the EnKF that first introduces the TDE into the EnKF.

2.
An AUV positional control method that solves the problem of uncertainty in the hydrodynamics parameters, where the traditional EnKF underperforms. 3.
Since the proposed solution utilizes the TDE algorithm, it does not need any information about the bounds of uncertain terms of the system hydrodynamics, except the rigid body inertia matrix parameter.

4.
Several Matlab/Simulink simulations are performed and the result demonstrates that the proposed discrete-time control law attains better position control precision, compared to the existing control laws, for trajectory tracking. The scenarios include various noise conditions, where the precision of the proposed control law in trajectory tracking is verified and the presented technique is also tested on several sampling times or ensembles. Finally, under the same noise conditions, the performance of the proposed discrete-time control law is tested against that of the conventional linear controller, TDC, and MPC controller.
The rest of this paper is organized into four parts as follows. Section 2 introduces the AUV model architecture, while in Section 3 a stability analysis of the proposed discretetime controller is presented. Additionally, the steps to implement the proposed modified EnKF algorithm are expressed. Finally, Section 4 presents the simulation results, and Section 5 concludes this work.

System Model Building
To describe the motion of the AUV in three-dimensional (3D) space, two coordinate systems are usually required ( Figure 1). The first one involves the earth-fixed frame {E}, while the other one is the body-fixed frame {B}. Specifically, in Figure 1a, the position and orientation of the AUV can be defined by the earth-fixed frame, while in Figure  1b, the body-fixed frame as a moving coordinate system describes the AUV velocity and acceleration. Since an AUV motion involves of six degrees of freedom (6-DoF), its dynamics can be written as follows [30]: Reference [30] also describes the the kinematics of AUV as: In Equation (1), M = M RB + M A , where M represents the inertia matrix, and M RB and M A denote the rigid body mass matrix and the added mass matrix, respectively. C(v) denotes the Coriolis and the centripetal matrix of the rigid body (C RB ) with added mass (C A ). Therefore, C(v) = C RB (v) + C A (v). D(v) represents the matrix, including the linear and the quadratic drag. Vector g(η) denotes the restoring force and moment. v = [u v w p q r] T , [u v w] T are the linear velocities and [ p q r] T are the angular velocities. τ represents the control input, which includes the forces and moments. In Equation ( 2), η = [x y z φ θ ψ] T . Considering frame {E}, x, y, and z describe the position of AUV for surge, sway, and heave, respectively. Likewise, φ, θ, and ψ represent the Euler angles for roll, pith and yaw, respectively. The non-singular J(η) denotes the kinematic transformation matrix from the frame {B} to frame {E}.  In this paper, the following assumptions are made, considering the study of the discretetime position control for AUV:

•
The AUV is regarded as a rigid body. Given that most AUVs are designed with pitch and roll stability, in the model, the pitch and roll motion are neglected. • The motions of surge, sway, and heave are considered and studied. Given that r(t) 0 and ψ(t) ψ c , with |ψ c | < π/2. By neglecting the pitch and roll motions, the AUV model can be simplified and degraded from a 6-DoF problem to a 4-DoF. Thus, the resulting 4-DoF of hydrodynamic and kinematic equations is used [31]:

Discrete-Time Controller Design in Trajectory Tracking
In this section, the design of the proposed discrete-time controller for AUV positional control in trajectory tracking noisy conditions is presented.
The control objective is to design a discrete-time control law, where the AUV can follow the reference trajectory as the time increases. The schematic diagram of the proposed controller is shown in Figure 2. The symbols used throughout this paper are presented in Table 1.

Linear discrete time controller Kinematics
The modified algorithm + -

Sensors
The proposed discrete-time controller  The real position at time kT m η r (k)

Dynamics Process noise
The reference position at time kT m

η(k)
The estimated position generated by proposed modified algorithm at time kT m η e (k) The estimated position error at time kT, The position error at time kT, The real velocity at time kT m/s v r (k) The reference velocity at time kT m/ŝ

v(k)
The estimated velocity generated by proposed modified algorithm at time kT For a continuous system, as a rule, there are two methods to design a discrete-time controller, either by discretizing an analog controller, or by building a discrete-time model of a continuous system and subsequently deriving the discrete-time controller. The difference between these two methods is that the first one uses the derivative of the system state in the control law. However, the system state is obtained through sensors that are affected by the noise, and thus, if the control law contains the derivative of the state of the system, it will increase the influence of the noise factor. Therefore, the second method is adopted in this work.
Hence, Equation (1) can be rewritten as follows: , and one can obtain: In Equation (10), f (t) and M A are assumed to be unknown. In other words, only the parameter M RB is known.
For the sampling time T, the control input generated from the discrete-time controller is considered to be constant during the sample interval, on account of zero-order holder (see Figure 2). Thus, the control input τ(t) remains constant τ(k) from time kT to time (k + 1)T. The relationship between the control input and the consequent velocity is presented in Figure 3. By integrating Equation (10) concerning the time over the sample interval, where with H(k) representing the entire uncertainty of the AUV hydrodynamics. According to [17], Equation (11) is considered to be an accurate discrete-time model of the continuous system of Equation (10). From Equation (11), one can write the following: The control law is given as follows: where with K P and K D being the proportional gain matrix and the derivative gain matrix, respectively. In Equation (14),Ĥ(k) denotes an estimate of H(k) by TDE. Hence,Ĥ(k) can be expressed as follows:Ĥ From Equation (11), one can write the following: Because v(k) and v(k − 1) in Equation (17) are not available, they can be replaced by the estimatev(k) andv(k − 1). Finally, by substitutingĤ(k) into Equation (14), the summative control law is obtained:

Discrete-Time Control Law 3.2. Stability Analysis
By combining Equation (11) with Equation (14), u(k) is substituted into Equation (19) and one can obtain the following: One can write from Equation (2) the following: Combining Equation (20) with Equation (21) yields the following: Let e(k) =η(k) − η(k). It is assumed that e(k) andė(k) are all bounded. One can write the following:η From Equation (22) and Equation (23) one can write the following: Equation (24) can be rewritten as follows:η (25) ε(k) is defined as follows: Equation (26) shows that ε(k) can be considered to represent the approximate error in the estimateĤ(k) by TDE.
Equation (25) can be simplified by using the forward difference approximation. The simplified equation becomes the following: If (−T Jε(k) + K P e(k) + (K D + I)ė(k)) is bounded, then the Bounded Input Bounded Output (BIBO) stability of the presented control law can be proved. Here, K P e(k) + (K D + I)ė(k) is bounded and the boundedness of ε(k) will be shown as follows.
From Equations (11), (14) and (26) one can write the following: let H 1 (k) = kT+T kT f (ξ)dξ and one can obtain: By using Equation (11) and Equation (29) one can write the following: From Equation (28) one can write the following: Substituting τ(k) into the above equation, we get the following: SubstitutingĤ(k) into Equation (32), we get the following: Using Equation (30) one can write the following: Substituting Equation (34) into Equation (33), we get the following: From Equation (28), one can obtain the following: Substituting Equation (36) into Equation (35), we get the following: where Here, B 1 and B 1 are considered to be the bounded disturbance. According to [17], it is already proven that the eigenvalues of A lie within the unit disc. Therefore, ε(k) is bounded and the BIBO stability of the presented control law is proved.

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 M RB parameter to be known. This is important, as the remaining parameters, such as C(v), D(v), and g(η), 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][34].
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: where H(k) contains all the uncertain terms. The process noise is not contained in Equation (41), and therefore, the uncorrelated process noise ς(k) and the measurement noise ζ(k) are added into the Equation (41): The observation equation is as follows: where H o is the observation operator. The ς(k) and ζ(k) are random Gaussian noise with mean 0. Their covariance matrix is Q and R, respectively. The modified algorithm based on the EnKF [24] becomes the following: 1.

2.
By using the state space form, the N-ensemble for variable state is presented as where i counts from 1 to N, N is the number of variable state ensemble members, and ς i (k) ∼ N(0, Q(k)) (i = 1, . . . , N) is the ensemble of the process noise. The uncertain terms H(k) can be eliminated by using TDE. When T is set small enough close to zero, , which denotes the mean ofv − i (k + 1). Then, the error covariance is obtained as follows:

3.
The N-ensemble for measurement data is presented as follows: where ζ i (k) ∼ N(0, R(k)) (i = 1, . . . , N) is the measurement noise ensemble. The Kalman gain is set as follows: and thus the estimation is expressed as follows: The mean of the estimation is the following:

4.
The estimated velocityv(k) can be transformed into the estimated positionη(k) by using the kinematics from Equation (21). Finally, the control input τ(k) is obtained by substitutingv(k) andη(k) .

Simulation
All simulations are performed in Matlab, adopting the fourth-order Runge-Kutta approach. The fixed-step size is set to 0.01 s and the control schemes are simulated for 100 s. In these simulations, some typical sinusoidal, ramp, and step input commands are used to verify the effectiveness of the designed discrete-time controller. In this paper, the hydrodynamic parameters of [18] are used (see Table 2). For simplicity, H o is set to the identity matrix and ψ c = 0. The reference positions are as follows: (48) and the start location of the AUV is at the origin of the coordinate system, imposed with a position error in the beginning. Then, in Equation (48) the sinusoidal, ramp, and step commands are provided for the controller. The control variables K p = diag{19, 8, 3} and K d = diag{37, 15, 4} are heuristically defined. First, K p is chosen, taking into account the reference position. Based on the results of the simulations, K p is adjusted to improve the system behavior. Second, fixing K p , K d is adjusted to improve the system behavior based on the results of the simulations.

The Proposed Controller under Different Noise Conditions
The proposed controller in trajectory tracking scenarios under three different noise conditions is tested. For each trial, the noise is considered to be independent in all directions of motion. The mean of random Gaussian noises ς(k) and ζ(k) are set as zero. For simplicity, the covariance matrices of noise are set to constant matrices. So, For practical reasons, all elements of the covariance matrices are assumed to be less than or equal to 1. For the sake of the trials, three cases are randomly generated as follows:

The Proposed Controller Utilizing Different Sampling Times
It is well known that different sampling times have a great impact on the performance of position tracking. For this trial, N is set to 100 and case 1 is chosen as the noise condition. Then, the proposed controller is tested for different sampling times T, i.e., 0.01 s, 0.02 s, 0.05 s, and 0.1 s. Figure 7 presents the 3D position tracking for the corresponding sampling time T. The latter figure highlights that the position tracking degrades as the sampling time increases. This happens not only because of the discrete-time controller, but also because of TDE. Accordingly, when the sampling time increases,Ĥ(k) is not approximately equal to H(k).

The Proposed Controller Utilizing Different Ensembles
In this trial, T is set to 0.01s and the proposed method is tested for ensembles equal to 50, 100, and 200. Figure 8 presents the related results and demonstrates that in all cases, the AUV tracks the reference positions effectively. Additionally, this trial highlights the position tracking precision of the proposed technique in terms of root-mean-square (RMS) errors of the AUV position at a steady state. The corresponding results over the three noise cases examined and for all ensembles are presented in Table 3. From the latter table, it can be concluded that there is no mathematical relationship between the RMS errors and ensemble size, and that a larger ensemble size increases the computational requirements. Table 4 shows the computational time under different noise conditions for ensembles equal to 50, 100, and 200. The computational time increases with the ensemble size. Thus, choosing a small ensemble size affords saving the AUV's energy that, in any case, is limited.

Evaluating Current Controllers under Various Noise Conditions
The proposed controller is challenged for the noise cases 1-3 of Section 4.1, against the conventional linear controller, TDC, and MPC controller. For the proposed controller, let T = 0.01 s and N = 100.
First, for a fair comparison, the control parameters of linear controller and TDC are the same as the proposed controller. Figure 9 presents the position tracking results and tracking errors, where it is evident that the linear controller in the presence of noise and uncertain hydrodynamics parameters does not pose an appealing positional accuracy. Regarding TDC, its tracking accuracy is mediocre. Indeed, the measurement velocity information provided for TDC presents a low accuracy, affecting, accordingly, the positional estimation. Compared to these traditional controllers, the proposed controller substantially reduces the positional tracking errors. Table 5 illustrates the RMS errors for each competitor controller for the noise cases 1-3. From the latter table, it is observed that the proposed controller affords the smallest positional RMS error , compared to the linear controller and TDC. In particular, the linear controller performs poorly, while the proposed controller outperforms the conventional TDC in RMS error by a percentage range of approximately 72.1-97.4%.  Then, the proposed controller is compared against an advanced control strategy, i.e., MPC. Running a MPC solver in MATLAB is a huge computational burden for computer, and thus MPC is simulated only for 80 s. Given that the proposed MPC for AUV for trajectory tracking in [26] applies to this work, this method is compared against the proposed one. Table 6 presents the MPC parameters. The type of MPC algorithm is State Feedback Predictive Control (SFPC), and the cost function is defined as follows: and Y d (k) denote the predicted position and the desired position, respectively. U(k) is the control input increment. Figure 10 presents the 3D position tracking results under various noise conditions, and Table 7 illustrates the RMS errors at a steady state for the MPC controller and the proposed controller. It indicates that the MPC controller and the proposed controller both precisely track the truth positions. Given that the suggested method meets the requirement of high tracking accuracy, the performance of the calculation time is examined, in particular. Table 8 presents the average calculation time of the MPC controller and that of the proposed controller in a single step for noise cases 1-3, highlighting that MPC imposes the highest computational burden, prohibiting it from real-time applications. The proposed controller manages at least 89.5% less average processing time over the conventional MPC controller. Hence, in practical applications, the proposed controller can offer high timeliness.   Finally, it is worth noting that the proposed controller is tested only against opensource controllers, namely the conventional linear controller, TDC, and MPC controller, because re-implementing current controllers might lead to a non-optimized solution that inevitably would underestimate the capabilities of the original method.

Conclusions
In this paper, a discrete-time position controller that is suitable for AUVs is developed. The proposed method considers noise and uncertain hydrodynamic parameters and, additionally, all the system parameters to be unknown, except from the rigid body inertia matrix, which is the norm for practical applications. The suggested technique modifies the Ensemble Kalman Filter accordingly to afford accurate positional estimation of an AUV under noisy conditions and major system uncertainties. The contribution of this paper is essential, as, compared to the traditional Ensemble Kalman Filter algorithm, the proposed one is more robust and effective for AUVs.
The effectiveness of the proposed modified Ensemble Kalman Filter method on quite a few simulated scenarios involving various nuisances and parameter setups is demonstrated. Indeed, the results of simulations highlight that the proposed discrete-time controller achieves the precise positional control for an AUV in the presence of both noise and uncertain hydrodynamics. By comparing the root-mean-square errors of the proposed controller against the conventional linear controller and time-delay controller, it can be found that the proposed discrete-time controller affords the most accurate positional tracking. In particular, the proposed controller outperforms the conventional time-delay controller in root-mean-square error by a percentage range of approximately 72.1-97.4%. Moreover, compared to the model predictive control, the proposed controller takes at least 89.5% less average calculation time. Thus, the proposed controller has the advantages of high tracking accuracy and high timeliness. Future research shall investigate the effectiveness of the proposed controller under rapidly changing system disturbances.