Adaptive Robust Unscented Kalman Filter for AUV Acoustic Navigation

Autonomous underwater vehicle (AUV) acoustic navigation is challenged by unknown system noise and gross errors in the acoustic observations caused by the complex marine environment. Since the classical unscented Kalman filter (UKF) algorithm cannot control the dynamic model biases and resist the influence of gross errors, an adaptive robust UKF based on the Sage-Husa filter and the robust estimation technique is proposed for AUV acoustic navigation. The proposed algorithm compensates the system noise by adopting the Sage-Husa noise estimation technique in an online manner under the condition that the system noise matrices are kept as positive or semi positive. In order to control the influence of gross errors in the acoustic observations, the equivalent gain matrix is constructed to improve the robustness of the adaptive UKF for AUV acoustic navigation based on Huber’s equivalent weight function. The effectiveness of the algorithm is verified by the simulated long baseline positioning experiment of the AUV, as well as the real marine experimental data of the ultrashort baseline positioning of an underwater towed body. The results demonstrate that the adaptive UKF can estimate the system noise through the time-varying noise estimator and avoid the problem of negative definite of the system noise variance matrix. The proposed adaptive robust UKF based on the Sage-Husa filter can further reduce the influence of gross errors while adjusting the system noise, and significantly improve the accuracy and stability of AUV acoustic navigation.


Introduction
With the development of underwater acoustic communication, energy, control, and navigation technologies, autonomous underwater vehicles (AUVs) have been widely used in submarine detection, ocean environmental monitoring, deep-sea facility laying and monitoring, marine mining, and other applications [1,2]. The autonomous navigation systems for AUVs include acoustic navigation, gravity-magnetic matching navigation, and integrated navigation systems (INS) [3,4]. Compared with gravity-magnetic matching navigation and INS, acoustic navigation does not need to store accurate underwater gravity-magnetic maps in advance, and has a smaller size and lighter weight. Although acoustic navigation technology is challenging due to high costs, difficulty in construction of seabed beacon, and complexity of calibration for obtaining optimal positioning accuracy, it is widely used in AUV navigation as an independent navigation system or as a key part of integrated navigation [5].
For AUV real-time navigation, different filter methods have been developed, including the extended Kalman filter (EKF), unscented Kalman filter (UKF), and particle filter (PF) [6,7]. The EKF uses the first-order Taylor expansion to calculate the Jacobian matrix, which not only increases the complexity of the mathematical computation, but also introduces linearization errors. The PF has good z k = H(x k ) + ∆ k (2) where x k , x k−1 are the state vectors of the AUV at times k, k − 1 , respectively; ϕ(x k−1 ) is the nonlinear state transition matrix; ω k−1 is the system noise; Γ(ω k−1 ) is the system noise distribution matrix; z k is the acoustic observation vector at the time of k; H(x k ) is the nonlinear observation matrix; and ∆ k is the measurement noise. The system noise and the measurement noise with covariance matrices Q k , R k are usually assumed to be uncorrelated Gaussian white noise: where δ k j is Kronecker delta function. The basic principle of the UKF is to take sigma points according to the UT in the original state distribution. Therefore, the mean and covariance of these points are equal to those of the original state distribution. Then, these points are substituted into the nonlinear function to get the transformed mean and covariance [28,29]. The standard UKF implementation for state estimation is given as follows.
Step 1: We initialize the state parameter and the corresponding covariance.
where x 0 ,x 0 denote the initial and mean state information, including the x coordinate, y coordinate, speed, turning radius, and turning angle of AUV, while P 0 stands for the initial covariance matrix.
Step 2: The parameters are then updated. The sigma points and weights are calculated as follows: where W i denote the weighted values of the mean and covariance, respectively; x ∼ N(x, P x ), x, P x denote the mean and covariance of coordinates, respectively; λ is the proportional parameter; and χ i,k is the weighted point that approximates the random variable distribution.
We calculate the prediction sigma points, the state value, and the covariance matrix as where Q k denotes the state noise covariance matrix.
Step 4: We calculate the variance and covariance matrix.
where R k denotes the measurement noise covariance matrix Step 5: The gain of the Kalman filter is calculated.
Step 6: We update the state vector and the covariance matrix in the following.

Adaptive UKF Based on the Sage-Husa Filter
When the system noise is unknown, the classical UKF cannot adjust the system noise. We focus on the above problem to study the adaptive UKF algorithm based on the Sage-Husa filter. The Sage-Husa filter adaptively estimates the system and measurement noise through the time-varying noise estimator, including the mean of the system noise vectorq k , the system noise variance matrixQ k , the mean of the measurement noise vectorr k , and the measurement noise variance matrixR k [30,31]. The estimators of the Sage-Husa filter can be expressed as [32]: where , and b is the forgetting factor with the range of 0.95 < b < 0.99 [33].
According to Equations (18) and (20), the Sage-Husa filter based on the time-varying noise estimation can theoretically obtain both the system and measurement noise matrices. However, the deviation of residual vector v k affects the calculation ofQ k ,R k simultaneously, easily leading to filter divergence. Hence, Equations (18) and (20) cannot be used simultaneously. Therefore, the adaptive Sensors 2020, 20, 60 5 of 16 UKF algorithm studied in this paper only adjusts the system noise using Equations (17) and (18). The covariance matrix in Equation (10) is rewritten as In addition, Equation (18) cannot guarantee the system noise variance matrixQ k or the semi positive or positive definite values, which may give it a negative definite value. The eigenvalue eig of the system noise variance matrixQ k is calculated. If the eig ≥ 0, theQ k is calculated by Equation (18), otherwiseQ k is calculated by Equation (22).

Adaptive Robust UKF Algorithm
The standard Kalman filter requires the system and measurement noise to be zero-mean white noise. Owing to the complexity of the marine environment, gross errors inevitably exist in the underwater acoustic observation [34]. Therefore, robust estimation is usually used to reduce the influence of gross errors on positioning results.
In underwater acoustic positioning, when the observation contains gross error, the measurement equation should be where G k is the interference matrix of the gross error, and ε k denotes the gross error.
If the standard UKF is still used for calculation, the prediction residual with gross error is as follows: where v k , v k are the prediction residual vectors without gross error and with gross error, respectively. The gross error is fully reflected in the prediction residual, so the state vector iŝ The gross error in the observation affects the state vectorx k through the gain matrix K k . Therefore, the influence of the gross error on the state estimate is significantly reduced or even eliminated by using the prediction residual to constrain the gain matrix. Based on Huber's constructed equivalent weight function [21], the equivalent gain matrix is constructed as: where S k is the standardized residual corresponding to the kth measurement or standardized correction for the kth predicted state element. Here, c is a constant, usually chosen as 1.0-2.0, and σ 0 is the variance scale factor. The constant 1/0.6745 is a correction factor for Fisher consistency in the Gaussian distribution [35]. When gross errors exist in observation, the adaptive UKF is unable to resist the effect of gross errors. In order to solve the problem, an adaptive robust UKF based on the Sage-Husa filter and Sensors 2020, 20, 60 6 of 16 robust estimation is proposed. Since the system noise variance matrixQ k and the measurement noise variance matrixR k cannot be estimated simultaneously, the proposed adaptive robust UKF applies the Sage-Husa filter to adaptively adjust the system noise variance matrix. At the same time, the equivalent gain matrix based on the Huber function is constructed to eliminate or reduce the effects of gross errors.
The flowchart of the proposed adaptive robust UKF is shown in Figure 1. The detailed steps of the algorithm are explained as follows: (1) The initial position information, speed, turning radius, and turning anglex 0 of the AUV are initialized and the sigma points χ k are generated.
(2) The state is predicted by the UT. Then, the state vectorx − k and the covariance matrix P − x,k are estimated by Equations (9) and (10).
(3) The measurement prediction vectorẑ − k , the variance matrix P z,k , and the covariance matrix P xz,k are calculated by Equations (11)- (13).
(4) The gain of the Kalman filter K k is computed by Equation (14). Based on the Kalman filter, the equivalent gain matrix K K is calculated to achieve robust estimation by Equation (28), while the state vectorx k is calculated by Equation (31).
(5) The mean of the system noise vectorq k and the system noise variance matrixQ k are calculated by Equations (17) and (18). (6) The eigenvalues eig of the system noise variance matrixQ k is finally calculated. If the eig ≥ 0, Q k is calculated by Equation (18); otherwise,Q k is calculated by Equation (22).

Experiments and Analysis
To verify the adaptive UKF, the simulated long baseline positioning experiment of the AUV and the real marine experimental data of the ultrashort baseline positioning of an underwater towed body are carried out. Then, the adaptive robust UKF is analyzed and compared in the different situations through the simulation experiment.

Simulation Experiment and Analysis
The simulation experiment is conducted for AUV positioning. The AUV motion trajectory and

Experiments and Analysis
To verify the adaptive UKF, the simulated long baseline positioning experiment of the AUV and the real marine experimental data of the ultrashort baseline positioning of an underwater towed body are carried out. Then, the adaptive robust UKF is analyzed and compared in the different situations through the simulation experiment.

Simulation Experiment and Analysis
The simulation experiment is conducted for AUV positioning. The AUV motion trajectory and the distribution of the seabed transponders are shown in Figure 2. The AUV receives the acoustic signal from the simulated seabed transponders in red, which are centered radially on the seafloor. The blue line represents the uniform turning motion of the AUV at a depth of 100 m, and the motion parameters include the x coordinate, y coordinate, z coordinate, speed, turning radius, and turning angle. The initial position of the AUV is (100 m, 0 m, −100 m), the turning radius is chosen as 100 m, and the turning angle is π/100 rad. A total of 100 epochs are simulated with a sampling interval of 1 s, and the Munk velocity [36] profile and ray tracing algorithm are used to generate the observations. Because the depth of the simulated AUV is the same, the relative depths of the AUV and underwater transponder are used as the known value for calculation. Therefore, the inside of the state vector x k is the x coordinate, y coordinate, turning angle, speed, and turning radius, respectively The original observations include acoustic travel time between the transducer equipped with the AUV and the seabed transponders, as well as the sound speed profile. Nowadays, the positioning requirement of the AUV in the sea is at the meter level. The high-precision positioning accuracy of AUV using a multi-sensor can reach the decimeter level, completely meeting the requirements of the fields of submarine detection and ocean environmental monitoring. According to the requirements of positioning accuracy, the systematic error and random error are added to the simulated observations, and the slant range error is 0.1 m. The simulated system noise variance matrix is Q = Q 0 × I 5×5 , with Q 0 = 0.01 m 2 . Here, I 5×5 is a unit matrix of five rows and five columns. The systematic error formula proposed by Xu et al. is used and can be expressed as [37]: where the constant term is c 1 = 0.1 m, the short-period internal wave error term is c 2 = 0.12 m, the long-period error term is c 3 = 0.3 m, the term related to the measurement range is c 4 = 0.02 m, the short period of the internal wave is T s = 15 × 60 s (15 min) and the long period of internal wave is T L = 12 × 3600 s (12 h), ||x − x || and is the distance between the transducer and the seabed transponder.
where the constant term is = 0.1 m, the short-period internal wave error term is = 0.12 m, the long-period error term is = 0.3 m, the term related to the measurement range is = 0.02 m, the short period of the internal wave is = 15× 60 s (15 min) and the long period of internal wave is = 12× 3600 s (12 h), || − || and is the distance between the transducer and the seabed transponder. The performance of the UKF and the adaptive UKF algorithm are compared under the condition that there is no gross error except the random error and the systematic error, and the wrong system noise matrix is simulated as The results are shown in Figures 3 and 4 and Table 1. It can be seen that when the system noise deviates significantly from the true value, for example, amplifying 100 times, the adaptive UKF can adaptively adjust and estimate the system noise, providing higher accuracy positioning results than those of UKF. When the system noise is close to the true value, there is no obvious difference in positioning accuracy between the adaptive UKF and the UKF algorithm.        In order to further verify the adaptive UKF, the ultrashort baseline data of the towed body is tested and analyzed for underwater positioning. The towed body refers to the marine integrated survey instrument, including the multibeam sounder, the sub-bottom profiler, and others. The towed body is regarded as the AUV for the real data experiment.

. Real Experiment and Analysis
In order to further verify the adaptive UKF, the ultrashort baseline data of the towed body is tested and analyzed for underwater positioning. The towed body refers to the marine integrated survey instrument, including the multibeam sounder, the sub-bottom profiler, and others. The towed body is regarded as the AUV for the real data experiment. The experiment was conducted in the South China Sea in April 2017. The experimental ship is equipped with a global acoustic positioning system (GAPS) ultrashort baseline unit, a differential global positioning system (DGPS), a gyrocompass, a sound velocity profiler (SVP), and an INS. The towed body equipped with the transponder is mounted behind the experimental ship. The position information of the ship can be obtained by the shipboard DGPS and the position of the ship-bottom transducer array can be obtained through coordinate transformation [38]. Data such as the attitude, heading, and speed of the ship are obtained through the gyrocompass and INS. The position of the underwater towed body can be gained in real time through the GAPS ultrashort baseline acoustic positioning system. The mean sound speed measured by the SVP is 1530 m/s. The sampling interval of the INS is 0.01 s and the sampling interval of the GAPS is 1 s. The complete motion trajectory of the experiment is shown in Figure 5, with the red line indicating the position of the transducer and the blue one indicating the position of the transponder. The positioning accuracy is evaluated by the position difference of the abovementioned algorithms and the output value of the instrument, which can be used as references. As shown in Figures 6 and 7, when the system noise is set as = 100 m 2 or = 10 m 2 , the accuracy of the UKF obviously decreases in the horizontal direction, the adaptive UKF can adjust the system noise online, and then improve the positioning results. When the system noise is set as = 0.01 m 2 or = 0.1 m 2 , the positioning accuracy of the adaptive UKF is comparable to that of the UKF in the y direction, and obviously higher than that of the UKF in the x direction. Since the exact system noise is hard to know, we conduct the experiments of the adaptive UKF and the UKF under As shown in Figures 6 and 7, when the system noise is set as Q 0 = 100 m 2 or Q 0 = 10 m 2 , the accuracy of the UKF obviously decreases in the horizontal direction, the adaptive UKF can adjust the system noise online, and then improve the positioning results. When the system noise is set as Q 0 = 0.01 m 2 or Q 0 = 0.1 m 2 , the positioning accuracy of the adaptive UKF is comparable to that of the UKF in the y direction, and obviously higher than that of the UKF in the x direction. Since the exact system noise is hard to know, we conduct the experiments of the adaptive UKF and the UKF under different system noise levels from 0.001 m 2 to 1000 m 2 , which are shown in Figure 8 and Table 2. From Figure 8 and Table 2, it can be seen that the accuracy of the UKF changes significantly with different system noise values, while the adaptive UKF can obtain more stable and accurate results with minor influence due to changes in the system noise.  The Value of (m 2 ) Method RMS-x (m) RMS-y (m)

Validation of Adaptive Robust UKF
In this section, the adaptive robust UKF is validated by the simulation experiment of AUV positioning, because the measured data from the ultrashort baseline positioning has only one observation at each observation time, and the validation of the robust algorithm cannot be completed without redundant observations. The experimental trajectory and error simulation of the simulation experiment are the same as in Section 4.1.1. The only difference is that gross errors are simulated based on normal distribution with zero mean and standard deviations of 0.005 s, and added in the acoustic observations every 20 s. At the same time, RMS is used to evaluate the performance of the UKF, the robust UKF, and the adaptive robust UKF.

Validation of Adaptive Robust UKF
In this section, the adaptive robust UKF is validated by the simulation experiment of AUV positioning, because the measured data from the ultrashort baseline positioning has only one observation at each observation time, and the validation of the robust algorithm cannot be completed without redundant observations. The experimental trajectory and error simulation of the simulation experiment are the same as in Section 4.1.1. The only difference is that gross errors are simulated based on normal distribution with zero mean and standard deviations of 0.005 s, and added in the acoustic observations every 20 s. At the same time, RMS is used to evaluate the performance of the UKF, the robust UKF, and the adaptive robust UKF.
First, the true system noise is added in the state equation and the measurement errors, including random error, systematic error, and gross error, are added in the acoustic observations. The performances of the UKF and the robust UKF are compared, which are shown in Figure 9 and Table 3. It can be seen that when there are gross errors in the acoustic observations, the UKF can be affected by the gross errors in the observations to obtain unreliable calculation results. The robust UKF can detect gross errors by predicting residuals and can achieve robust estimation by the equivalent gain matrix. Table 3 shows that the mean RMS values of the x direction and y direction robust UKF are about 0.103 m and 0.135 m, respectively, which are much smaller than those of the UKF (0.264 m and 0.651 m, respectively). The same conclusions can be drawn for the maximum and minimum RMS values for the robust UKF and the UKF. Therefore, the robust UKF can efficiently control the influence of gross errors and considerably improve the positioning accuracy.
Sensors 2020, 20, x FOR PEER REVIEW 13 of 17 First, the true system noise is added in the state equation and the measurement errors, including random error, systematic error, and gross error, are added in the acoustic observations. The performances of the UKF and the robust UKF are compared, which are shown in Figure 9 and Table 3. It can be seen that when there are gross errors in the acoustic observations, the UKF can be affected by the gross errors in the observations to obtain unreliable calculation results. The robust UKF can detect gross errors by predicting residuals and can achieve robust estimation by the equivalent gain matrix. Table 3 shows that the mean RMS values of the x direction and y direction robust UKF are about 0.103 m and 0.135 m, respectively, which are much smaller than those of the UKF (0.264 m and 0.651 m, respectively). The same conclusions can be drawn for the maximum and minimum RMS values for the robust UKF and the UKF. Therefore, the robust UKF can efficiently control the influence of gross errors and considerably improve the positioning accuracy.  Then, the performance of the UKF, the adaptive UKF, the robust UKF, and the robust adaptive UKF algorithm are compared under the condition that measurement errors exist, including random error, systematic error, and gross error; and the wrong system noise matrix is simulated as = × × , = 1 m 2 or = 0.1 m 2 . The results are shown in Figures 10 and 11 and Table 4. It can be seen that: (1) the UKF has no ability to resist the influence of gross error and imprecise system noise, both of which reduce its positioning accuracy greatly; (2) the robust UKF can effectively control  Then, the performance of the UKF, the adaptive UKF, the robust UKF, and the robust adaptive UKF algorithm are compared under the condition that measurement errors exist, including random error, systematic error, and gross error; and the wrong system noise matrix is simulated as Q = Q 0 × I 5×5 , Q 0 = 1 m 2 or Q 0 = 0.1 m 2 . The results are shown in Figures 10 and 11 and Table 4. It can be seen that: (1) the UKF has no ability to resist the influence of gross error and imprecise system noise, both of which reduce its positioning accuracy greatly; (2) the robust UKF can effectively control the influence of gross error by using robust estimation, but the system noise must be known precisely; (3) the adaptive UKF can adaptively adjust and estimate the system noise based on the Sage-Husa filter, but it cannot resist the influence of the gross error; (4) the adaptive robust UKF can estimate system noise using the Sage-Husa filter and achieve robust estimation with the equivalent gain matrix, which is why it can deal with both the problems of unknown noise statistics and gross error, and performs as the best algorithm in terms of positioning accuracy and reliability.

Conclusions
Because of the complex marine environment, the it is difficult to accurately obtain the system noise, and the acoustic observations usually have gross errors, which influence the positioning accuracy of the classical UKF. Based on the Sage-Husa filter, an adaptive robust UKF algorithm is proposed in this paper. The following conclusions can be drawn from our analysis, computation, and comparison.
(1) He unknown or imprecise system noise can cause the divergence of the classical UKF, so this paper proposes an adaptive UKF based on the Sage-Husa filter. No matter how much the initial system noise is given, the adaptive UKF can accurately estimate the system noise through the time-varying noise estimator. At the same time, the adaptive UKF can avoid the problem of a negative definite value of the system noise variance matrix based on the eigenvalues judgement.
(2) When there are gross errors in the observation, the adaptive UKF has no ability to resist the effect of gross errors. To solve the problem, this study presents an adaptive robust UKF based on the Sage-Husa filter and the equivalent gain matrix. The adaptive robust UKF can adaptively adjust the system noise to solve the divergence problem and control the effects of gross measurement errors on dynamic state estimates. Compared with the classical UKF and adaptive UKF, the accuracy of the adaptive robust UKF is significantly improved. Therefore, the proposed adaptive robust UKF can provide an effective method for AUV acoustic navigation and positioning.
In actual marine navigation and positioning, if abnormal system noise and measurement noise occur simultaneously, the proposed adaptive robust UKF may provide unsatisfactory results or even lead to filter divergence in this rare situation. Therefore, future research should focus on how to adjust the system noise and the measurement noise at the same time.