### 1. Introduction

### 2. Underwater Navigation Aided with USBL

### 2.1 Position Measurements of USBL in the Horizontal Plane

*ψ*and the slant angle

_{bearing}*θ*of the two directions that are orthogonal to each other as well as the time delay of the transmitted-received signal

_{slant}*δT*, as shown in Fig. 2. If the sound velocity

*c*is known, the relative three-dimensional position in underwater can be calculated with one range

*R*(=

*cδT*/2) and two angles

*ψ*and

_{bearing}*θ*. When the water depth is greater than the horizontal distance, the horizontal position measurement error can be assumed to have independent normal distributions in the

_{slant}*x*- and

*y*-directions. In this condition, the USBL system has been successfully applied to aiding a navigation system as an auxiliary sensor in a previous study (Lee et al., 2017). If an AUV is performing a long-range exploration in an ice sheet or operating in a shallow water, the error in the radial direction will be constant regardless of the range as well. However, the error in the angular direction that is perpendicular to the radial direction will increase with range. Therefore, an error modeling approach of the USBL able to handle this feature should be developed for aiding underwater navigation in shallow water.

*σ*

_{1}, 2

*σ*

_{1}, 3

*σ*

_{1}) of the standard deviation

*σ*

_{1}of the first principal component (PC1) for each position dataset. Because the error distribution of the USBL measurement has nonlinear characteristics, underwater vehicles operating in shallow water require an error modeling to consider the correlation between the

*x-*and

*y*-axis directions.

### 2.2 MD and PCA of the USBL Measurements

*μ*and covariance

*Σ*, the probability density function of the k-dimensional multivariate vector

**= (**

*X**X*

_{1},…,

*X*)

_{k}*is expressed as follows: where*

^{T}*Σ*is the covariance matrix

*can be calculated as*

_{i}*σ*

_{1}and

*σ*

_{2}represent the standard deviations of the principal components, and

*ρ*

_{12}represents the correlation coefficient between

*x*

_{1}and

*x*

_{2}. If the standard deviations in the

*x*- and

*y*-axis directions are equal, the MD is equal to the ED (MD

*= ED*

_{i}*).*

_{i}**(**

*X**n*×

*p*) that includes

*n*measurements (

*x*) composed of

_{i}*p*variables.

*X**is determined by removing the mean*

_{c}*x̄*from each column vector

_{j}

*x**(*

_{j}*n*×1) in the data matrix

**(**

*X**n*×

*p*). In this sense, if singular value decomposition is performed,

*X**can be expressed as where*

_{c}**is a diagonal matrix of singular values**

*Λ**λ*for

_{i}*i*= 1,…,

*a*which is interpreted as the total variance for each principal component (PC) ordered as

*λ*

_{1}>

*λ*

_{2}>… >

*λ*.

_{a}*U*is a column vector indicating the normalized value for each PC axis, and

**is a unit matrix indicating the direction of the PC axis.**

*V**p*= 2,

*a*= 2, and the USBL dataset, i.e., (

*x*,

*y*), can be handled using the PCA. The average of the position dataset is calculated and eliminated, and the PCA can be performed on the matrix

*X*as follows, by using the singular value decomposition of the MATLAB function

_{c}*svd()*.

*S*represents the diagonal matrix of the singular values

*Λ*. The right column of Fig. 4 indicates that the normalized MD data of the USBL position error of points ①–④ have a 2-D symmetric normal distribution. As shown in Table 1, as the range increases, the first singular value of the PC (PC1) increases proportionally (0.99, 1.75, 5.55, and 8.55). However, the singular value of the second PC (PC2) does not change significantly. In Fig. 4, concentric circles represent the equidistance of the Mahalanobis distance MD, 2MD, and 3MD corresponding to

*σ*

_{1},

*σ*

_{2}, and

*σ*

_{3}, respectively.

### 2.3 USBL-aided Navigation System of AUVs in Shallow Water

*x**( = {*

_{k}*X*,

_{k}*Y*,

_{k}*Z*,

_{k}*ψ*,

_{k}*u*,

_{k}*v*,

_{k}*w*,

_{k}*r*}

_{k}*) is composed of 3-D position, heading angle, 3-D velocity, and yaw angular velocity of the vehicle. The system disturbance vector*

^{T}*w*is mean zero and the covariance matrix is

_{k}

*Q**. If the 3-D position, heading, and 3-D velocity are observable, a measurement model can be expressed as*

_{k}

*y**is the measured states, the measurement matrix*

_{k}

*H**= [*

_{k}*I*

_{7×7}0

_{7×1}]. We assumed that the noise vector is the normal distribution

*v**∼*

_{k}*N*(0,

*R**) where the mean is zero and the covariance matrix*

_{k}

*R**is*

_{k}*X*and

_{k}*Y*is not correlated with other state variables, we can separately handle the covariance matrix

_{k}

*R**of the*

_{xyk}*x*and

*y*position derived from the error of the USBL measurements. Then the covariance matrix can be expressed as follows in the horizontal plane: where

*x*and

*y*, respectively, and

*ρ*

_{xy}*σ*

_{x}*σ*represents the error covariance correlated with the two variables. Because the USBL measurements of the

_{y}*x*and

*y*position in the shallow water are correlated with each other, it is necessary to decorrelate the error covariance by using its eigenvector before implementing a Kalman filter.

*x*can be converted into an arbitrary coordinate

**using the unitary matrix**

*x̃**Φ*(

*Φ*

^{H}*Φ*=

*I*). If

**is defined on the principal axis of the ellipse representing the error covariance of the USBL position measurement, it can be expressed as**

*x̃***=**

*x*

*ϕ*_{1}

*x̃*

_{1}+

*ϕ*_{2}

*x̃*

_{2}in the horizontal 2-D plane. Fig. 5 illustrates an example of matching the eigenvector to the main axis of the error covariance of the USBL measurements. In the figure,

*ϕ*_{1}and

*ϕ*_{2}are the eigenvectors of the error covariance matrix matched to the principal-axis of the error distribution.

*R**is a Hermitian matrix, the eigenvalues can be decomposed into a diagonal matrix of*

_{xyk}*R*=

_{xyk}

*Φ**Λ*

*Φ**(Scharlau, 1985), where*

^{H}*σ*are eigenvalues. All eigenvalues of the covariance matrix are non-negative, i.e.,

_{i}*σ*≧ 0, and

_{i}*x̃*=

*Φ*

^{H}*x*, if the coordinate axis is set in the principal axis of the error distribution and then the error covariance is calculated, it can be decomposed into a diagonal matrix with the eigenvalues as follows:

*σ*illustrated in Fig. 5, we can decide that the USBL measurement is an outlier.

### 3. Underwater Navigation with the Uncorrelated Error Model of USBL

### 3.1 Motion Data of an AUV and Simulation of the USBL Measurements

*u*,

*v*,

*w*; Fig. 7(b) shows the AUV’s latitude and longitude measured by the GPS,

*y*and

*x*, and Fig. 7(c) shows the heading, roll and pitch angles of the AUV by the AHRS. All the sensor information was stored at 100 sps (samples per second) rate, and the sampling rate of the DVL was 10 sps.

*x*- and

*y*-directions; The middle parts are outliers generated as explained previously; The lower parts show the simulated error of the USBL measurements including the two components of the errors.

### 3.2 Simulation of USBL-aided Underwater Navigation with the Uncorrelated Error Model

*σ*= 1.0 m and

_{r}*σ*= 1.0°, respectively. The procedure of the simulation was as follows: (1) Define the rms error

_{ψ}*σ*in the radial direction and the rms error

_{r}*σ*in the orthogonal direction to the radial direction; (2) Calculate the first principal axis PC1 and the second principal axis PC2 through PCA at the estimated position of the vehicle; (3) Decorrelated the error covariance matrix by obtaining the

_{ψ}*σ*

_{1}and

*σ*

_{2}corresponding to the principal components of PC1 and PC2 with the rotation matrix at the time instant when the USBL measurement is received; (4) Apply the decorrelated covariance matrix to the Kalman filter of the navigation system.

*σ*, in the principal axes. When the difference between the USBL measurement and the estimated position of the navigation system exceeds the OJL, the navigation system decides the USBL measurement as outlier and discards the signal. In the proposed navigation system, the OJL was determined by the five times of the MD, 5-

*σ*, for the difference between the USBL measurement and the estimated position. Whenever the USBL measurement was newly acquired, the MD

_{MD}*σ*was calculated with the rms errors of the radial direction and its orthogonal direction,

_{MD}*σ*and

_{r}*σ*, respectively, at the estimated position of the vehicle. When the MD deviated from the OJL 5-

_{ψ}*σ*, the USBL measurement was discarded in the simulation.

_{MD}*xy*-plane. The USBL measurements were indicated by the blue dots and the estimated position was marked in the red solid lines. Figs. 10(c) and 11(c) are enlarged plots in the time of [3,050 3,300] s for Figs 10(a) and 11(a), respectively. The estimated positions of the two simulations exhibited a slight difference, which was attributed to the effect of the unrejected outliers within the OJL. Comparing the two simulation results, therefore, we can find the outliers larger than the OJL were almost completely rejected and the proposed navigation system stably produced the estimated position of the vehicle even when the USBL measurement contaminated by outliers. Figs. 10(b) and 11(b) show the estimation errors of the navigation system in the

*x*- and

*y*-positions. The rms error of the estimated position was 3.50 m when only the intrinsic error of the USBL sensor was considered, and 3.87 m when the outliers were included additionally. The estimation error of the navigation system with the outliers increased by approximately 10% compared to that of the navigation system with no outlier. This increment was caused by the outliers less than the OJL, which was updated in the navigation system. However, the two estimation errors show a very similar pattern.

*σ*is indicated by the red dotted line. In the case of no outlier, the MD in Fig. 10(d) did not exceed the OJL over the whole time. In the case with the outliers, the MD frequently exceeded the OJL due to the outliers, as shown in Fig. 11(d). Although the differences between the estimated position and the USBL measurements were mostly within 4-

_{MD}*σ*, the OJL was set as 5-

_{MD}*σ*considering margin of the estimation error of the navigation system in these simulations. The navigation system stably detected and rejected the outliers larger than the criterion, as shown in the enlarged subplot in Fig. 11(d).

_{MD}### 3.3 Performance of Outlier Rejection

*xy*-plane for the time [2350 2600] s, respectively. In these figures, the black dashed lines indicate the GPS trajectories of the AUV, that is, the true positions, the blue points represent the measured positions with the USBL, and the colored solid lines indicates the trajectories of the estimated positions for the case of four different OJLs. In Fig. 12(a), the estimated positions of the proposed method for the four levels of OJL between 3.5-

*σ*and 5-

_{MD}*σ*were exactly the same, so the four colored solid lines were all overlapped on one trajectory. That is, the USBL-aided integrated navigation system with the USBL decorrelation model identified and removed the outliers in the same way even when the OJL was changed between 3.5-

_{MD}*σ*and 5-

_{MD}*σ*in the time period.

_{MD}*N*was 410. The number of the outliers’ magnitude exceeding the OJL would be less than 410. Because the magnitude of the random outliers used in the simulation was uniformly distributed within the range of [0

_{OL}*L*max (= 30)]

*m*, the expected number of outliers can be expressed as

*N*

_{expected}=

*N*×(1−

_{OL}*OJL*/

*L*

_{max}) in case of when there was no navigation error. The simulations were performed by adjusting the OJL for the navigation system with the conventional error model of USBL. Fig. 13 shows the estimated position errors with the conventional error model according to the OJL, as well as the number cut off the normal USBL signals needed for the navigation system.

*OJL*≤ 13 m, had minimum value in the range of 14 m ≤

*OJL*≤ 21 m, and gradually increased at

*OJL*> 21 m. The OJL should be set larger than 21 m to design the navigation system so that the normal USBL signals were not cut off. Otherwise, some of the normal signals larger than the OJL would be discarded. When the OJL was 22 m, 116 signals were judged as outliers and discarded without losing normal signal. In this case, the expected number of outliers will be

*N*

_{expected}= 109. Because the estimated position of the navigation system included errors, it is reasonable that the number of the blocked outliers differed from the expected value by approximately 6%. When the OJL was 16 m, the position estimation error was minimized, as shown in Fig. 13 and Table 3, while the number of the total blocked signal was 228. Among them, 47 normal USBL signals were blocked. The number of the blocked outliers in the simulation was 181, which differed approximately 5% from the expected number of 191. When the OJL was adjusted from 16 to 22 m, the blockage of the 47 normal signals was prevented. However, the 65 outliers were additionally considered as normal signals.

*OJL*= 3.5-

*σ*and

_{MD}*σ*= 2.0 m, the number of the signals detected as outliers and cut-off (including the USBL signals), and the number of the cut-off normal USBL signals. In the figure, the horizontal axis represents the change of the OJL, of which the angular rms error of the USBL

_{R}*σ*is varying within the range [0.5 1.5]°. In the simulations, the rms error

_{ψ}*σ*was also related with the covariance of the noise vector in the navigation system. From the point of view of outlier rejection, a value larger

_{ψ}*σ*than 1.2 is required because the normal USBL signals were lost when it was designed with

_{ψ}*σ*≤ 1.2°.

_{ψ}*σ*was set smaller, the number of the cut-off USBL signals increased. When

_{ψ}*σ*is set larger than 1.2, no signal was lost. In the range of

_{ψ}*σ*≥ 0.8°, as the value of

_{ψ}*σ*increases, the number of the rejected outliers decreased, but the change was very small. In

_{ψ}*σ*< 0.8°, the number of the rejected outliers was independent of the level of

_{ψ}*σ*. When

_{ψ}*σ*= 1.2°, one normal signal was lost, and the 282 outliers were rejected. In this case, an equivalent OJL in the Euclidean space would be 9.3 m. From the simulations, the proposed method has shown better performance of outlier rejection than the conventional method. In these simulations, on the other hand, the measurement error model of the navigation system was related with the level of

_{ψ}*σ*of the OJL 3.5-

_{ψ}*σ*. When

_{MD}*σ*was larger than 0.8°, the estimated position errors increased as

_{ψ}*σ*increases in the measurement error model of the system. So, it is required to designate a fixed value for

_{ψ}*σ*in the error covariance of the navigation system. Therefore, the navigation system with the uncorrelated model of USBL is more robust to outliers than the navigation system with a conventional error model of it.

_{ψ}### 3.4 Blackout Response Characteristics

*σ*= 1.0 m,

_{r}*σ*= 1.0°, respectively, and the outlier decision criterion was designed as 4-

_{ψ}*σ*in normal operation. In the simulation, the new reception of the USBL after the blackout occurred was considered a valid signal when the position deviation of

_{MD}*Δx*=

*x̃*−

*x̂*, i.e., the difference between the measured position and the estimated position, was within 8-

*σ*.

_{MD}*σ*. So, even if a normal USBL signal is received, the measurement cannot be used immediately as the innovation of the navigation system under a large deviation

_{MD}*Δx*.

*σ*, exceeding the threshold level of 8-

_{MD}*σ*. So, the AUV judged this signal as an outlier and did not use it as an innovation signal for the navigation system. Even when the normal USBL position measurement data were subsequently received, the navigation system continued to discard the USBL position measurement, because the deviation

_{MD}*Δx*exceeded the OJL. At 2,540 s, when the position error was within the range of 8-

*σ*, the navigation system used the USBL position measurement signal to update the error covariance, and the estimation error of the navigation system rapidly decreased and converged to the true position within 10 seconds. A further study on the integrity judgment reflecting the USBL positioning data received after a blackout and the navigation error is required.

_{MD}