An Adaptive UKF Based SLAM Method for Unmanned Underwater Vehicle

This work proposes an improved unscented Kalman filter (UKF)-based simultaneous localization and mapping (SLAM) algorithm based on an adaptive unscented Kalman filter (AUKF) with a noise statistic estimator. The algorithm solves the issue that conventional UKF-SLAM algorithms have declining accuracy, with divergence occurring when the prior noise statistic is unknown and time-varying. The new SLAM algorithm performs an online estimation of the statistical parameters of unknown system noise by introducing a modified Sage-Husa noise statistic estimator. The algorithm also judges whether the filter is divergent and restrains potential filtering divergence using a covariancematchingmethod.This approach reduces state estimation error, effectively improving navigation accuracy of the SLAM system. A line feature extraction is implemented through a Hough transform based on the ranging sonar model. Test results based on unmanned underwater vehicle (UUV) sea trial data indicate that the proposed AUKF-SLAM algorithm is valid and feasible and provides better accuracy than the standard UKF-SLAM system.


Introduction
The simultaneous localization and mapping (SLAM) algorithm [1,2] was first proposed by Smith, Self, and Cheeseman in 1988 to provide localization and map building for mobile robots and is now widely used in many different mobile robot systems.The SLAM algorithm was first used for unmanned underwater vehicle (UUV) navigation in September 1997 in a collaborative project between the Naval Undersea Warfare Center (NUWC) and Groupe d'Etudes Sous-Marines de l' Atlantique (GESMA).The objective of the trial using SLAM was to get a UUV starting in an unknown location and without previous knowledge of the environment to build a map using its onboard sensors and then use the same map to compute the robot's location.
Given the recent wider use of UUV in the marine environment, it is notable that truly autonomous SLAM-based UUV navigation is still lacking.However, it is challenging to develop SLAM-based UUV navigation due to factors such as system complexity, weak perception, nonstructure, increasing system noise, and unknown statistical characteristics.
SLAM solutions can be divided into two categories: a nonprobabilistic probability estimation method and methods primarily based on probability estimation; Table 1 gives the summery of SLAM methods.The probability estimation method first developed [3] was the EKF based SLAM algorithm, which suffers difficulty solving data association problems, high computational costs due to the calculation of Jacobian matrices, and inconsistency due to errors introduced during linearization.In trying to reduce storage and computational requirements, Thrun et al. [4] proposed a SLAM algorithm based on sparse extended information filter.However, this method is only applicable for creating feature maps and requires the existence of features that are easy to extract and distinguish in the environment, such as point, line, and face features.More recently, Montemerlo et al. [5] proposed a Rao-Blackwellized particle filter based SLAM method (FastSLAM), where each particle stores its map and robot positioning result.However, this algorithm results in a calculation and storage problem proportional to the number of particles and is unable to avoid the disadvantages of particle degradation and sample dilution.
The unscented Kalman filter (UKF) [6,7] is a nonlinear filter based on unscented transform (UT).For nonlinear systems, UKFs avoids linearization of the state and measurement equations.Additionally, the UKF principle is simple and easy to implement as it does not require the calculation of Jacobians at each time step, and the UKF is accurate up to second order moments in the probability distribution function propagation whereas the EKF is accurate up to first order moment [8].However, when an UKF is used in underwater SLAM, it needs to predict mathematical model of the system and a priori knowledge of the noise statistics.In many practical applications, prior statistics of the noise is unknown or not accurate.Even if this information is known, the statistical characteristics easily change due to internal and external uncertainties that reflect strong time-varying characteristics.Thus, a conventional UKF does not have the adaptive ability to respond to changes in the noise statistics, which can lead to large estimation errors and even cause divergence in the case of unknown and time-varying noise statistics [9][10][11].
In order to solve the above problem, we apply an adaptive UKF (i.e., an adaptive unscented Kalman filter, or AUKF) filtering algorithm to underwater SLAM.By introducing a modified (i.e., suboptimal and unbiased) Sage-Husa maximum a posterior (MAP) noise statistic estimator, the new algorithm provides online estimation of the statistical parameters of unknown system noises and restrains filtering divergence.In addition, the method uses a covariance matching criterion approach to determine the convergence of the filter.When the filter is divergent, the proposed method introduces an adaptive fading factor to correct prediction error covariance, adjust the filter gain matrix K, and suppress filter divergence, thus enhancing the fast tracking capability of the filter.Test results based on sea trial data of UUV indicate that the proposed AUKF-SLAM algorithm provides better navigational accuracy than a conventional UKF-SLAM algorithm.

Adaptive UKF Algorithm
2.1.UKF Algorithm.Unscented Kalman filters were first proposed by Julier and Uhlmann [12].The algorithm's main principle is to select a number of sampling points in the state distribution (sigma points), which can completely capture the true mean and covariance of state distribution.Those sigma points are then substituted into the nonlinear function to obtain the corresponding nonlinear function point set, and it can solve the mean and covariance after transformation by these points.
The mean, estimate variance, and measurement variance obtained from the unscented transform are introduced into a gradually recursive process of Kalman filtering to obtain the UKF.The main steps of a UKF algorithm are as follows.
(1) Initialization (2) For  ∈ {1, . . ., ∞} (i) calculate sigma points (ii) UKF prediction (iii) UKF update where  is the system noise covariance,  is the observation noise covariance, K is the Kalman gain, and   is the weight of the mean and covariance.
where   represents the state vector of the system at time ,  −1 represents the control,   represents the measurement value of the state at time , and  −1 and   are independent white Gaussian noise with time-varying means of   and   and covariances of   and   , respectively.Note that   is a nonnegative definite symmetric matrix, while   is positive definite symmetric matrix.
Emphasis should be placed on recent data when estimating time-varying noise statistics; that is, the algorithm should gradually forget data that is too old.In this paper, we adopt a fading memory weighted exponent method to design a timevarying noise statistics estimator.According to the literature [13], we selected the weighting coefficient to satisfy which can be written as The recursive formula of a fading memory time-varying noise statistical estimator described by q , Q , r , R is where   =   − ℎ(x |−1 ) is the output residual sequence of the UKF.

Filter Divergence Suppression Method.
Since suboptimal Sage-Husa filters often diverge, in this paper we judge whether filtering divergence is occurring according to convergence conditions derived from the covariance matching criterion.If the convergence conditions are satisfied, the Sage-Husa algorithm is applied.If filtering divergence occurs, the proposed method calculates an adaptive weighting coefficient   through a computational fading factor formula and applies this coefficient to correct P |−1 ; thus, the role of the observables is strengthened and the filter divergence is suppressed.
The convergence conditions can be written as where  ( ≥ 1) is an adjustable coefficient presetting and V  is the residual sequence, such that V  =   − h( |−1 ).
The correction method of covariance P |−1 is The adaptive weighting coefficient   is calculated based on the fading factor formula [14,15] where tr(⋅) accounts for matrix trace.Here, 0 <  ≤ 1 is a forgetful factor (typically about 0.95) used to increase the filter's tracking ability, with greater values of the factor creating a smaller proportion of information before time  and causing a more prominent residual vector effect.This method has a strong tracking ability for sudden status changes but still keeps tracking for slowly varying state and mutation status changes when the filter reaches a steady state.
(3) Convergence Judgment.At this stage, the method uses (17) to judge whether the filter is converging.If the filter is converging, move to the next step; otherwise correct P |−1 using ( 18)∼( 19).
(5) Recursively Estimate System Statistical Noise Characteristics.Recursively estimate the system's statistical noise characteristics according to (16).

AUV Nonlinear Dynamic Model.
As seen from Figure 1,  is the global coordinate system established with the initial location and bow of the UUV, where , , and  describe the position and heading of the UUV within this system.While  is the UUV vessel frame system and  is the sonar coordinate system.The North-East coordinates are given by , with its North direction based on magnetic North.Obviously,   =  +   0 , where   is the heading of the UUV as measured by its OCTANS.
Note that there is a distance offset (1.85 m in the  direction, 0.65 m in the  direction, and a very small deviation in the  direction that can be ignored) between the mounting positions of three ranging sonar and the UUV's center of gravity.The coordinate systems  and  are shown in more detail in Figure 2. We assume that the positions of the origin of S and  in  are (  ,   ) and (  ,   ), respectively, and that three ranging sonar are mounted together (i.e., their mounting positions are the same).This gives a mounting deviation of three ranging sonar in  of  = 1.85 m and  = 0.65 m, where  is the deviation in the  direction and  is the deviation in the  direction.The mounting angle of the middle sonar is , with mounting angles of  ± 7.5 for the left and right sonar.According to Figure 2, using the ranging sonar as a reference we get The method uses a simple 4 DOF (degree of freedom) constant velocity kinematics to predict how the state will evolve from time  − 1 to time : where [, , , ] is the position and heading of the UUV in ; [, V, , ] gives the line velocity and angle velocity of the UUV in , and  is the sample time.In this equation, n() is the portion of the system noise with a time-varying mean and covariance, with the covariance of vector  given by system noise where   is a delta function.

Feature Model.
The feature data used in this paper is derived from measurements of the structured port environment, so the algorithm itself is what chooses for line features with which to build a feature map.The line feature model used in the proposed method is 3.1.3.Observation Model.The UUV uses a Doppler velocity log (DVL), compass, and pressure sensor to provide direct measurements of the vehicle's velocity, heading, and depth, respectively.Thus, the observation model is linear, and so the common model becomes where z is the observation vector, m is a white Gaussian with zero mean, and H varies with changes in the measurement: 3.1.4.Ranging Sonar Model.The transmission beam of a ranging sonar creates a conical surface, which is a fan in a two-dimensional plane.David Ribas et al. [16] proposed the underwater mechanical scanning imaging sonar model based on the terrestrial single beam ranging sonar model [17].In this paper, we determine the location of line features in the environment using the measurement data from the ranging sonar.
In Figure 3,  represents the horizontal beam width,  represents the incidence angle, and   is the range at which the bin was measured from the sensor.Reference frame  defines the position of the transducer head at the moment the sensor obtains a particular bin, where [   ,    ,    ] is the transformation defining the position of S with respect to  to the chosen base reference.Both [   ,    ,    ] and   are obtained from information in the data buffer.
To emulate the effect of the horizontal beam width, the model uses a set of  values at a given resolution within an aperture of ±/2 around the direction in which the transducer is oriented.This set of values is also referred to as    , where Each value    represents a bearing parameter for a line tangent of the arc that models the horizontal beam width.As stated earlier, not only are all lines tangent to the arc candidates for line features, but the ones inside the maximum incidence angle limits of ± are also considered candidates.For this reason, the algorithm takes each  value at a given resolution for each value of    and within an aperture of ±; that is, The results of this operation are  ×  different values of    for the given aperture.These are the bearings for a set of lines representing all the possible candidates compatible with the bin.Given the geometry of the problem, the range parameter   , corresponding to each one of the   , bearings is L in e fe at u re L Figure 3: Ranging sonar model to distinguish line features.

Feature Extraction.
In the sea trial, the application environment of the SLAM algorithm is a cross-section of the ports, dams, and other environment structures.Note that the scanning surface of the sonar and the vertical wall or other vertical extensions of the surface will create line features in the resulting acoustic image.However, the parameters of such static line features will not change as the sonar position changes.The most popular line feature extraction methods include the split-and-merge method [18] proposed by Pavlidis, the RANSAC method [19] proposed by Fischler and Bolles, and the Hough transform (HT) [20] proposed by Illingworth and Kittler.Between these choices, HT is the most popular line feature extraction method, and a number of others have developed many methods to improve HT line feature extraction [21][22][23].In this paper, we use the HT method to extract line features from the ranged sonar data.The HT is a voting scheme where the distance values of each ranged sonar image accumulates evidence for the presence of a line feature.Units that get the most votes in the HT space correspond to line features in the actual environment.

Data Processing of Ranging Sonar Data.
A data buffer helps to separate and manage the stream of measurements produced by the continuous arrival of the sonar beams.The buffer stores variables such as the range and bearing for each bin used in the voting, the position and heading in the North-East coordinate system , and the transmit angle of the beams so that - parameter pairs used to present line features are extracted based on HT.
The steps to process the data set from each ranged sonar scan [24,25] are given below, using the left sonar as an example.First, the data buffer is set, the data is loaded with the range values, and a 0-1 matrix is built where the units without range values are set to 0 and units with range values are set to 1.In the second step, the transmit angle of the sonar in , the time, the position and heading of the UUV in , and the position and transmit angle of the sonar in  are stored into the data buffer.The third step defines the base frame , where  is the current position of the sonar head when the voting is performed.Finally, the position and heading of the sonar instrument in  at every moment is acquired and stored in the data buffer.

Hough Transform.
There are three steps to extract line features from the sonar image data.First, the data from all three sonar instruments are loaded, and distance resolution, angle resolution, and threshold value are defined.Secondly, the accumulator is defined, and the index values of the nonzero elements of the accumulator are found.Finally, we use ( 27)∼(29) to vote, and the - parameter pairs that get the most votes are used to represent the detected line features in .

Data Association.
Once the model has extracted line features in the environment based on the HT algorithm, it needs to create an environment map and improve the state estimation of the UUV by fusing the detected line features.The next step is then to perform data association [26] to determine if a measured line    corresponds to any of the features   ,  = 1, . . .,  already existing in the map and so used to update the system or if it is a new line and has to be incorporated into the map.To make this distinction, the most popular individual compatibility nearest neighbor data association algorithm is used to select the best candidate.
Given the transformation of  and , the position of the th line measurement in  can be represented by If we assume the position of the line feature  already exists in the map in ; that is, Then the position of line feature  in V is And line feature  corresponds to the th line measurement, where Here,    and    are the parameters of the line features in the  frame of reference,   and   are the parameters of the line features presented in the  frame of reference, and s  is the noise with a zero-mean white noise with covariance R affecting the line feature observation.
The proposed method uses an innovation term ^ to calculate the discrepancy between the measurement and its prediction, with its associate covariance matrix S  obtained by To determine if the correspondence is valid, an individual compatibility (IC) test [27] using the Mahalanobis distance is carried out where  = dim(ℎ  ) and  is the desired confidence level.Data association is only performed if and when a line feature is detected based on HT.If the data association is successful, that is, the line feature exists in the map, then the model updates the state estimate.Otherwise, state augmentation is carried out where the new measurement is added to the current state vector as a new feature.However, the algorithm cannot do this augmentation directly because the new feature is represented in .Thus, the algorithm must first perform a change of reference using the following: . . .DVL can measure a UUV's current velocity, bottom tracking speed, and so on.However, in the sea trial DVL was only used to measure the bottom track speed while OCTANS was used to measure the UUV's heading in real-time, that is, the angle between the front of the UUV and magnetic North.The pressure sensor provided depth data by measuring the water pressure, and three ranging sonar provided online environment perception and measurement.Three ranging sonar mounted in the horizontal frame is shown as Figure 5.

AUKF-SLAM Algorithm Verification Based on Sea Trial Data
A general description of the AUKF-SLAM algorithm is given in Figure 6.

Acquisition of Embankment Measurement Points Using
Ranging Sonar.Given a true trajectory as provided by GPS, we can obtain embankment measurement features by fusing this GPS information with the ranging sonar data and then where   and   represent distance values and mounting angles of the three sonar instruments.For the sea trial, the mounting angles were  1 = 7.5 ∘ ,  2 = 0 ∘ , and  3 = −7.5 ∘ , corresponding to the left ( = 1), middle ( = 2), and right ( = 3) sonar.The relationship between  and  is shown in Figure 2, with the position of point  in  given by  The position of  relative to  is we can obtain the position of point  in  using the following operator Figure 7 gives the embankment measurement, where the green, red, and blue points were acquired by the left, middle, and right sonar, respectively.

Line Feature Extraction the Embankment Based on
Ranging Sonar Model.The proposed model can extract line features using an HT form ranging sonar model based on the following assumptions: (a) angle resolution of the HT space is 5 ∘ and the distance resolution is 1 m; (b) the largest number of votes is selected as the threshold; (c) a 10 ∘ arc is used to model the horizontal beam width of the ranging sonar; that is,  = 10 ∘ ; (d) the model does not consider uncertainty in the vertical beam width; (e) assume that there is only an echo signal when transmit beam of the ranging sonar is parallel to the vertical extension surface; that is,  = 0 ∘ .
In the sea trail the algorithm extracted six line features L1∼L6.The - parameter, time, and position of the UUV when the feature was detected are given in Table 2 for each line feature.

Test Validation Based on Sea Trial Data.
To verify the performance of the AUKF-SLAM algorithm, we compared the results of tests using AUKF-SLAM, UKF-SLAM, and EKF-SLAM algorithms against trial data with the UUV and assuming that the statistical properties of the system noise was unknown in all cases.

Test Conditions.
The actual statistical characteristics of the system noise during the field trial was unknown, so for this work we assumed that actual system noise behavior was based on two laws, one time-varying and one constant.Using these laws, we conducted both a time-varying noise test and a constant noise test.Table 3 1, and it should be transformed to the heading  in the  coordinate system for state updating.The heading  is shown as Figure 16.
(1) Performance Analysis of the AUKF-SLAM Algorithm.We calculate the estimation error of the algorithm by where   time.The results indicate that once the Sage-Husa UKF-SLAM tends to diverge according to convergence conditions, the adaptive weighting coefficient is introduced to correct covariance and ensure the tracking ability.
(2) Root-Mean Square Error.We use the root-mean square error (RMSE) of the position to compare the performance of the various nonlinear filters where  is the total running step and (  ,   ) and ( x , ŷ ) are the true position and estimated position, respectively, of the   the AUKF-SLAM algorithm is smaller than the RMSE found using the UKF-SLAM algorithm by 1.9152 m; in the constant noise test the AUKF-SLAM algorithm RMSE is smaller by 0.9855 m.
From the above analysis, we can see that the AUKF-SLAM algorithm has good tracking ability and produces a RMSE that is smaller than what the other algorithms are capable of achieving for both time-varying and constant system noise.Also, the AUKF-SLAM algorithm produces a smaller RMSE in the presence of time-varying system noise as compared to when operating in a system with constant noise.
Figure 17 shows a comparison of the line features extracted during the environment and feature measurement of the

Conclusion
The proposed AUKF-SLAM algorithm adopts an improved Sage-Hausa suboptimal unbiased maximum posterior noise statistical estimator to estimate unknown system noise.The algorithm estimates and corrects the statistical character of noise in real time and decreases estimated error.At the same time, the algorithm judges whether the filter is converging and introduces an adaptive forgetting factor to correct the predicted covariance adjust the Kalman gain, and restrain the divergence of the filter when it diverges, therefore increasing the algorithm's fast tracking ability.The AUKF-SLAM algorithm provides a new method for simultaneous underwater localization and mapping in an unknown environment.Assistant navigation based on the proposed AUKF-SLAM algorithm can help UUV's fulfil missions requiring  marine environment monitoring, marine terrain inspection, and long-term underwater tasks.

Figure 1 :Figure 2 :
Figure 1: Global coordinate system, the UUV vessel frame system and the sonar coordinate system.

4. 1 .
Test Conditions.The data used in the test conducted in this work comes from a sea trial completed in October 2010 near Dalian Xiaoping Island using a UUV developed by the authors.The navigation route of the UUV was from point  to point  in Figure4.As configured for the trails, the UUV possessed a number of different sensors including DVL, OCTANS, depth sensor, and three ranging sonar which mounted in the horizontal frame as a whole set to observe the environment.The initial position of the UUV (point ) was longitude 121.5231 ∘ , north latitude 38.8271 ∘ , and the trial ended at longitude 121.5083 ∘ , North latitude 38.8328 ∘ (position B).The initial heading of the UUV was −70.70 ∘ .During the trail, the UUV stayed near the surface of the water so that GPS was available throughout the trial.The total navigation time was 17 minutes and 6 seconds.

Figure 4 :
Figure 4: Satellite image of the sea trial test area.

Figure 5 :
Figure 5: The ranging sonar mounted in the horizontal frame.

Figure 7 :
Figure 7: Feature measurement of the embankment.

Figure 10 :
Figure 10: Comparison of position error in the North direction with time-varying noise.

Figure 11 :
Figure 11: Adaptive weighting coefficient of AUKF-SLAM algorithm in time-varying noise test.

Figure 12 :Figure 13 :
Figure 12: Comparison of the trajectory estimations with constant noise.

Figure 14 :Figure 15 :
Figure 14: Comparison of position error in the North direction with constant noise.

Figure 16 :
Figure 16: Heading of the UUV during the field trial.

Figure 17 :
Figure 17: Comparison of line features extracted from the environment and the feature measurement of the embankment.

Table 1 :
Summery of SLAM methods.
gives the results of the system noise tests and the resulting laws, where  is the step number.The observation noise is  = diag ([0.04 2 0.04 2 0.04 2 0.04 2 0.04 2 ]) .
, and  , are the estimation error in the East and North directions at time k, respectively, while (  ,   ) and ( x , ŷ ) are the true position and estimated position of the UUV at time .Note that | , | and | , | represent absolute values of the error in the East and North directions at time k, respectively.Obviously, small values of |  | and |  | indicate higher accuracy in the filtering algorithm.Figures 8∼10 and Figures 11∼14 show that |  | and |  | for the proposed AUKF-SLAM algorithm are lower than the estimation errors given by the other algorithms.From Figures11 and 15, it can be seen that the adaptive weighting coefficient is greater than one at some certain

Table 2 :
Extracted line feature parameters.

Table 3 :
Change law of system noise.

Table 4 :
Comparison of RMSE.Table 4 gives the RMSE for each of the tested algorithms.From the table, we can see that the RMSE of the AUKF-SLAM algorithm is the smallest for both time-varying system noise and constant system noise.The RMSE in the case of time-varying noise for the AUKF-SLAM algorithm is smaller than the RMSE found in the constant noise scenario by 2.3534 m.In the time-varying noise test, the RMSE of