#### Abstract

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 covariance matching method. 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.

#### 1. 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–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 , 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.

#### 2. 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 (i) calculate sigma points (ii) UKF prediction (iii) UKF update where is the system noise covariance, is the observation noise covariance, is the Kalman gain, and is the weight of the mean and covariance.

##### 2.2. Adaptive UKF Algorithm

Adaptive filtering technology has become a focus of research attempting to solve the filter divergence problem caused by the inaccurate statistical properties of the noise and the mathematical model itself. Sage-Husa suboptimal unbiased maximum a posterior (MAP) noise estimators have the advantage that their recursive formula is simple, and the principle is clear and easy to implement. Therefore we chose to use this type of system to estimate unknown system noise.

###### 2.2.1. System Noise Estimation Method

Suppose a class of nonlinear dynamic systems is described as where represents the state vector of the system at time , represents the control, represents the measurement value of the state at time , and 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 time-varying 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 , , , is where is the output residual sequence of the UKF.

###### 2.2.2. 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 ; thus, the role of the observables is strengthened and the filter divergence is suppressed.

The convergence conditions can be written as where is an adjustable coefficient presetting and is the residual sequence, such that .

The correction method of covariance is

The adaptive weighting coefficient is calculated based on the fading factor formula [14, 15] where accounts for matrix trace. Here, 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.

###### 2.2.3. Realization of Adaptive UKF Algorithm

(1)* Initialization.* State initialization is done according to (1) and (20)

(2)* Time Update.* Given values for the variables and , we solve and based on the unscented transform, that is, using (4) and (7), and covariance is predicted by (5).

(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 using (18)~(19).

(4)* Measurement Update.* Obtain observation covariances and and filter gain according to (8)~(10), and then do a measurement update using (11)~(12).

(5)* Recursively Estimate System Statistical Noise Characteristics.* Recursively estimate the system’s statistical noise characteristics according to (16).

#### 3. SLAM Algorithm Based on Adaptive UKF

##### 3.1. SLAM System Model

###### 3.1.1. 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, , 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 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 and , 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 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 to time : where is the position and heading of the UUV in ; gives the line velocity and angle velocity of the UUV in , and is the sample time. In this equation, 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.

###### 3.1.2. 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 is the observation vector, is a white Gaussian with zero mean, and 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 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 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

##### 3.2. 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–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.

###### 3.2.1. 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.

###### 3.2.2. 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 .

##### 3.3. 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 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 is the noise with a zero-mean white noise with covariance 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 obtained by

To determine if the correspondence is valid, an individual compatibility (IC) test [27] using the Mahalanobis distance is carried out where 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:

#### 4. AUKF-SLAM Algorithm Verification Based on Sea Trial Data

##### 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 Figure 4. 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 ). 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.

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.

A general description of the AUKF-SLAM algorithm is given in Figure 6.

##### 4.2. 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 performing a coordinate transformation for the measurement data in a two-dimensional plane. Assume the position of point in is . Then for each ranging sonar, we have where and represent distance values and mounting angles of the three sonar instruments. For the sea trial, the mounting angles were , , and , corresponding to the left (), middle (), and right () sonar.

The relationship between and is shown in Figure 2, with the position of point in given by

The position of relative to is , so we can obtain the position of point in using the following synthesis operator

Figure 7 gives the embankment measurement, where the green, red, and blue points were acquired by the left, middle, and right sonar, respectively.

##### 4.3. Line Feature Extraction of 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 and the distance resolution is 1 m;(b)the largest number of votes is selected as the threshold;(c)a arc is used to model the horizontal beam width of the ranging sonar; that is, ;(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, .

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.

##### 4.4. 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.

###### 4.4.1. 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 gives the results of the system noise tests and the resulting laws, where is the step number. The observation noise is

###### 4.4.2. Test Results

Figures 8~15 are the test results given for both time-varying and constant noise conditions. Figures 8 and 12 compare the trajectory estimations from the different algorithms against the UUV’s GPS trajectory with time-varying noise and constant noise, respectively. Figures 9 and 13 compare position errors in the East direction between the different algorithms for the different types of noise, while Figures 10 and 14 provide a comparison of the position errors in the North direction. Figures 11 and 15 show the adaptive weighting coefficient of AUKF-SLAM algorithm in time-varying and constant noise test, respectively.

###### 4.4.3. Analysis of Test Results

The heading measured by the OCTANS is relative to the magnetic north of coordination system shown in Figure 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 and are the estimation error in the East and North directions at time , respectively, while and 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 , 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 Figures 11 and 15, it can be seen that the adaptive weighting coefficient is greater than one at some certain 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 are the true position and estimated position, respectively, of the UUV at time . 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 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 embankment, while Figure 18 is a simultaneous localization and mapping based on AUKF. During UUV navigation, the algorithm uses the three ranging sonar to perceive the environment, continuously extracting - parameters based on the Hough transform. The model accomplishes navigation based on the AUKF-SLAM algorithm by repeatedly observing available line features to continuously correct the UUV’s position and update its existing line features map. The AUKF-SLAM algorithm avoids the error acceleration caused by dead reckoning and ensures that a UUV no longer has to periodically float to the surface for GPS correction; one of the most important conditions for UUV’s tasked with long-term missions where they must remain hidden.

#### 5. 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.

#### Acknowledgments

This research work is supported by the National Natural Science Foundation of China (Grant no. E091002/50979017), the Ph.D. Programs Foundation of Ministry of Education of China and Basic Technology (Grant no. 20092304110008), the Research Operation Item Foundation of Central University (Grant no. HEUCFZ 1026), the Harbin Science and Technology Innovation Talents of Special Fund Project (Outstanding Subject Leaders) (no. 2012RFXXG083), and the New Century Excellent Talents Foundation of Education of China (no. NCET-10-0053).