UKF-Based Inverted Ultra-Short Baseline SLAM With Current Compensation

This paper presents an inverted ultra-short baseline (iUSBL) simultaneous localization and mapping (SLAM) based on Unscented Kalman filter (UKF) with current compensation. The complexity of the underwater environment prevents using a Global Positioning System (GPS) or visible-light cameras. Therefore, we rely on an acoustic positioning system to achieve autonomous underwater vehicle (AUV) navigation relative to the beacons. Our system uses an AUV equipped with a time-synchronized iUSBL array to measure the range and angle between the vehicle and acoustic beacons via one-way travel-time. Then, we propose UKF-based online SLAM to filter these acoustic measurements, and add the compensation for inherent underwater current. Compared to traditional SLAM applications usually involving many landmarks, this iUSBL acoustic system requires only sparse beacons to estimate the vehicle pose. Consequently, it does not cause UKF to have an excessive computational burden. Numerical results confirm the validity of the proposed method. In particular, the proposed algorithm prevents cumulative errors from proliferating quite well when using double beacons.


I. INTRODUCTION
Over the years, autonomous underwater vehicles (AUVs) have become an integral instrument for a variety of applications in industrial assistance, science studies, and defense missions. The navigation reliability of AUVs is challenging in an underwater context due to the tight environmental constraints when the GPS does not work, particularly in terms of navigational accuracy. Navigational reliability has been achieved through the development and use of a sophisticated sensor system, such as doppler velocity log (DVL)-aided inertial navigation systems (INS). Unfortunately, since no external aiding measurement is available while navigating underwater, the inertial position error grows unbounded over time [1]. Moreover, the required sensors are expensive, which drives up underwater vehicle price, size, and power use.
The associate editor coordinating the review of this manuscript and approving it for publication was Heng Wang .
In contrast, the acoustic ranging positioning systems had a long history of providing accurate navigation for underwater vehicles without the need for expensive sensors [2], [3]. Typically, AUV is equipped with an acoustic transmitter that establishes communication with a set of hydrophones. Given the propagation velocity of sound in underwater, the distance between the AUV and hydrophones can be calculated through the propagation time of the acoustic signal [4]. Then, the position for the AUV with respect to the set of hydrophones can be obtained by geometric methods. Due to the long distance transmission and the small attenuation loss of the underwater acoustic propagation, the underwater acoustic positioning technology has been fully developed, which includes long base line (LBL), short base line (SBL) and ultra-short base line (USBL) localization [5]. LBL systems require multiple geo-referenced transponders, limiting the operational area and carrying a penalty in setup time and difficulty. SBL and USBL typically mount on the hull of the deployment VOLUME 10, 2022 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ ship, requiring the fusion of ship attitude and position into the navigation solution for the underwater vehicle, thus the systems tend to be complex and costly [6]. On the other hand, traditional approaches for acoustic localization typically use two-way travel-time (TWTT) between beacons and an AUV to measure and estimate a vehicle position, and use phase difference to determine the relative range and bearing from the beacon to the vehicle. The problem with using TWTT is that it requires the AUV to have an active acoustic system to transmit the acoustic signal. It consumes significant power, which decreases runtime and adds cost. The advent of chip scale atomic clocks [7] has enabled the use of one-way travel-time (OWTT) [8] ranging through clock synchronization of the beacon and AUV. Rypkema et al. [9] presented experimental results with OWTT-iUSBL system localization, which uses a single acoustic beacon time synchronized to a passive USBL receiver array mounted on an underwater vehicle. This is referred to as inverted USBL. Acoustic data is collected using the array, and processed to get range, azimuth, and inclination to the acoustic source from vehicle. If the acoustic source is at a known location, instantaneous absolute vehicle position can be calculated using this information and AUV compass data. The current scheme is also used in this paper.
We propose an OWTT-iUSBL localization method based simultaneous localization and mapping (SLAM) technology in underwater environment. Although SLAM is now usually implemented with optical sensors such as lidar and cameras, using SLAM was also conceived as early as several decades ago [10]. Nowadays, a substantial corpus of methods and experience about SLAM has been developed in maritime applications [11]- [13]. Among the algorithms developed to solve the SLAM problem, the extended Kalman filter (EKF) remains a popular choice and has been used in many applications [14], [15], primarily due to its relative low computational complexity and ease of implementation. However, EKF-based SLAM is vulnerable to linearization errors, which can cause poor performance or even divergence. In order to reduce the estimation errors due to the EKF's linearization, we consider the unscented Kalman filter (UKF) [16], [17]. The UKF has been shown to generally perform better than the EKF in nonlinear estimation problems, and one would expect similar gains in SLAM.
We realize localization and mapping in the framework of SLAM, in which beacon as a landmark transmits an acoustic signal to iUSBL array fixed on the AUV. The ability to compensate for current disturbance is regarded as a key feature to realize accurate AUV localization due to the particularity of the underwater environment [18], [19]. Consequently, after formulating the problem of simultaneous localization and mapping, we present an efficient solution for online iUSBL array measurement with current compensation in a sparse target (landmark) environment.
The rest of this paper is organized as follows: section II presents the problem of SLAM using iUSBL array measurements. Then, section III describes in detail the Unscented Kalman Filter (UKF)-based SLAM solution. Section IV shows and analyzes the simulation results. Finally, Section V concludes the paper.

II. PROBLEM STATEMENT
The classical SLAM technique always uses an agent to build up a map within an unknown environment, i.e. the mapping, while at the same time keeping track of its own position, i.e. the localization [11]. In this paper, the agent is an AUV which is supposed to be equipped with USBL array and some other navigation sensors, such as an inertial measurement unit (IMU) and a depth sensor. Also, an acoustic beacon fixed underwater is used as a landmark for SLAM. The AUV relies on these sensors to detect and process acoustic beacon signal for localization and mapping. The beacon does not communicate with the vehicle, but instead periodically broadcasts an acoustic signal at a rate of 1 Hz [9]. It relies on a GPS receiver mounted on it to output the pulse-per-second (PPS) signals to provide precise time synchronization. The iUSBL acoustic receiver consists of a tetrahedral hydrophone array with d spacing used to detect the acoustic signal broadcasted by beacon. The geometry of the iUSBL is equivalent to a uniform planar array (UPA) of M elements (herein M = 4) observing a point-like target (beacon) at observation angle φ and inclination θ, respectively, from the array pointing direction, as shown in Figure 1.
The objective of SLAM is to provide a joint estimation of vehicle location and orientation parameters, together with the map of surrounding objects at each time instant k = 0, . . . , T . We assume that the vehicle location corresponds to the iUSBL location. Vehicle location (x, y), depth d U , heading ψ, and instantaneous velocity v form the usual state vector  and beacon. Since landmarks are assumed stationary, the state vector for the ith landmarks is Furthermore, because most AUVs cannot hover while overcoming the current, this can affect the vehicle position [20]. In this work, we use the same consideration as [21] which they considered stationary sea current and treated sea current as a constant vector with random noise. Also, we do not consider the velocity of currents in the depth direction. The velocity components of a 2-D (North and East) current vector are expressed as c = [v cx , v cy ]. Thus, the state vector x k , current estimation c, and landmark m combine into a dynamic esti- Like typical millimeter wave (MMW) radar-based SLAM [22], iUSBL provides the range and angular measurements of surrounding objects via its hydrophones array. The geometry of the problem is described in Figure 1 for a single target (beacon) observation. At time k the iUSBL measures the range r, observation angle φ which is to observe a landmark at angle from the array pointing direction, and inclination θ to every observed target: We assume that the data association problem is resolved and that every measurement corresponds to the proper landmark in the map with index i.
The measurement model is given by the nonlinear function: The function H (X k ) is determined according to iUSBL principles and the i-th beacon position, presented in Figure 1, generally as: where atan2(y, x) evaluates the angle of the vector in the Euclidean plane, given its coordinates (x, y). It can be alternatively defined as the principal value of the argument function atan2(y, x) = Arg(x + jy) in the interval (−π, π]. In this paper, we use the iUSBL hydrophone array mounted on AUV to directly measure the range between AUV and beacon. We assume that the distance between beacon and AUV is far enough that the acoustic signals received by each iUSBL hydrophone can be seen as a series of parallel signals, as shown in Figure 2. The iUSBL hydrophone array periodically receives a LFM (linear frequency modulation) signal broadcasted by an acoustic beacon that the iUSBL is synchronized with the beacon using the PPS signal. Since AUV and beacon use PPS synchronization, the beacon broadcasts acoustic signal while iUSBL hydrophones start sampling. We use matched filtering and beamforming to process acoustic data recorded by AUV and obtain the OWTT. We assume this travel-time from the i-th beacon is t i and the speed of underwater sound c is known. So the range value r i of measurement function (2) can be rewritten as: Similarly, we calculate the azimuth according to timedelays difference in the signals received by the diagonal hydrophones. Figure 3 displays the iUSBL array geometry. The value φ in measurement functions (2) can be rewritten as: where d is hydrophone elements spacing with half wavelength (λ/2), and t 3 − t 1 is the time delay (phase shift) a LFM signal arrives at hydrophone 3 and hydrophone 1, diagrammatically illustrated in Figure 3. For filter, the state transition equation in discrete time formula is: We assume the vehicle moves at nearly constant velocity.
Because the current estimates are constant and beacons are stationary, the discrete-time system state equation can be given as follows: where t is the time interval between the measurements, define the standard deviation of measurement noise of depth, heading, and velocity, respectively.
The state vector X k is composed of the vehicle states x k , current speed c, and the landmark's states m. Therefore, the estimates of the total state vector X k , maintained in the form of its mean vectorX k and the corresponding total covariance matrix P k is given as follows: where P vv , P vv , and P ll are the covariance matrices of the vehicle pose, current, and landmarks, respectively, while submatrices with different subscripts, like P ab = P T ba , denote the corresponding cross correlation matrices.
Other than that, when there is no current, the vehicle is expected to maintain a desired track, which is in line with the heading. However, under current presence, the vehicle heading is still expected to maintain at the same angle, but with the resultant, actual vehicle course direction will drift due to the action of current, as depicted in Figure 4. The vehicle needs to maintain the desired course, so its heading cannot be kept the same as it would have been without current. With reference to Figure 4, the desired course direction ψ from the current position to the next waypoint is known. And, we estimate the compensated heading ψ cmd by root-finding of the non-linear equation: As the UKF estimation proceeds step by step, v cx and v cy will converge to stable values. Finally, the geographical current estimates (magnitude and direction) are obtained [23]:

III. SIMULTANEOUS LOCALIZATION AND MAPPING WITH iUSBL MEASUREMENTS
We consider a discrete-time system whose dynamics are described by a set of nonlinear functions. The state vector X k transforms at a time step k − 1 to k using the process model f , as shown in (5). The measurement vector Z k is related to state vector using the observation function H as shown in (1).

A. UKF INITIALIZATION
The UKF is initialized by the USBL mounted on the vehicle when the vehicle is on the surface awaiting deployment. We assume that the position of the beacon (x 1 , y 1 , d 1 ) is known. The USBL hydrophones acoustically calculate the one-way travel-time range (r U ) to the beacon utilizing the time synchronization signal and acoustic speed and calculate the angle (φ U , θ U ) to a beacon using the phase-difference method, as demonstrated in Figure 2. The AHRS sensor and depth pressure sensor provide information about the vehicle heading ψ and depth d U , respectively. In the case, we can combine this information to project an estimate of the position (δx, δy, δd U ) of the beacon relative to the USBL array: where we assume that any pitch and roll experienced by the vehicle is negligible. Given the known position of the beacon (x i , y i , d i ), the vehicle position measured by the USBL array is given by: In the initial state, (11) can be rewritten as: is the relative position when the vehicle is at the initial mode in which the heading and velocity are both zero (ψ 0 = v 0 = 0). The initial state vector is given as: Furthermore, the initial mean and covariance matrix of the state can be defined as: Simultaneously GPS mounted on the vehicle can also measure the position and heading when the vehicle is on the surface. These measurements calculate the ground-truth values for range and angle between the vehicle and beacon, r TR , φ TR and θ TR , as: These ground-truth values can be used to compare with the initial USBL measurements, r U 0 , θ U 0 , and φ U 0 , and the difference represents the measurement error in our iUSBL system.

B. TIME UPDATE
Select the sigma points at (k − 1)-th time series: where (·) (i) means the ith column of the matrix square root of (·), n denotes the dimension of the state vector, λ = α 2 (n + κ) −n is a scaling parameter, which can be adjusted to improve the accuracy of the approximation, α determines the speed of the sigma points and is usually set to a small positive value (e.g., 1e − 4 ≤ α ≤ 1), and κ is a secondary scaling parameter which is usually set to κ = 3 − n.
Predict the mean and covariance of the samples in time k according to Equation (5): where W (m) i = 1/ (2(n + λ)) i = 1, 2, . . . , 2n (24) β is used to incorporate prior knowledge of the distribution of X. For normal distribution, the optimal choice is β = 2. The state variance matrix Q k−1 is set a small value in parameter estimation tasks.

C. MEASUREMENT UPDATE
Predict the state estimate of the sigma points: Calculate the gain matrix: The filtering result and its updated estimate covariance can be obtained:X Examining (30), notice that the value of measurement vector Z k in this equation is equal to the corresponding terms in (13), , when the UKF performs for the first time.

D. NEW LANDMARK INITIALIZATION
In order to achieve true exploration, this work enriches with an extra step of landmark initialization, where newly discovered landmarks are added to the map. Landmark initialization is performed by inverting the observation function H and using its mean and covariance to compute, from the USBL pose and the measurements, the observed landmark state and its necessary mean and covariance. The landmark is initialized based on the pose of the vehicle and the measurement by where Moreover, its mean and covariance iŝ

A. SCENARIO
We use MATLAB simulations to verify the effectiveness of the proposed UKF-based iUSBL SLAM method. In the simulations we consider a virtual iUSBL array with M = 4 hydrophone elements with d = 8 cm ≈ λ/2 spacing, as shown in Figure 2. Beacon A transmits a 10 − 12 kHz, 20 ms LFM up-chirp, and beacon B transmits a 11 − 9 kHz, 20 ms LFM down-chirp, both in sync at a rate of 1 Hz. For simplicity, it is assumed that iUSBL mounted on AUV maintains its depth the same as beacon, d U = d i = 3 m. For the beacon position and AUV course, we refer to the real experimental data in [9] and add the errors in range and angle produced during transmission after processing raw acoustic measurements using matched filtering. Acoustic range and azimuth measurement errors are µ ± σ = 0.03 ± 1.49 m and µ ± σ = −0.01 ± 3.16 • , respectively. AUV reached the desired depth before moving towards the first waypoint of the lawnmower path. It traveled the whole path by controlling its depth, then surfaced at the ending point. The mission lasted approximately 1270 s, resulting in the vehicle track seen in black in Figure 5. The blue circles in the figure illustrate iUSBL measurement results that we use MATLAB to simulate sampling and filtering acoustic signals received by four iUSBL hydrophones. When the vehicle is traveling along the lawnmower track, we use our proposed UKF-based online SLAM with sea currents estimation method to filter the waypoints measured by iUSBL array, and the filtered trajectory is shown in magenta in Figure 6. The red arrows in the figure are the results of the current estimation, which will be elaborated in Section IV-C. As shown in Figures 5 and 6, when the simulation makes use of two independent beacons, we can use the range-only values obtained from two beacons to estimate vehicle position via multilateration. Because the range circles from both beacons can generate two intersections, by judging the area where the vehicle is located, we can disambiguate the correct point and get more accurate vehicle positions than a single beacon. Thus, the magenta trace on the right of Figure 6 shows minor deviation.
In order to compare with our method, we also use EKF-SLAM to process vehicle position measured by the iUSBL array. A simulation example includes four cases: (a) -EKM-SLAM with single acoustic beacon, (b) -EKM-SLAM with double acoustic beacon, (c) -proposed algorithms realised via UKF-SLAM with single acoustic beacon, (d) -UKF-SLAM with double acoustic beacon. For better visibility, iUSBL array measurement points are not shown in the figures. In the following subsection, the performance of our method is evaluated numerically with three metrics, tested through N C = 20 Monte-Carlo realizations, as shown in Figure 7.

B. LOCATION ERROR
We use the Root Mean Square Error (RMSE) as the metric to analyze the performance of our method rigorously. The first to be investigated is the RMSE of vehicle position:  where p denotes the vehicle position in 2-D (the first and seconde elements in the state vector)), k expresses discrete time, and the subscript notation (·) (n) indicates the iteration of Monte-Carlo realization. The second metric is the RMSE of vehicle heading ψ at the fourth element in the state vector: Figure 7 illustrates the simulation results for two cases with different beacon numbers and the result of applying EKF and UKF methods, respectively. Figure 8 is the enlarged figure of the bottom plot of Figure 7. Referring to the vehicle course, we can see that the bottom plot of Figure 7 displays severe errors at the sharp turns, especially heading errors. Furthermore, the plots show that the scenario deploying two acoustic beacons provides a more accurate AUV track estimation than the scenario with only one beacon if the AUV moves away from the beacon over time. In our lawnmower path simulation, for the UKF-based SLAM, the dual beacon scenario has a maximum RMSE of 1.55 m on the y-axis, but the single beacon with a maximum RMSE of 5.58 m. Likewise, on the x-axis, the corresponding maximum RMSE values are 1.2 m and 3.06 m, respectively. The difference in heading RMSE is relatively slight. Overall, the error of UKF-SLAM is slightly smaller than that of EKF-SLAM.

C. CURRENT ESTIMATION
We estimate all the vehicle heading, velocity, and sea currents as zeros in the initial state. Also, the error in the position estimate is zero as it was in Section III-A. We simulate    the currents field maintaining stability in AUV sailing area, which speed magnitude remains at 0.15 m/s and bearing is 45 • . In Figure 9, the estimated path is shown along with the estimated direction of the sea current during the mission represented at intervals of 10 seconds by the red arrows. Figures 10 and 11 illustrate the estimated current bearing and magnitude, respectively. As the UKF algorithm operates recursively, the current estimation results converge to a highly stable value, shown in Table 1. The Direction and the low intensity of the estimated current are roughly coherent with what is presupposed, but the precision needs to be improved, especially in the case of two beacons. It will be the direction of our future efforts.

V. CONCLUSION
In this paper, we investigate an inverted ultra-short baseline underwater SLAM method based on UKF. Specifically, a joint acoustic signal filtering and sea current estimation problem is formulated to keep track of AUV location and update a map under sparse beacons. The extraction of AUV range and angle measurements from raw acoustic data online processing is firstly addressed, and then the UKF algorithm is designed to estimate vehicle position and orientation using iUSBL array acoustic measurements. The proposed algorithm also considers sea current estimation and compensation. Since iUSBL-aided localization does not need many beacons, it largely lightens the excessive computational burden caused by too many landmarks in classical SLAM. Extensive simulation results demonstrated that the proposed scheme obtain more accurate position information on the basis of effectively estimating the magnitude and direction of current compared to the dead reckoning errors. As our future works, we will verify the effectiveness of the proposed localization and mapping method using experiments in real scenarios and improve the accuracy of current estimate.