# An Adaptive UKF Based SLAM Method for Unmanned Underwater Vehicle

Authors: Hongjian Wang; Guixia Fu; Juan Li; Zheping Yan; Xinqian Bian
Journal: Mathematical Problems in Engineering
 (2013)

---

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


---

## Body

## 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; Table1 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.Table 1
Summery of SLAM methods.
Algorithm
Drawbacks
EKF-SLAM
(1) Difficulty to solve data association problems(2) High computational costs(3) Inconsistency due to errors introduced during linearization
EIF-SLAM
(1) Only applicable for creating feature maps(2) Requires the existence of features, such as point, line, and face features.
FastSLAM
(1) Calculation and storage problem proportional to the number of particles(2) Particle degradation and sample dilutionThe 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 matrixK, 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(1)x-0=E[x0],P0=E[(x0-x-0)(x0-x-0)T].(2)
Fork∈{1,…,∞}
(i)
calculate sigma points(2)χk-1=[x-k-1x-k-1±(nx+λ)Pk-1].(ii)
UKF prediction(3)χk∣k-1=f(χk-1,uk-1),(4)x-k∣k-1=∑i=02nxWi(m)χi,k∣k-1,(5)Pk∣k-1=∑i=02nxWi(c)[χi,k∣k-1-x-k∣k-1][χi,k∣k-1-x-k∣k-1]T+Q,(6)zk∣k-1=h(χk∣k-1,uk-1),(7)z-k∣k-1=∑i=02nxWi(m)zi,k∣k-1.(iii)
UKF update(8)Pz~kz~k=∑i=02nxWi(c)[zi,k∣k-1-z-k∣k-1][zi,k∣k-1-z-k∣k-1]T+R,(9)Pxkzk=∑i=02nxWi(c)[χi,k∣k-1-x-k∣k-1][zi,k∣k-1-z-k∣k-1]T,(10)Kk=PxkzkPz~kz~k-1,(11)x-k=x-k∣k-1+Kk(zk-z-k∣k-1),(12)Pk=Pk∣k-1-KkPz~kz~kKkT,where Q is the system noise covariance, R is the observation noise covariance, K is the Kalman gain, and Wi 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(13)xk=f(xk-1,uk-1)+Wk-1,zk=h(xk)+Vk,
where xk represents the state vector of the system at time k,  uk-1 represents the control, zk represents the measurement value of the state at time k, and Wk-1 and Vk are independent white Gaussian noise with time-varying means of qk and rk and covariances of Qk and Rk, respectively. Note that Qk is a nonnegative definite symmetric matrix, while Rk 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
(14)βi=βi-1b,0<b<1,∑i=1kβi=1
which can be written as
(15)βi=dkbi-1,i=1,…,k,dk=(1-b)(1-bk).
The recursive formula of a fading memory time-varying noise statistical estimator described by q^k, Q^k, r^k, R^k is
(16)q^k=(1-dk)q^k-1+dk[x^k-∑i=02nxWi(m)f(χi,k-1)]Q^k=(1-dk)Q^k-1+dk[∑i=02nxWi(c)(χi,k∣k-1-x^k∣k-1)KkεkεkTKkT+Pk-∑i=02nxWi(c)(χi,k∣k-1-x^k∣k-1)(χi,k∣k-1-x^k∣k-1)T∑i=02nxWi(c)(χi,k∣k-1-x^k∣k-1)],r^k=(1-dk)r^k-1+dk[zk-∑i=02nxWi(m)h(χi,k∣k-1)],R^k=(1-dk)R^k-1+dk[εkεkT-∑i=02nxWi(c)×(zi,k∣k-1-z^k∣k-1)×(zi,k∣k-1-z^k∣k-1)T∑i=02nxWi(c)],
where εk=zk-h(x^k∣k-1) 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λk through a computational fading factor formula and applies this coefficient to correct Pk|k-1; thus, the role of the observables is strengthened and the filter divergence is suppressed.The convergence conditions can be written as(17)vkTvk≤S·tr[E(vkvkT)],
where S(S≥1) is an adjustable coefficient presetting and vk is the residual sequence, such that vk=zk-h(x-k∣k-1).The correction method of covariancePk|k-1 is
(18)Pk∣k-1=λk·∑i=02nxWi(c)[χi,k∣k-1-x-k∣k-1]×[χi,k∣k-1-x-k∣k-1]T+Qk-1.The adaptive weighting coefficientλk is calculated based on the fading factor formula [14, 15]
(19)λk={λ0λ0≥11λ0<1,λ0=tr[Nk]tr[Mk],Nk=tr(C0,k-R)T,Mk=tr(∑i=02nxWi(c)[zi,k∣k-1-z-k∣k-1][zi,k∣k-1-z-k∣k-1]T),C0,k={vkvkTk=1ρC0,k+vkvkT1+ρk>1,
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 k 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)
(20)q^0=q0,Q^0=Q0.(2)Time Update. Given values for the variables x^k-1 and Pk-1, we solve x-k|k-1 and z-k∣k-1 based on the unscented transform, that is, using (4) and (7), and covariance Pk∣k-1 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 Pk∣k-1 using (18)~(19).(4)Measurement Update. Obtain observation covariances Pz~kz~k and Pxkzk and filter gain Kk 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).


## 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(1)x-0=E[x0],P0=E[(x0-x-0)(x0-x-0)T].(2)
Fork∈{1,…,∞}
(i)
calculate sigma points(2)χk-1=[x-k-1x-k-1±(nx+λ)Pk-1].(ii)
UKF prediction(3)χk∣k-1=f(χk-1,uk-1),(4)x-k∣k-1=∑i=02nxWi(m)χi,k∣k-1,(5)Pk∣k-1=∑i=02nxWi(c)[χi,k∣k-1-x-k∣k-1][χi,k∣k-1-x-k∣k-1]T+Q,(6)zk∣k-1=h(χk∣k-1,uk-1),(7)z-k∣k-1=∑i=02nxWi(m)zi,k∣k-1.(iii)
UKF update(8)Pz~kz~k=∑i=02nxWi(c)[zi,k∣k-1-z-k∣k-1][zi,k∣k-1-z-k∣k-1]T+R,(9)Pxkzk=∑i=02nxWi(c)[χi,k∣k-1-x-k∣k-1][zi,k∣k-1-z-k∣k-1]T,(10)Kk=PxkzkPz~kz~k-1,(11)x-k=x-k∣k-1+Kk(zk-z-k∣k-1),(12)Pk=Pk∣k-1-KkPz~kz~kKkT,where Q is the system noise covariance, R is the observation noise covariance, K is the Kalman gain, and Wi 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(13)xk=f(xk-1,uk-1)+Wk-1,zk=h(xk)+Vk,
where xk represents the state vector of the system at time k,  uk-1 represents the control, zk represents the measurement value of the state at time k, and Wk-1 and Vk are independent white Gaussian noise with time-varying means of qk and rk and covariances of Qk and Rk, respectively. Note that Qk is a nonnegative definite symmetric matrix, while Rk 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
(14)βi=βi-1b,0<b<1,∑i=1kβi=1
which can be written as
(15)βi=dkbi-1,i=1,…,k,dk=(1-b)(1-bk).
The recursive formula of a fading memory time-varying noise statistical estimator described by q^k, Q^k, r^k, R^k is
(16)q^k=(1-dk)q^k-1+dk[x^k-∑i=02nxWi(m)f(χi,k-1)]Q^k=(1-dk)Q^k-1+dk[∑i=02nxWi(c)(χi,k∣k-1-x^k∣k-1)KkεkεkTKkT+Pk-∑i=02nxWi(c)(χi,k∣k-1-x^k∣k-1)(χi,k∣k-1-x^k∣k-1)T∑i=02nxWi(c)(χi,k∣k-1-x^k∣k-1)],r^k=(1-dk)r^k-1+dk[zk-∑i=02nxWi(m)h(χi,k∣k-1)],R^k=(1-dk)R^k-1+dk[εkεkT-∑i=02nxWi(c)×(zi,k∣k-1-z^k∣k-1)×(zi,k∣k-1-z^k∣k-1)T∑i=02nxWi(c)],
where εk=zk-h(x^k∣k-1) 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λk through a computational fading factor formula and applies this coefficient to correct Pk|k-1; thus, the role of the observables is strengthened and the filter divergence is suppressed.The convergence conditions can be written as(17)vkTvk≤S·tr[E(vkvkT)],
where S(S≥1) is an adjustable coefficient presetting and vk is the residual sequence, such that vk=zk-h(x-k∣k-1).The correction method of covariancePk|k-1 is
(18)Pk∣k-1=λk·∑i=02nxWi(c)[χi,k∣k-1-x-k∣k-1]×[χi,k∣k-1-x-k∣k-1]T+Qk-1.The adaptive weighting coefficientλk is calculated based on the fading factor formula [14, 15]
(19)λk={λ0λ0≥11λ0<1,λ0=tr[Nk]tr[Mk],Nk=tr(C0,k-R)T,Mk=tr(∑i=02nxWi(c)[zi,k∣k-1-z-k∣k-1][zi,k∣k-1-z-k∣k-1]T),C0,k={vkvkTk=1ρC0,k+vkvkT1+ρk>1,
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 k 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)
(20)q^0=q0,Q^0=Q0.(2)Time Update. Given values for the variables x^k-1 and Pk-1, we solve x-k|k-1 and z-k∣k-1 based on the unscented transform, that is, using (4) and (7), and covariance Pk∣k-1 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 Pk∣k-1 using (18)~(19).(4)Measurement Update. Obtain observation covariances Pz~kz~k and Pxkzk and filter gain Kk 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).


## 2.2.1. System Noise Estimation Method

Suppose a class of nonlinear dynamic systems is described as(13)xk=f(xk-1,uk-1)+Wk-1,zk=h(xk)+Vk,
where xk represents the state vector of the system at time k,  uk-1 represents the control, zk represents the measurement value of the state at time k, and Wk-1 and Vk are independent white Gaussian noise with time-varying means of qk and rk and covariances of Qk and Rk, respectively. Note that Qk is a nonnegative definite symmetric matrix, while Rk 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
(14)βi=βi-1b,0<b<1,∑i=1kβi=1
which can be written as
(15)βi=dkbi-1,i=1,…,k,dk=(1-b)(1-bk).
The recursive formula of a fading memory time-varying noise statistical estimator described by q^k, Q^k, r^k, R^k is
(16)q^k=(1-dk)q^k-1+dk[x^k-∑i=02nxWi(m)f(χi,k-1)]Q^k=(1-dk)Q^k-1+dk[∑i=02nxWi(c)(χi,k∣k-1-x^k∣k-1)KkεkεkTKkT+Pk-∑i=02nxWi(c)(χi,k∣k-1-x^k∣k-1)(χi,k∣k-1-x^k∣k-1)T∑i=02nxWi(c)(χi,k∣k-1-x^k∣k-1)],r^k=(1-dk)r^k-1+dk[zk-∑i=02nxWi(m)h(χi,k∣k-1)],R^k=(1-dk)R^k-1+dk[εkεkT-∑i=02nxWi(c)×(zi,k∣k-1-z^k∣k-1)×(zi,k∣k-1-z^k∣k-1)T∑i=02nxWi(c)],
where εk=zk-h(x^k∣k-1) 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λk through a computational fading factor formula and applies this coefficient to correct Pk|k-1; thus, the role of the observables is strengthened and the filter divergence is suppressed.The convergence conditions can be written as(17)vkTvk≤S·tr[E(vkvkT)],
where S(S≥1) is an adjustable coefficient presetting and vk is the residual sequence, such that vk=zk-h(x-k∣k-1).The correction method of covariancePk|k-1 is
(18)Pk∣k-1=λk·∑i=02nxWi(c)[χi,k∣k-1-x-k∣k-1]×[χi,k∣k-1-x-k∣k-1]T+Qk-1.The adaptive weighting coefficientλk is calculated based on the fading factor formula [14, 15]
(19)λk={λ0λ0≥11λ0<1,λ0=tr[Nk]tr[Mk],Nk=tr(C0,k-R)T,Mk=tr(∑i=02nxWi(c)[zi,k∣k-1-z-k∣k-1][zi,k∣k-1-z-k∣k-1]T),C0,k={vkvkTk=1ρC0,k+vkvkT1+ρk>1,
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 k 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)
(20)q^0=q0,Q^0=Q0.(2)Time Update. Given values for the variables x^k-1 and Pk-1, we solve x-k|k-1 and z-k∣k-1 based on the unscented transform, that is, using (4) and (7), and covariance Pk∣k-1 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 Pk∣k-1 using (18)~(19).(4)Measurement Update. Obtain observation covariances Pz~kz~k and Pxkzk and filter gain Kk 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 Figure1, L is the global coordinate system established with the initial location and bow of the UUV, where x, y, and ψ describe the position and heading of the UUV within this system. While V is the UUV vessel frame system and S is the sonar coordinate system. The North-East coordinates are given by E, with its North direction based on magnetic North. Obviously,  zψ=ψ+ψL0, where zψ is the heading of the UUV as measured by its OCTANS.Figure 1
Global coordinate system, the UUV vessel frame system and the sonar coordinate system.Note that there is a distance offset (1.85 m in theX direction, 0.65 m in the Y direction, and a very small deviation in the Z direction that can be ignored) between the mounting positions of three ranging sonar and the UUV’s center of gravity. The coordinate systems S and V are shown in more detail in Figure 2. We assume that the positions of the origin of S and V in E are (Sx,Sy) and (Vx,Vy), 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 V of a=1.85m and b=0.65m, where a is the deviation in the X direction and b is the deviation in the Y 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
(21)Sx=Vx+a2+b2*cos(ϕ-zψ),Sy=Vy+a2+b2*sin(ϕ-zψ).Figure 2
Installation position and angle deviation of the ranging sonar.The method uses a simple 4 DOF (degree of freedom) constant velocity kinematics to predict how the state will evolve from timek-1 to time k:
(22)[xyzψuvwr]k=[x+uTcos(ψ)-vTsin(ψ)y+uTsin(ψ)+vTcos(ψ)z+wTψ+rTuvwr]k-1+nk,
where [x,y,z,ψ] is the position and heading of the UUV in L; [u,v,w,r] gives the line velocity and angle velocity of the UUV in V, and k is the sample time. In this equation, n(k) is the portion of the system noise with a time-varying mean and covariance, with the covariance of vector n given by system noise Q(23)E[nk]=qk,E[(nk-qk)(nj-qj)T]=δkjQk,
where δkj 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(24)Xfi,k=Xfi,k-1i=1,…,n.


#### 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(25)zk=Hxk∣k-1+mk,
where z is the observation vector, m is a white Gaussian with zero mean, and H varies with changes in the measurement:
(26)H=[0010000000010000000010000000010000000010].


#### 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 Figure3, α represents the horizontal beam width, β represents the incidence angle, and ρS is the range at which the bin was measured from the sensor. Reference frame S defines the position of the transducer head at the moment the sensor obtains a particular bin, where [xSB,ySB,θSB] is the transformation defining the position of S with respect to B to the chosen base reference. Both [xSB,ySB,θSB] and ρS are obtained from information in the data buffer.Figure 3
Ranging sonar model to distinguish line features.To emulate the effect of the horizontal beam width, the model uses a set ofi 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 θSB, where
(27)θSB-α2≤θiB≤θSB+α2.Each valueθiB 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 k value at a given resolution for each value of θiB and within an aperture of  ±β; that is,
(28)θiB-β≤θi,kB≤θiB+β.The results of this operation arei×k different values of θiB 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 ρi,kB corresponding to each one of the θi,kB bearings is
(29)ρi,kB=xSBcos(θi,kB)+ySBsin(θi,kB)+ρScos(θi,k).


### 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 systemE, 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 S, the time, the position and heading of the UUV in E, and the position and transmit angle of the sonar in E are stored into the data buffer. The third step defines the base frame B, where B is the current position of the sonar head when the voting is performed. Finally, the position and heading of the sonar instrument in B 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 B.


### 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 ZiV corresponds to any of the features Fj,j=1,…,n 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 ofB and V, the position of the ith line measurement in V can be represented by
(30)ZiV=[ρiVθiV]T.If we assume the position of the line featurej already exists in the map in L; that is,
(31)XlL=[ρjθj].Then the position of line featurej in V is
(32)XlV=[ρj-xcosθj-ysinθjθj-ψ].And line featurej corresponds to the ith line measurement, where
(33)zi,kV=hj(xk,si),hj(xk,si)=[ρj-xcosθj-ysinθjθj-ψ]+si.
Here, ρiV and θiV are the parameters of the line features in the V frame of reference, ρi and θi are the parameters of the line features presented in the L frame of reference, and si is the noise with a zero-mean white noise with covariance R affecting the line feature observation.The proposed method uses an innovation termνij to calculate the discrepancy between the measurement and its prediction, with its associate covariance matrix Sij obtained by
(34)vij,k=zi,kV-zj,k|k-1,zj,k|k-1=hj(x-k|k-1,si),Sij,k=Pz~kz~k+Rk.To determine if the correspondence is valid, an individual compatibility (IC) test [27] using the Mahalanobis distance is carried out
(35)Dij2=νij,kTSij,k-1iνij,k<χd,α2,
where d=dim(hj) 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 inV. Thus, the algorithm must first perform a change of reference using the following:
(36)X^(k)=[X^V(k)X^f1(k)⋮X^fn(k)]⟹X^(k)+=[X^V(k)X^f1(k)⋮X^fn(k)X^V(k)⊕ziV(k)].


## 3.1. SLAM System Model

### 3.1.1. AUV Nonlinear Dynamic Model

As seen from Figure1, L is the global coordinate system established with the initial location and bow of the UUV, where x, y, and ψ describe the position and heading of the UUV within this system. While V is the UUV vessel frame system and S is the sonar coordinate system. The North-East coordinates are given by E, with its North direction based on magnetic North. Obviously,  zψ=ψ+ψL0, where zψ is the heading of the UUV as measured by its OCTANS.Figure 1
Global coordinate system, the UUV vessel frame system and the sonar coordinate system.Note that there is a distance offset (1.85 m in theX direction, 0.65 m in the Y direction, and a very small deviation in the Z direction that can be ignored) between the mounting positions of three ranging sonar and the UUV’s center of gravity. The coordinate systems S and V are shown in more detail in Figure 2. We assume that the positions of the origin of S and V in E are (Sx,Sy) and (Vx,Vy), 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 V of a=1.85m and b=0.65m, where a is the deviation in the X direction and b is the deviation in the Y 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
(21)Sx=Vx+a2+b2*cos(ϕ-zψ),Sy=Vy+a2+b2*sin(ϕ-zψ).Figure 2
Installation position and angle deviation of the ranging sonar.The method uses a simple 4 DOF (degree of freedom) constant velocity kinematics to predict how the state will evolve from timek-1 to time k:
(22)[xyzψuvwr]k=[x+uTcos(ψ)-vTsin(ψ)y+uTsin(ψ)+vTcos(ψ)z+wTψ+rTuvwr]k-1+nk,
where [x,y,z,ψ] is the position and heading of the UUV in L; [u,v,w,r] gives the line velocity and angle velocity of the UUV in V, and k is the sample time. In this equation, n(k) is the portion of the system noise with a time-varying mean and covariance, with the covariance of vector n given by system noise Q(23)E[nk]=qk,E[(nk-qk)(nj-qj)T]=δkjQk,
where δkj 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(24)Xfi,k=Xfi,k-1i=1,…,n.


### 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(25)zk=Hxk∣k-1+mk,
where z is the observation vector, m is a white Gaussian with zero mean, and H varies with changes in the measurement:
(26)H=[0010000000010000000010000000010000000010].


### 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 Figure3, α represents the horizontal beam width, β represents the incidence angle, and ρS is the range at which the bin was measured from the sensor. Reference frame S defines the position of the transducer head at the moment the sensor obtains a particular bin, where [xSB,ySB,θSB] is the transformation defining the position of S with respect to B to the chosen base reference. Both [xSB,ySB,θSB] and ρS are obtained from information in the data buffer.Figure 3
Ranging sonar model to distinguish line features.To emulate the effect of the horizontal beam width, the model uses a set ofi 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 θSB, where
(27)θSB-α2≤θiB≤θSB+α2.Each valueθiB 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 k value at a given resolution for each value of θiB and within an aperture of  ±β; that is,
(28)θiB-β≤θi,kB≤θiB+β.The results of this operation arei×k different values of θiB 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 ρi,kB corresponding to each one of the θi,kB bearings is
(29)ρi,kB=xSBcos(θi,kB)+ySBsin(θi,kB)+ρScos(θi,k).


## 3.1.1. AUV Nonlinear Dynamic Model

As seen from Figure1, L is the global coordinate system established with the initial location and bow of the UUV, where x, y, and ψ describe the position and heading of the UUV within this system. While V is the UUV vessel frame system and S is the sonar coordinate system. The North-East coordinates are given by E, with its North direction based on magnetic North. Obviously,  zψ=ψ+ψL0, where zψ is the heading of the UUV as measured by its OCTANS.Figure 1
Global coordinate system, the UUV vessel frame system and the sonar coordinate system.Note that there is a distance offset (1.85 m in theX direction, 0.65 m in the Y direction, and a very small deviation in the Z direction that can be ignored) between the mounting positions of three ranging sonar and the UUV’s center of gravity. The coordinate systems S and V are shown in more detail in Figure 2. We assume that the positions of the origin of S and V in E are (Sx,Sy) and (Vx,Vy), 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 V of a=1.85m and b=0.65m, where a is the deviation in the X direction and b is the deviation in the Y 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
(21)Sx=Vx+a2+b2*cos(ϕ-zψ),Sy=Vy+a2+b2*sin(ϕ-zψ).Figure 2
Installation position and angle deviation of the ranging sonar.The method uses a simple 4 DOF (degree of freedom) constant velocity kinematics to predict how the state will evolve from timek-1 to time k:
(22)[xyzψuvwr]k=[x+uTcos(ψ)-vTsin(ψ)y+uTsin(ψ)+vTcos(ψ)z+wTψ+rTuvwr]k-1+nk,
where [x,y,z,ψ] is the position and heading of the UUV in L; [u,v,w,r] gives the line velocity and angle velocity of the UUV in V, and k is the sample time. In this equation, n(k) is the portion of the system noise with a time-varying mean and covariance, with the covariance of vector n given by system noise Q(23)E[nk]=qk,E[(nk-qk)(nj-qj)T]=δkjQk,
where δkj 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(24)Xfi,k=Xfi,k-1i=1,…,n.


## 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(25)zk=Hxk∣k-1+mk,
where z is the observation vector, m is a white Gaussian with zero mean, and H varies with changes in the measurement:
(26)H=[0010000000010000000010000000010000000010].


## 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 Figure3, α represents the horizontal beam width, β represents the incidence angle, and ρS is the range at which the bin was measured from the sensor. Reference frame S defines the position of the transducer head at the moment the sensor obtains a particular bin, where [xSB,ySB,θSB] is the transformation defining the position of S with respect to B to the chosen base reference. Both [xSB,ySB,θSB] and ρS are obtained from information in the data buffer.Figure 3
Ranging sonar model to distinguish line features.To emulate the effect of the horizontal beam width, the model uses a set ofi 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 θSB, where
(27)θSB-α2≤θiB≤θSB+α2.Each valueθiB 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 k value at a given resolution for each value of θiB and within an aperture of  ±β; that is,
(28)θiB-β≤θi,kB≤θiB+β.The results of this operation arei×k different values of θiB 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 ρi,kB corresponding to each one of the θi,kB bearings is
(29)ρi,kB=xSBcos(θi,kB)+ySBsin(θi,kB)+ρScos(θi,k).


## 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 systemE, 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 S, the time, the position and heading of the UUV in E, and the position and transmit angle of the sonar in E are stored into the data buffer. The third step defines the base frame B, where B is the current position of the sonar head when the voting is performed. Finally, the position and heading of the sonar instrument in B 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 B.


## 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 systemE, 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 S, the time, the position and heading of the UUV in E, and the position and transmit angle of the sonar in E are stored into the data buffer. The third step defines the base frame B, where B is the current position of the sonar head when the voting is performed. Finally, the position and heading of the sonar instrument in B 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 B.


## 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 ZiV corresponds to any of the features Fj,j=1,…,n 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 ofB and V, the position of the ith line measurement in V can be represented by
(30)ZiV=[ρiVθiV]T.If we assume the position of the line featurej already exists in the map in L; that is,
(31)XlL=[ρjθj].Then the position of line featurej in V is
(32)XlV=[ρj-xcosθj-ysinθjθj-ψ].And line featurej corresponds to the ith line measurement, where
(33)zi,kV=hj(xk,si),hj(xk,si)=[ρj-xcosθj-ysinθjθj-ψ]+si.
Here, ρiV and θiV are the parameters of the line features in the V frame of reference, ρi and θi are the parameters of the line features presented in the L frame of reference, and si is the noise with a zero-mean white noise with covariance R affecting the line feature observation.The proposed method uses an innovation termνij to calculate the discrepancy between the measurement and its prediction, with its associate covariance matrix Sij obtained by
(34)vij,k=zi,kV-zj,k|k-1,zj,k|k-1=hj(x-k|k-1,si),Sij,k=Pz~kz~k+Rk.To determine if the correspondence is valid, an individual compatibility (IC) test [27] using the Mahalanobis distance is carried out
(35)Dij2=νij,kTSij,k-1iνij,k<χd,α2,
where d=dim(hj) 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 inV. Thus, the algorithm must first perform a change of reference using the following:
(36)X^(k)=[X^V(k)X^f1(k)⋮X^fn(k)]⟹X^(k)+=[X^V(k)X^f1(k)⋮X^fn(k)X^V(k)⊕ziV(k)].


## 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 pointA to point B 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.Figure 4
Satellite image of the sea trial test area.The initial position of the UUV (pointA) 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.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 Figure5.Figure 5
The ranging sonar mounted in the horizontal frame.A general description of the AUKF-SLAM algorithm is given in Figure6.Figure 6
Basic principle of the AUKF-SLAM algorithm.


### 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 pointP in S is XPS=[xSyS]T. Then for each ranging sonar, we have
(37)XPS=[xSyS]=[ρi×sin(θi×π180)ρi×cos(θi×π180)],
where ρi and θi 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 (i=1), middle (i=2), and right (i=3) sonar.The relationship betweenV and S is shown in Figure 2, with the position of point P in V given by
(38)XPV=[xVyV]=[xS+ayS+b].The position ofV relative to L is XVL=[xLyLφL]T, so we can obtain the position of point P in L using the following synthesis operator
(39)XPL=XVL⊕XPV=[xL+xVcosφL-yVsinφLyL+xVsinφL+yVcosφL].Figure7 gives the embankment measurement, where the green, red, and blue points were acquired by the left, middle, and right sonar, respectively.Figure 7
Feature measurement of the embankment.


### 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 is5∘ and the distance resolution is 1 m;(b)
the largest number of votes is selected as the threshold;(c)
a10∘ 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.Table 2
Extracted line feature parameters.
Line feature
ρ
θ
Time (step)
Position of UUV in E(m)
L1
13
−0.2618
260~310
(−786.3968, −40.6424)
L2
33
0
320~370
(−949.0549, −47.3198)
L3
39
0
430~500
(−1279.0335, 47.42)
L4
15
−0.0873
570~610
(−1504.7369, 206.5273)
L5
31
−0.0873
640~720
(1449.9067, 483.5776)
L6
35
−0.2618
730~820
(−1278.9245, 649.0867)


### 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. Table3 gives the results of the system noise tests and the resulting laws, where k is the step number. The observation noise is
(40)R=diag([0.0420.0420.0420.0420.042]).Table 3
Change law of system noise.
Test
Value ofQ
Time-varying noise test
Q
k
=
{
diag
(
[
9
e
-
6
1
e
-
6
8
e
-
5
3
e
-
5
9
e
-
6
1
e
-
6
8
e
-
6
3
e
-
6
]
)
1
<
k
<
320
diag
(
[
9
e
-
5
1
e
-
5
8
e
-
3
3
e
-
3
9
e
-
4
1
e
-
4
8
e
-
5
3
e
-
5
]
)
320
≤
k
<
640
diag
(
[
9
e
-
5
1
e
-
5
8
e
-
4
3
e
-
4
9
e
-
4
1
e
-
3
8
e
-
4
3
e
-
4
]
)
640
≤
k
<
820
Constant noise test
Q
k
=
diag
(
[
5
e
-
6
5
e
-
6
5
e
-
4
9
e
-
3
2
e
-
4
2
e
-
4
4
e
-
4
4
e
-
4
]
)


#### 4.4.2. Test Results

Figures8~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.Figure 8
Comparison of the trajectory estimations with time-varying noise.Figure 9
Comparison of position error in the East direction with time-varying noise.Figure 10
Comparison of position error in the North direction with time-varying noise.Figure 11
Adaptive weighting coefficient of AUKF-SLAM algorithm in time-varying noise test.Figure 12
Comparison of the trajectory estimations with constant noise.Figure 13
Comparison of position error in the East direction with constant noise.Figure 14
Comparison of position error in the North direction with constant noise.Figure 15
Adaptive weighting coefficient of AUKF-SLAM algorithm in constant noise test.


#### 4.4.3. Analysis of Test Results

The headingzψ measured by the OCTANS is relative to the magnetic north of E coordination system shown in Figure 1, and it should be transformed to the heading ψ in the L coordinate system for state updating. The heading ψ is shown as Figure 16.Figure 16
Heading of the UUV during the field trial.(1) Performance Analysis of the AUKF-SLAM Algorithm. We calculate the estimation error of the algorithm by
(41)δk=[δe,kδn,k]=[x^k-xky^k-yk],
where δe,k and δn,k are the estimation error in the East and North directions at time k, respectively, while (xk,yk) and (x^k,y^k) are the true position and estimated position of the UUV at time k. Note that |δe,k| and |δn,k| represent absolute values of the error in the East and North directions at time k, respectively. Obviously, small values of |δe| and |δn| indicate higher accuracy in the filtering algorithm. Figures 8~10 and Figures 11~14 show that |δe| and |δn| 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 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
(42)RMSEpos=1T∑k=1T((xk-x^k)2+(yk-y^k)2),
where T is the total running step and (xk,yk) and (x^k,y^k) are the true position and estimated position, respectively, of the UUV at time k. 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.Table 4
Comparison of RMSE.
Algorithm
Time-varying noise test (m)
Constant noise test (m)
AUKF-SLAM
26.1487
28.5021
UKF-SLAM
28.0639
29.4876
EKF-SLAM
38.5579
39.8530From 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.Figure17 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.Figure 17
Comparison of line features extracted from the environment and the feature measurement of the embankment.Figure 18
SLAM diagram based on the AUKF.


## 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 pointA to point B 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.Figure 4
Satellite image of the sea trial test area.The initial position of the UUV (pointA) 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.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 Figure5.Figure 5
The ranging sonar mounted in the horizontal frame.A general description of the AUKF-SLAM algorithm is given in Figure6.Figure 6
Basic principle of the AUKF-SLAM algorithm.


## 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 pointP in S is XPS=[xSyS]T. Then for each ranging sonar, we have
(37)XPS=[xSyS]=[ρi×sin(θi×π180)ρi×cos(θi×π180)],
where ρi and θi 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 (i=1), middle (i=2), and right (i=3) sonar.The relationship betweenV and S is shown in Figure 2, with the position of point P in V given by
(38)XPV=[xVyV]=[xS+ayS+b].The position ofV relative to L is XVL=[xLyLφL]T, so we can obtain the position of point P in L using the following synthesis operator
(39)XPL=XVL⊕XPV=[xL+xVcosφL-yVsinφLyL+xVsinφL+yVcosφL].Figure7 gives the embankment measurement, where the green, red, and blue points were acquired by the left, middle, and right sonar, respectively.Figure 7
Feature measurement of the embankment.


## 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 is5∘ and the distance resolution is 1 m;(b)
the largest number of votes is selected as the threshold;(c)
a10∘ 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.Table 2
Extracted line feature parameters.
Line feature
ρ
θ
Time (step)
Position of UUV in E(m)
L1
13
−0.2618
260~310
(−786.3968, −40.6424)
L2
33
0
320~370
(−949.0549, −47.3198)
L3
39
0
430~500
(−1279.0335, 47.42)
L4
15
−0.0873
570~610
(−1504.7369, 206.5273)
L5
31
−0.0873
640~720
(1449.9067, 483.5776)
L6
35
−0.2618
730~820
(−1278.9245, 649.0867)


## 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. Table3 gives the results of the system noise tests and the resulting laws, where k is the step number. The observation noise is
(40)R=diag([0.0420.0420.0420.0420.042]).Table 3
Change law of system noise.
Test
Value ofQ
Time-varying noise test
Q
k
=
{
diag
(
[
9
e
-
6
1
e
-
6
8
e
-
5
3
e
-
5
9
e
-
6
1
e
-
6
8
e
-
6
3
e
-
6
]
)
1
<
k
<
320
diag
(
[
9
e
-
5
1
e
-
5
8
e
-
3
3
e
-
3
9
e
-
4
1
e
-
4
8
e
-
5
3
e
-
5
]
)
320
≤
k
<
640
diag
(
[
9
e
-
5
1
e
-
5
8
e
-
4
3
e
-
4
9
e
-
4
1
e
-
3
8
e
-
4
3
e
-
4
]
)
640
≤
k
<
820
Constant noise test
Q
k
=
diag
(
[
5
e
-
6
5
e
-
6
5
e
-
4
9
e
-
3
2
e
-
4
2
e
-
4
4
e
-
4
4
e
-
4
]
)


### 4.4.2. Test Results

Figures8~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.Figure 8
Comparison of the trajectory estimations with time-varying noise.Figure 9
Comparison of position error in the East direction with time-varying noise.Figure 10
Comparison of position error in the North direction with time-varying noise.Figure 11
Adaptive weighting coefficient of AUKF-SLAM algorithm in time-varying noise test.Figure 12
Comparison of the trajectory estimations with constant noise.Figure 13
Comparison of position error in the East direction with constant noise.Figure 14
Comparison of position error in the North direction with constant noise.Figure 15
Adaptive weighting coefficient of AUKF-SLAM algorithm in constant noise test.


### 4.4.3. Analysis of Test Results

The headingzψ measured by the OCTANS is relative to the magnetic north of E coordination system shown in Figure 1, and it should be transformed to the heading ψ in the L coordinate system for state updating. The heading ψ is shown as Figure 16.Figure 16
Heading of the UUV during the field trial.(1) Performance Analysis of the AUKF-SLAM Algorithm. We calculate the estimation error of the algorithm by
(41)δk=[δe,kδn,k]=[x^k-xky^k-yk],
where δe,k and δn,k are the estimation error in the East and North directions at time k, respectively, while (xk,yk) and (x^k,y^k) are the true position and estimated position of the UUV at time k. Note that |δe,k| and |δn,k| represent absolute values of the error in the East and North directions at time k, respectively. Obviously, small values of |δe| and |δn| indicate higher accuracy in the filtering algorithm. Figures 8~10 and Figures 11~14 show that |δe| and |δn| 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 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
(42)RMSEpos=1T∑k=1T((xk-x^k)2+(yk-y^k)2),
where T is the total running step and (xk,yk) and (x^k,y^k) are the true position and estimated position, respectively, of the UUV at time k. 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.Table 4
Comparison of RMSE.
Algorithm
Time-varying noise test (m)
Constant noise test (m)
AUKF-SLAM
26.1487
28.5021
UKF-SLAM
28.0639
29.4876
EKF-SLAM
38.5579
39.8530From 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.Figure17 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.Figure 17
Comparison of line features extracted from the environment and the feature measurement of the embankment.Figure 18
SLAM diagram based on the AUKF.


## 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. Table3 gives the results of the system noise tests and the resulting laws, where k is the step number. The observation noise is
(40)R=diag([0.0420.0420.0420.0420.042]).Table 3
Change law of system noise.
Test
Value ofQ
Time-varying noise test
Q
k
=
{
diag
(
[
9
e
-
6
1
e
-
6
8
e
-
5
3
e
-
5
9
e
-
6
1
e
-
6
8
e
-
6
3
e
-
6
]
)
1
<
k
<
320
diag
(
[
9
e
-
5
1
e
-
5
8
e
-
3
3
e
-
3
9
e
-
4
1
e
-
4
8
e
-
5
3
e
-
5
]
)
320
≤
k
<
640
diag
(
[
9
e
-
5
1
e
-
5
8
e
-
4
3
e
-
4
9
e
-
4
1
e
-
3
8
e
-
4
3
e
-
4
]
)
640
≤
k
<
820
Constant noise test
Q
k
=
diag
(
[
5
e
-
6
5
e
-
6
5
e
-
4
9
e
-
3
2
e
-
4
2
e
-
4
4
e
-
4
4
e
-
4
]
)


## 4.4.2. Test Results

Figures8~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.Figure 8
Comparison of the trajectory estimations with time-varying noise.Figure 9
Comparison of position error in the East direction with time-varying noise.Figure 10
Comparison of position error in the North direction with time-varying noise.Figure 11
Adaptive weighting coefficient of AUKF-SLAM algorithm in time-varying noise test.Figure 12
Comparison of the trajectory estimations with constant noise.Figure 13
Comparison of position error in the East direction with constant noise.Figure 14
Comparison of position error in the North direction with constant noise.Figure 15
Adaptive weighting coefficient of AUKF-SLAM algorithm in constant noise test.


## 4.4.3. Analysis of Test Results

The headingzψ measured by the OCTANS is relative to the magnetic north of E coordination system shown in Figure 1, and it should be transformed to the heading ψ in the L coordinate system for state updating. The heading ψ is shown as Figure 16.Figure 16
Heading of the UUV during the field trial.(1) Performance Analysis of the AUKF-SLAM Algorithm. We calculate the estimation error of the algorithm by
(41)δk=[δe,kδn,k]=[x^k-xky^k-yk],
where δe,k and δn,k are the estimation error in the East and North directions at time k, respectively, while (xk,yk) and (x^k,y^k) are the true position and estimated position of the UUV at time k. Note that |δe,k| and |δn,k| represent absolute values of the error in the East and North directions at time k, respectively. Obviously, small values of |δe| and |δn| indicate higher accuracy in the filtering algorithm. Figures 8~10 and Figures 11~14 show that |δe| and |δn| 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 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
(42)RMSEpos=1T∑k=1T((xk-x^k)2+(yk-y^k)2),
where T is the total running step and (xk,yk) and (x^k,y^k) are the true position and estimated position, respectively, of the UUV at time k. 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.Table 4
Comparison of RMSE.
Algorithm
Time-varying noise test (m)
Constant noise test (m)
AUKF-SLAM
26.1487
28.5021
UKF-SLAM
28.0639
29.4876
EKF-SLAM
38.5579
39.8530From 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.Figure17 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.Figure 17
Comparison of line features extracted from the environment and the feature measurement of the embankment.Figure 18
SLAM diagram based on the AUKF.


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



---

*Source: 605981-2013-11-06.xml*