# AUV-Based Plume Tracking: A Simulation Study

Authors: Awantha Jayasiri; Raymond G. Gosine; George K. I. Mann; Peter McGuire
Journal: Journal of Control Science and Engineering
 (2016)

---

## Abstract

This paper presents a simulation study of an autonomous underwater vehicle (AUV) navigation system operating in a GPS-denied environment. The AUV navigation method makes use of underwater transponder positioning and requires only one transponder. A multirate unscented Kalman filter is used to determine the AUV orientation and position by fusing high-rate sensor data and low-rate information. The paper also proposes a gradient-based, efficient, and adaptive novel algorithm for plume boundary tracking missions. The algorithm follows a centralized approach and it includes path optimization features based on gradient information. The proposed algorithm is implemented in simulation on the AUV-based navigation system and successful boundary tracking results are obtained.


---

## Body

## 1. Introduction

Most plume tracking and detection systems reported in the literature are based on surface dynamic oceanographic features. These are detected by satellites and their images are preprocessed for selecting regions of interest to generate optimal tracking sequences [1, 2]. As automated data collection is becoming more prevalent, optimum path planning and trajectory designs for autonomous underwater vehicles (AUVs) are becoming more important, since those planning approaches are required to navigate the AUV for collecting information. In order to track evolving features of interest in the ocean using predictive ocean models, several waypoint selection algorithms are developed and experimentally tested in [3]. Design and control of trajectories for AUVs to obtain optimal data collection are presented in [4]. The AUVs rely on GPS data for accurate position fixes. However, due to the dielectric contrast and high dielectric loss factor of seawater compared to air, most of the strength of the GPS signal is reflected back or attenuated and the AUV has to surface occasionally for position update. Therefore, in situations like under-ice oil spills and deep-sea exploration, the detection and tracking tasks need to be performed without GPS support and rely on a very low frequency acoustic communication channel.A review of the state-of-the-art AUV navigation techniques is presented in [5, 6], along with a brief comparison of their mission-based suitability. The methods discussed include inertial, acoustic, and geophysical AUV navigation. To this extent, obtaining GPS-based surface fixes as well as utilizing a Long Base Line (LBL), Short Base Line (SBL), or Ultra Short Base Line (USBL) system has been the standard practice for AUV navigation [7]. These systems have similar deployment and transponder positioning challenges. Inverted USBL configuration, where the USBL array is located on the vehicle, interrogates transponders placed in known positions [7]. The study reported in [8] discusses underwater transponder positioning (UTP), which requires only one transducer due to the tight coupling with the vehicle’s Inertial Navigation System (INS), and serves as an alternative approach.In this paper, we use the inverted USBL configuration, which enables AUV positioning to be single-referenced and hence greatly reduces the complex operational logistics. A GPS-capable transponder, which is suspended from surface ice or a platform, is considered. The transponder transmits its position with each ping to the AUV and provides a means to position update in a GPS-denied undersea environment. It is also assumed that several receivers are placed along the body of the AUV on a noncoplanar configuration, keeping a sufficient space between each of the receivers. The transponder and AUV’s clocks are synchronized and the transponder broadcasts a unique signal with a known delay [9, 10]. Upon arrival of the transmitted signal to the AUV, a single trip travel time for each of the receivers is recorded. The recorded travel times are then used for the measurement update in state estimation. In between transponder broadcasts, short-term dead-reckoning is utilized based on high-rate INS data.In operation conditions such as rough bathymetry or limited sensor range, where DVL bottom-track data is unavailable, the AUV’s dynamic model-based position information is incorporated for position estimation [11–13]. A Linear Quadratic Regulator (LQR) is implemented based on the estimated states and further improved by adding a Proportional-Integral (PI) Controller for rudder control. The proposed approach is implemented in the nonlinear dynamic model of the REMUS AUV (Figure 1) presented in [14].Figure 1
REMUS AUV (image credit: Kongsberg Maritime).The paper has the following contributions:(1)
a multirate unscented Kalman filter is employed for sensor fusion in AUV localization;
(2)
a novel efficient adaptive plume boundary tracking algorithm is developed using gradient information;
(3)
numerous simulation results are presented to verify the approach.The rest of the paper is organized as follows. In Section2, an introduction on sensor package is given and the AUV navigation algorithm is presented. In Section 3, simulated results of AUV navigation are discussed. In Section 4, a plume boundary tracking algorithm is presented with its simulation results. The paper concludes by reporting the future research directions. A preliminary version of this paper appears in proceedings of the 27th Canadian Conference on Electrical and Computer Engineering (CCECE) [15]. In this version we focused on providing a detailed description of the work and more simulation results.


## 2. Sensor and System Modeling

### 2.1. Preliminaries and Notation

According to [16], n
(
=
6
) Degrees Of Freedom (DOF) model of AUV dynamics and kinematics can be derived as(1)
F
=
B
v
u
i
n
-
C
v
v
+
D
v
v
+
g
q
=
M
v
˙
(2)
η
˙
=
J
q
v
,where M
n
×
n matrix represents the inertia of the vehicle and hydrodynamic added-mass; C
n
×
n matrix includes rigid body Coriolis and centrifugal components as well as added-mass derivatives corresponding to the velocity coupling; D
n
×
n matrix includes energy dissipative terms due to relative motion between vehicle and surrounding fluid; g
n
×
1 combines gravitational and buoyancy forces; and B
n
×
m is the thruster control matrix, where m is the number of thrusters. Furthermore, v
=
[
u
T
ω
T
]
T
∈
R
6, where u
=
[
u
1
u
2
u
3
]
T and ω
=
[
ω
1
ω
2
ω
3
]
T are body-fixed linear and angular velocities. The total force matrix is represented as F. Also, η
=
[
p
T
q
T
]
T, where p
=
[
x
y
z
]
T is the position vector relative to the inertial reference frame origin and q
=
[
ϕ
θ
ψ
]
T is the vector of Euler angles, which are roll, pitch, and yaw, respectively. u
i
n
∈
R
m is the control input of the thrusters. The Jacobian J
=
J
1
(
q
)
0
3
×
3
0
3
×
3
J
2
(
q
), where J
1 is the coordinate transform matrix from u to p
˙ and J
2 relates ω to Euler rate vector q
˙. These are computed as follows:(3)
J
1
=
c
ψ
c
θ
-
s
ψ
c
ϕ
+
c
ψ
s
θ
s
ϕ
s
ψ
s
ϕ
+
c
ψ
c
ϕ
s
θ
s
ψ
c
θ
c
ψ
c
ϕ
+
s
ϕ
s
θ
s
ψ
-
c
ψ
s
ϕ
+
s
θ
s
ψ
c
ϕ
-
s
θ
c
θ
s
ϕ
c
θ
c
ϕ
J
2
=
1
s
ϕ
t
θ
c
ϕ
t
θ
0
c
ϕ
-
s
ϕ
0
s
ϕ
c
θ
c
ϕ
c
θ
,where s
⇒
s
i
n, c
⇒
c
o
s, and t
⇒
t
a
n, with 0
≤
ϕ
<
2
π, -
π
/
2
<
θ
<
π
/
2, and 0
≤
ψ
<
2
π. We assume that θ
≠
±
π
/
2 as otherwise J
2 approaches a singularity. If such an operation is required, quaternion-based attitude representation can be adopted as in [17]. The coefficient matrices we used in this work are based on the REMUS AUV dynamic model [14]. Our AUV has a typical set of sensors as mentioned in Table 1. The noise in each acoustic receiver is the quantization error introduced by the analog to digital converter of the system [18]. The transponder emits signals at 1 Hz and the control loop runs at 100 Hz. The AUV navigation system including USBL array is depicted in Figure 2.Table 1
Sensor characteristics.
Sensor type
Measurand
Frequency
Noise,σ
Accelerometer
Specific force
100 Hz
0.1 m/s2
Gyroscope
Angular velocity
100 Hz
0.005 rad/s
Attitude sensors 1 and 2
Roll & pitch
10 Hz
0.05 rad
Attitude sensor 3
Yaw
10 Hz
0.2 rad
Pressure sensor
Pressure
5 Hz
200 Pa
DVL
Linear velocity
5 Hz
0.05 m/s
Acoustic receivers
Time delay
500 kHz
1
/
3 μsFigure 2
AUV navigation system adopted from [19].


### 2.2. Sensor Modeling

#### 2.2.1. IMU Measurements

The Inertial Measurement Unit (IMU) sensor consists of accelerometers and gyroscopes (gyros), which measure specific forces and angular rates in the body-fixed coordinate system. The accelerometer readingZ
a can be modeled as [20](4)
Z
a
=
u
˙
+
ω
×
u
+
J
1
T
g
→
+
b
a
+
n
a
,where ω
× is the skew symmetric matrix cross product form of the vector ω, which is given as(5)
ω
×
=
0
-
ω
3
ω
2
ω
3
0
-
ω
1
-
ω
2
ω
1
0
,where g
→
=
[
0
0
9.81
m
/
s
2
]
T is the gravity vector and b
a
=
0.05
m
/
s
2 is the bias of the reading with b
˙
a
=
0. The measurement noise is distributed as n
a
~
N
(
0
,
σ
a
2
I
3
), with σ
a
2 as the variance for each direction. The gyro reading Z
g is modeled as [20](6)
Z
g
=
ω
+
b
g
+
n
g
,where b
g
=
0.01
r
a
d
/
s is the bias of the reading with b
˙
g
=
0. The noise of the reading is distributed as n
g
~
N
(
0
,
σ
g
2
I
3
), with σ
g
2 as the variance on each axis.


#### 2.2.2. Attitude Measurements

Attitude sensors composed of magnetometers and compasses measure roll, pitch, and yaw angles. These sensors are modeled asZ
ϕ
θ
ψ [20]:(7)
Z
ϕ
θ
ψ
=
q
+
n
ϕ
θ
ψ
,where the sensor noise is distributed as n
ϕ
θ
ψ
~
N
(
0
,
Q
ϕ
θ
ψ
) with Q
ϕ
θ
ψ
=
diag
⁡
(
σ
ϕ
2
,
σ
θ
2
,
σ
ψ
2
) having σ
ϕ
2
,
σ
θ
2
,
σ
ψ
2 as the variances of ϕ
,
θ
,
ψ measurements.


#### 2.2.3. Pressure Sensor Reading

A model for the pressure sensor is given as [20](8)
Z
p
r
=
ρ
s
w
g
→
T
p
+
J
1
l
p
r
+
n
p
r
,where Z
p
r is the sensor reading, ρ
s
w is the seawater density, and l
p
r is the sensor location in body frame. The sensor noise is distributed as n
p
r
~
N
(
0
,
σ
p
r
2
) with σ
p
r
2 as the variance.


#### 2.2.4. DVL Sensor Reading

The DVL update (in processed form) is modeled as(9)
Z
d
v
l
=
u
+
n
d
v
l
,where Z
d
v
l is the DVL reading and the sensor noise is distributed as n
d
v
l
~
N
(
0
,
Q
d
v
l
) with Q
d
v
l
=
σ
d
v
l
2
I
3 having σ
d
v
l
2 as the variance in each direction.


#### 2.2.5. Velocity Estimation Using Dynamic Model

When DVL bottom-track data is unavailable, the body-fixed linear velocities are calculated using AUV’s nonlinear dynamic model and used in the measurement update process:(10)
Z
m
o
d
=
u
+
n
m
o
d
,where Z
m
o
d is an estimate for u calculated using (1) and n
m
o
d
~
N
(
0
,
Q
m
o
d
). The noise matrix Q
m
o
d for this estimate can be calculated recursively as(11)
Q
m
o
d
=
G
t
Q
m
o
d
G
t
T
+
V
t
M
t
V
t
T
,where G
t
=
I
3
+
M
-
1
∂
F
/
∂
u
d
t and V
t
=
∂
u
/
∂
u
i
n. The matrix M
t
=
0.01
I
3 represents the noise in control space, which includes the noises in rudder and elevator angle control and the noise in thruster force.Therefore, the linear velocity measurementZ
u
=
Z
d
v
l or Z
m
o
d and its noise covariance Q
u
=
Q
d
v
l or Q
m
o
d accordingly.


#### 2.2.6. Time of Arrival Measured Using USBL

The time of arrival (TOA) of the acoustic wave from the transponder to thei
t
h receiver r
i
(
i
=
1
,
…
,
5
) is modeled as follows:(12)
Z
t
i
=
f
p
+
J
1
l
r
i
-
T
r
x
y
z
/
V
s
f
+
n
r
i
,where Z
t
i is the counted reading, f is the sampling frequency, l
r
i is the position of r
i on the AUV body, T
r
x
y
z is the transponder position, V
s is the speed of sound in the seawater, and n
r
i is the quantization noise. Note that n
r
i
~
U
(
-
1
/
(
2
f
)
,
1
/
(
2
f
)
). In this work we assume V
s
=
1500
m
/
s. However, in practical implementations V
s can be computed using Conductivity Temperature Depth (CTD) sensor readings [21]. Furthermore, we assume that TOA is disturbed by a noise of n
t
i
~
N
(
0
,
Q
t
) with Q
t
=
σ
T
O
A
2
I
5 and σ
T
O
A
=
1
/
3
μ
s, having σ
T
O
A
2 as the noise variance.A tightly coupled approach was adopted to infer the range and angle of arrival information. This is performed by feeding an array of TOA measurements to the estimation program. LetZ
t be the vector that includes all TOA measurements as(13)
Z
t
=
Z
t
1
,
…
,
Z
t
5
T
.


### 2.3. System Modeling

Incorporating the kinematics and measurements, the system equations can be written as follows [20]:(14)
q
˙
=
J
2
Z
g
-
b
g
,
b
˙
g
=
0
,
p
˙
=
J
1
u
,
u
˙
=
Z
a
-
ω
×
u
-
J
1
T
g
→
-
b
a
,
b
˙
a
=
0
.The state vector consists of 15 states:(15)
y
=
q
T
b
g
T
p
T
u
T
b
a
T
T
.The process noise covariance matrix is given as(16)
R
=
J
2
σ
g
2
I
3
J
2
T
d
t
2
0
0
0
0
0
0
J
1
σ
u
2
J
1
T
d
t
2
0
0
σ
a
2
I
3
d
t
2
0
0
0
0
.The measurement model is written as Z
=
h
+
Q, where Z is the measurement vector, h is the measurement function, and Q is the measurement noise covariance. They are given as follows:(17)
Z
=
Z
p
r
Z
t
Z
ϕ
θ
ψ
Z
u
T
,
h
=
ρ
s
w
g
→
T
p
+
J
1
l
p
r
f
p
+
J
1
l
r
i
-
T
r
x
y
z
/
V
s
f
,
i
∈
1,5
=
h
1
q
=
h
2
u
=
h
3
,
Q
=
σ
p
r
2
0
0
0
0
Q
t
0
0
0
0
Q
ϕ
θ
ψ
0
0
0
0
Q
u
.


## 2.1. Preliminaries and Notation

According to [16], n
(
=
6
) Degrees Of Freedom (DOF) model of AUV dynamics and kinematics can be derived as(1)
F
=
B
v
u
i
n
-
C
v
v
+
D
v
v
+
g
q
=
M
v
˙
(2)
η
˙
=
J
q
v
,where M
n
×
n matrix represents the inertia of the vehicle and hydrodynamic added-mass; C
n
×
n matrix includes rigid body Coriolis and centrifugal components as well as added-mass derivatives corresponding to the velocity coupling; D
n
×
n matrix includes energy dissipative terms due to relative motion between vehicle and surrounding fluid; g
n
×
1 combines gravitational and buoyancy forces; and B
n
×
m is the thruster control matrix, where m is the number of thrusters. Furthermore, v
=
[
u
T
ω
T
]
T
∈
R
6, where u
=
[
u
1
u
2
u
3
]
T and ω
=
[
ω
1
ω
2
ω
3
]
T are body-fixed linear and angular velocities. The total force matrix is represented as F. Also, η
=
[
p
T
q
T
]
T, where p
=
[
x
y
z
]
T is the position vector relative to the inertial reference frame origin and q
=
[
ϕ
θ
ψ
]
T is the vector of Euler angles, which are roll, pitch, and yaw, respectively. u
i
n
∈
R
m is the control input of the thrusters. The Jacobian J
=
J
1
(
q
)
0
3
×
3
0
3
×
3
J
2
(
q
), where J
1 is the coordinate transform matrix from u to p
˙ and J
2 relates ω to Euler rate vector q
˙. These are computed as follows:(3)
J
1
=
c
ψ
c
θ
-
s
ψ
c
ϕ
+
c
ψ
s
θ
s
ϕ
s
ψ
s
ϕ
+
c
ψ
c
ϕ
s
θ
s
ψ
c
θ
c
ψ
c
ϕ
+
s
ϕ
s
θ
s
ψ
-
c
ψ
s
ϕ
+
s
θ
s
ψ
c
ϕ
-
s
θ
c
θ
s
ϕ
c
θ
c
ϕ
J
2
=
1
s
ϕ
t
θ
c
ϕ
t
θ
0
c
ϕ
-
s
ϕ
0
s
ϕ
c
θ
c
ϕ
c
θ
,where s
⇒
s
i
n, c
⇒
c
o
s, and t
⇒
t
a
n, with 0
≤
ϕ
<
2
π, -
π
/
2
<
θ
<
π
/
2, and 0
≤
ψ
<
2
π. We assume that θ
≠
±
π
/
2 as otherwise J
2 approaches a singularity. If such an operation is required, quaternion-based attitude representation can be adopted as in [17]. The coefficient matrices we used in this work are based on the REMUS AUV dynamic model [14]. Our AUV has a typical set of sensors as mentioned in Table 1. The noise in each acoustic receiver is the quantization error introduced by the analog to digital converter of the system [18]. The transponder emits signals at 1 Hz and the control loop runs at 100 Hz. The AUV navigation system including USBL array is depicted in Figure 2.Table 1
Sensor characteristics.
Sensor type
Measurand
Frequency
Noise,σ
Accelerometer
Specific force
100 Hz
0.1 m/s2
Gyroscope
Angular velocity
100 Hz
0.005 rad/s
Attitude sensors 1 and 2
Roll & pitch
10 Hz
0.05 rad
Attitude sensor 3
Yaw
10 Hz
0.2 rad
Pressure sensor
Pressure
5 Hz
200 Pa
DVL
Linear velocity
5 Hz
0.05 m/s
Acoustic receivers
Time delay
500 kHz
1
/
3 μsFigure 2
AUV navigation system adopted from [19].


## 2.2. Sensor Modeling

### 2.2.1. IMU Measurements

The Inertial Measurement Unit (IMU) sensor consists of accelerometers and gyroscopes (gyros), which measure specific forces and angular rates in the body-fixed coordinate system. The accelerometer readingZ
a can be modeled as [20](4)
Z
a
=
u
˙
+
ω
×
u
+
J
1
T
g
→
+
b
a
+
n
a
,where ω
× is the skew symmetric matrix cross product form of the vector ω, which is given as(5)
ω
×
=
0
-
ω
3
ω
2
ω
3
0
-
ω
1
-
ω
2
ω
1
0
,where g
→
=
[
0
0
9.81
m
/
s
2
]
T is the gravity vector and b
a
=
0.05
m
/
s
2 is the bias of the reading with b
˙
a
=
0. The measurement noise is distributed as n
a
~
N
(
0
,
σ
a
2
I
3
), with σ
a
2 as the variance for each direction. The gyro reading Z
g is modeled as [20](6)
Z
g
=
ω
+
b
g
+
n
g
,where b
g
=
0.01
r
a
d
/
s is the bias of the reading with b
˙
g
=
0. The noise of the reading is distributed as n
g
~
N
(
0
,
σ
g
2
I
3
), with σ
g
2 as the variance on each axis.


### 2.2.2. Attitude Measurements

Attitude sensors composed of magnetometers and compasses measure roll, pitch, and yaw angles. These sensors are modeled asZ
ϕ
θ
ψ [20]:(7)
Z
ϕ
θ
ψ
=
q
+
n
ϕ
θ
ψ
,where the sensor noise is distributed as n
ϕ
θ
ψ
~
N
(
0
,
Q
ϕ
θ
ψ
) with Q
ϕ
θ
ψ
=
diag
⁡
(
σ
ϕ
2
,
σ
θ
2
,
σ
ψ
2
) having σ
ϕ
2
,
σ
θ
2
,
σ
ψ
2 as the variances of ϕ
,
θ
,
ψ measurements.


### 2.2.3. Pressure Sensor Reading

A model for the pressure sensor is given as [20](8)
Z
p
r
=
ρ
s
w
g
→
T
p
+
J
1
l
p
r
+
n
p
r
,where Z
p
r is the sensor reading, ρ
s
w is the seawater density, and l
p
r is the sensor location in body frame. The sensor noise is distributed as n
p
r
~
N
(
0
,
σ
p
r
2
) with σ
p
r
2 as the variance.


### 2.2.4. DVL Sensor Reading

The DVL update (in processed form) is modeled as(9)
Z
d
v
l
=
u
+
n
d
v
l
,where Z
d
v
l is the DVL reading and the sensor noise is distributed as n
d
v
l
~
N
(
0
,
Q
d
v
l
) with Q
d
v
l
=
σ
d
v
l
2
I
3 having σ
d
v
l
2 as the variance in each direction.


### 2.2.5. Velocity Estimation Using Dynamic Model

When DVL bottom-track data is unavailable, the body-fixed linear velocities are calculated using AUV’s nonlinear dynamic model and used in the measurement update process:(10)
Z
m
o
d
=
u
+
n
m
o
d
,where Z
m
o
d is an estimate for u calculated using (1) and n
m
o
d
~
N
(
0
,
Q
m
o
d
). The noise matrix Q
m
o
d for this estimate can be calculated recursively as(11)
Q
m
o
d
=
G
t
Q
m
o
d
G
t
T
+
V
t
M
t
V
t
T
,where G
t
=
I
3
+
M
-
1
∂
F
/
∂
u
d
t and V
t
=
∂
u
/
∂
u
i
n. The matrix M
t
=
0.01
I
3 represents the noise in control space, which includes the noises in rudder and elevator angle control and the noise in thruster force.Therefore, the linear velocity measurementZ
u
=
Z
d
v
l or Z
m
o
d and its noise covariance Q
u
=
Q
d
v
l or Q
m
o
d accordingly.


### 2.2.6. Time of Arrival Measured Using USBL

The time of arrival (TOA) of the acoustic wave from the transponder to thei
t
h receiver r
i
(
i
=
1
,
…
,
5
) is modeled as follows:(12)
Z
t
i
=
f
p
+
J
1
l
r
i
-
T
r
x
y
z
/
V
s
f
+
n
r
i
,where Z
t
i is the counted reading, f is the sampling frequency, l
r
i is the position of r
i on the AUV body, T
r
x
y
z is the transponder position, V
s is the speed of sound in the seawater, and n
r
i is the quantization noise. Note that n
r
i
~
U
(
-
1
/
(
2
f
)
,
1
/
(
2
f
)
). In this work we assume V
s
=
1500
m
/
s. However, in practical implementations V
s can be computed using Conductivity Temperature Depth (CTD) sensor readings [21]. Furthermore, we assume that TOA is disturbed by a noise of n
t
i
~
N
(
0
,
Q
t
) with Q
t
=
σ
T
O
A
2
I
5 and σ
T
O
A
=
1
/
3
μ
s, having σ
T
O
A
2 as the noise variance.A tightly coupled approach was adopted to infer the range and angle of arrival information. This is performed by feeding an array of TOA measurements to the estimation program. LetZ
t be the vector that includes all TOA measurements as(13)
Z
t
=
Z
t
1
,
…
,
Z
t
5
T
.


## 2.2.1. IMU Measurements

The Inertial Measurement Unit (IMU) sensor consists of accelerometers and gyroscopes (gyros), which measure specific forces and angular rates in the body-fixed coordinate system. The accelerometer readingZ
a can be modeled as [20](4)
Z
a
=
u
˙
+
ω
×
u
+
J
1
T
g
→
+
b
a
+
n
a
,where ω
× is the skew symmetric matrix cross product form of the vector ω, which is given as(5)
ω
×
=
0
-
ω
3
ω
2
ω
3
0
-
ω
1
-
ω
2
ω
1
0
,where g
→
=
[
0
0
9.81
m
/
s
2
]
T is the gravity vector and b
a
=
0.05
m
/
s
2 is the bias of the reading with b
˙
a
=
0. The measurement noise is distributed as n
a
~
N
(
0
,
σ
a
2
I
3
), with σ
a
2 as the variance for each direction. The gyro reading Z
g is modeled as [20](6)
Z
g
=
ω
+
b
g
+
n
g
,where b
g
=
0.01
r
a
d
/
s is the bias of the reading with b
˙
g
=
0. The noise of the reading is distributed as n
g
~
N
(
0
,
σ
g
2
I
3
), with σ
g
2 as the variance on each axis.


## 2.2.2. Attitude Measurements

Attitude sensors composed of magnetometers and compasses measure roll, pitch, and yaw angles. These sensors are modeled asZ
ϕ
θ
ψ [20]:(7)
Z
ϕ
θ
ψ
=
q
+
n
ϕ
θ
ψ
,where the sensor noise is distributed as n
ϕ
θ
ψ
~
N
(
0
,
Q
ϕ
θ
ψ
) with Q
ϕ
θ
ψ
=
diag
⁡
(
σ
ϕ
2
,
σ
θ
2
,
σ
ψ
2
) having σ
ϕ
2
,
σ
θ
2
,
σ
ψ
2 as the variances of ϕ
,
θ
,
ψ measurements.


## 2.2.3. Pressure Sensor Reading

A model for the pressure sensor is given as [20](8)
Z
p
r
=
ρ
s
w
g
→
T
p
+
J
1
l
p
r
+
n
p
r
,where Z
p
r is the sensor reading, ρ
s
w is the seawater density, and l
p
r is the sensor location in body frame. The sensor noise is distributed as n
p
r
~
N
(
0
,
σ
p
r
2
) with σ
p
r
2 as the variance.


## 2.2.4. DVL Sensor Reading

The DVL update (in processed form) is modeled as(9)
Z
d
v
l
=
u
+
n
d
v
l
,where Z
d
v
l is the DVL reading and the sensor noise is distributed as n
d
v
l
~
N
(
0
,
Q
d
v
l
) with Q
d
v
l
=
σ
d
v
l
2
I
3 having σ
d
v
l
2 as the variance in each direction.


## 2.2.5. Velocity Estimation Using Dynamic Model

When DVL bottom-track data is unavailable, the body-fixed linear velocities are calculated using AUV’s nonlinear dynamic model and used in the measurement update process:(10)
Z
m
o
d
=
u
+
n
m
o
d
,where Z
m
o
d is an estimate for u calculated using (1) and n
m
o
d
~
N
(
0
,
Q
m
o
d
). The noise matrix Q
m
o
d for this estimate can be calculated recursively as(11)
Q
m
o
d
=
G
t
Q
m
o
d
G
t
T
+
V
t
M
t
V
t
T
,where G
t
=
I
3
+
M
-
1
∂
F
/
∂
u
d
t and V
t
=
∂
u
/
∂
u
i
n. The matrix M
t
=
0.01
I
3 represents the noise in control space, which includes the noises in rudder and elevator angle control and the noise in thruster force.Therefore, the linear velocity measurementZ
u
=
Z
d
v
l or Z
m
o
d and its noise covariance Q
u
=
Q
d
v
l or Q
m
o
d accordingly.


## 2.2.6. Time of Arrival Measured Using USBL

The time of arrival (TOA) of the acoustic wave from the transponder to thei
t
h receiver r
i
(
i
=
1
,
…
,
5
) is modeled as follows:(12)
Z
t
i
=
f
p
+
J
1
l
r
i
-
T
r
x
y
z
/
V
s
f
+
n
r
i
,where Z
t
i is the counted reading, f is the sampling frequency, l
r
i is the position of r
i on the AUV body, T
r
x
y
z is the transponder position, V
s is the speed of sound in the seawater, and n
r
i is the quantization noise. Note that n
r
i
~
U
(
-
1
/
(
2
f
)
,
1
/
(
2
f
)
). In this work we assume V
s
=
1500
m
/
s. However, in practical implementations V
s can be computed using Conductivity Temperature Depth (CTD) sensor readings [21]. Furthermore, we assume that TOA is disturbed by a noise of n
t
i
~
N
(
0
,
Q
t
) with Q
t
=
σ
T
O
A
2
I
5 and σ
T
O
A
=
1
/
3
μ
s, having σ
T
O
A
2 as the noise variance.A tightly coupled approach was adopted to infer the range and angle of arrival information. This is performed by feeding an array of TOA measurements to the estimation program. LetZ
t be the vector that includes all TOA measurements as(13)
Z
t
=
Z
t
1
,
…
,
Z
t
5
T
.


## 2.3. System Modeling

Incorporating the kinematics and measurements, the system equations can be written as follows [20]:(14)
q
˙
=
J
2
Z
g
-
b
g
,
b
˙
g
=
0
,
p
˙
=
J
1
u
,
u
˙
=
Z
a
-
ω
×
u
-
J
1
T
g
→
-
b
a
,
b
˙
a
=
0
.The state vector consists of 15 states:(15)
y
=
q
T
b
g
T
p
T
u
T
b
a
T
T
.The process noise covariance matrix is given as(16)
R
=
J
2
σ
g
2
I
3
J
2
T
d
t
2
0
0
0
0
0
0
J
1
σ
u
2
J
1
T
d
t
2
0
0
σ
a
2
I
3
d
t
2
0
0
0
0
.The measurement model is written as Z
=
h
+
Q, where Z is the measurement vector, h is the measurement function, and Q is the measurement noise covariance. They are given as follows:(17)
Z
=
Z
p
r
Z
t
Z
ϕ
θ
ψ
Z
u
T
,
h
=
ρ
s
w
g
→
T
p
+
J
1
l
p
r
f
p
+
J
1
l
r
i
-
T
r
x
y
z
/
V
s
f
,
i
∈
1,5
=
h
1
q
=
h
2
u
=
h
3
,
Q
=
σ
p
r
2
0
0
0
0
Q
t
0
0
0
0
Q
ϕ
θ
ψ
0
0
0
0
Q
u
.


## 3. State Estimation, Control, and Navigation

Based on (14), the state vector is propagated through time and a multirate unscented Kalman filter (UKF) is implemented for state estimation. The filter update process runs asynchronously as a response to the measurement readings. Once the transmitted signal is received by all of the receivers, the system performs the position update. If the difference in time of arrival of the signal between the first and last receiver is very small, then the AUV movement at that time is neglected.


### 3.1. Controller Implementation

The navigation system is decoupled for ease of control implementation, assuming negligible coupling effects between vertical and horizontal planes. AUV movement in these two planes is governed by the control of its elevator angle(
δ
e
) and rudder angle (
δ
r
), respectively. Two controllers are developed based on estimated states and the linearised versions of state equations to control δ
e and δ
r. Mechanical constraints on these angles are such that -
π
/
3
≤
δ
e
,
δ
r
≤
π
/
3.δ
e is controlled by C
δ
e:(18)
C
δ
e
=
L
Q
R
e
=
e
k
1
u
3
+
e
k
2
ω
2
+
e
k
3
θ
+
e
k
4
z
+
e
k
5
∫
z
e
r
r
,where L
Q
R
e is the Linear Quadratic Regulator (LQR) controller for elevator angle, e
k
1
,
…
,
e
k
5 are LQR gains, and z
e
r
r
=
z
-
W
P
z (i.e., the difference between z and current waypoint z coordinate).δ
r is controlled by C
δ
r, which combines LQR and Proportional-Integral (PI) Controllers:(19)
C
δ
r
=
L
Q
R
r
+
P
I
r
,
L
Q
R
r
=
r
k
1
u
2
+
r
k
2
ω
3
+
r
k
3
ψ
e
r
r
,
P
I
r
=
k
p
d
⊥
+
k
i
∫
d
⊥
,where r
k
1
,
r
k
2
,
r
k
3 are LQR gains and k
p
,
k
i are proportional and integral gains. Also, ψ
e
r
r
=
ψ
-
ψ
d, where(20)
ψ
d
=
t
a
n
-
1
W
P
y
-
y
W
P
x
-
x
,and d
⊥ is the perpendicular distance from the AUV position to the assigned path x
,
y plane. After several iterations to reduce settling time, overshoot, and steady state errors, the optimized controller gains were found to be(21)
e
k
1
=
0.5110
,
e
k
2
=
-
4.1867
,
e
k
3
=
-
6.6161
,
e
k
4
=
3.1698
,
e
k
5
=
0.0035
.
r
k
1
=
0.3137
,
r
k
2
=
-
0.9071
,
r
k
3
=
-
1.1180
,
k
p
=
0.01
,
k
i
=
0.00001
.AUV reaching the waypoint is determined by(22)
W
P
x
y
z
-
p
<
1
m
.


### 3.2. An Algorithm for Localization and Autonomous Navigation

Incorporating the state estimation and control strategies, an algorithm is developed for AUV localization and navigation and is shown in Algorithm1.Algorithm 1:AUV localization and navigation algorithm.
(1)    Initialize with: y
0 and its covariance P
0
(2)    for  i
∈
(
1
,
…
,
n
u
m
b
e
r
o
f
W
P
)  do
(3)        for  k
∈
(
1
,
…
,
∞
)  do
(4)            Read IMU measurements
(5)            Calculate sigma points
(6)            Compute y
k
-
,
P
k
- using y
k
-
1
+
,
P
k
-
1
+ and (14)
(7)            Redraw sigma points
(8)            if  Attitude measurements are available  then
(9)             Read Z
ϕ
θ
ψ
(10)         if  Pressure measurement is available  then
(11)          Read Z
p
r
(12)         if  DVLbottom-track is available  then
(13)          Z
u
=
Z
D
V
L, Q
u
=
Q
D
V
L
(14)         else
(15)          Z
u
=
Z
m
o
d, Q
u
=
Q
m
o
d
(16)         if  Trsignal is received by all receivers  then
(17)          Construct Z
t
(18)         Compute y
k
+
,
P
k
+
(19)         Implement C
δ
s and C
δ
r using (18) and (19)
(20)         Check the condition in (22)
(21)         if  WPis achieved  then
(22)          Exit and start from the next WPOne advantage of this approach is that gyroscope readings are not treated as measurements and henceω is not included in the state vector. Consequently, the dimension of the state vector is reduced. In fact, ω is used for the state-based controller implementation. However, since the gyros have very low noise (σ
=
0.005
r
a
d
/
s), we assume that eliminating the bias error from gyro readings will provide ω with sufficient accuracy.


### 3.3. Simulation Setup

Navigation simulations are performed assuming a2400
m
×
1000
m
×
10
m three-dimensional space. The ocean current velocity is assumed as V
C
(
m
/
s
)
=
[
0.2
0.4
0.3
]
T in x, y, and z directions, respectively. The mission is to perform a lawnmower-type navigation in the environment under five different cases:(1)
navigation without acoustic-based position fixes,
(2)
navigation with position aiding and DVL dropout,
(3)
dynamic model-aided navigation when DVL data is unavailable,
(4)
navigation with transponder dropout and no velocity aiding,
(5)
navigation with incremental transponder dropouts.The start and end positions of the mission are given as 1200 m, 0 m, and −10 m and −1200 m, 1000 m, and −10 m in inertial x
,
y
,
z directions, respectively. While navigating between these two positions, a 10 m step change occurs in vertical (z) direction.


### 3.4. Bias Estimation

The predefined bias values of the accelerometers and gyroscopes are estimated. This is depicted in Figure3. Figures 3(a)–3(c) show the estimates of accelerometer bias in directions u
1
–
u
3, respectively. Figures 3(d)–3(f) show gyro bias estimates, respectively, in ω
1
–
ω
3. All the bias estimates converged quickly within less than 30 s and remained constants thereafter.Figure 3
Estimated accelerometer and gyro biases.
(a)
Accelerometer bias inu
1
(b)
Accelerometer bias inu
2
(c)
Accelerometer bias inu
3
(d)
Gyro bias inω
1
(e)
Gyro bias inω
2
(f)
Gyro bias inω
3


### 3.5. Navigation without Acoustic-Based Position Fixes

Simulation is performed without transponder aiding forx
,
y position fixes throughout the run. However, velocity aiding is provided by DVL measurements. The results are shown in Figure 4. Figures 4(b)–4(d) show the error in AUV position estimates of inertial x
,
y
,
z directions, respectively. The errors in x
,
y positions are still low due to the accurate localization based on low noise DVL measurements. The error in z is still very low as the estimation of z is updated based on pressure sensor readings.Figure 4
Navigation with DVL support and withoutx
,
y position fixes.
(a)
2D plot of navigation with DVL support
(b)
Error inx with DVL support
(c)
Error iny with DVL support
(d)
Error inz with DVL support


### 3.6. Navigation with DVL Dropout

Navigation simulation is performed with a DVL data dropout over a 5000 s period, but transponder aiding is provided throughout the run. During the DVL dropout period, (9) and (10) are not used and the lines (12)–(15) in Algorithm 1 are not implemented in the controller. The results are shown in Figure 5. In Figure 5(a), the blue path shows the navigation in normal conditions where DVL data is available. The red path from A to B shows the time where DVL data is unavailable.Figure 5
Navigation with DVL dropout for 5000 s.
(a)
2D plot of navigation with DVL dropout from A to B
(b)
Error inx with DVL dropout for 5000 s
(c)
Error iny with DVL dropout for 5000 s
(d)
Error inz with DVL dropout for 5000 s


### 3.7. Model-Aided Navigation under DVL Dropout

In this case, the AUV’s dynamic model-based information is incorporated in the DVL dropout period. Figures6(a) and 6(b) depict the 2D and 3D plots of the trajectory, respectively. Note that from A to B the algorithm uses model-based data and successful navigation is performed. Figures 7(a)–7(c) show the errors associated with this model-aided navigation which occurs between 0.5
×
1
0
4 s and 1
×
1
0
4 s. Figures 8(a)–8(c) show the errors in Euler angles under model-aided navigation from A to B shown in Figure 6. The errors in ϕ and θ are very low and under the range of ±0.02 rad. However, error in ψ is under ±0.05 rad. The computation of Euler angles is not affected by the DVL dropout.Figure 6
2D and 3D plots under model-aided navigation from A to B.
(a)
2D plot
(b)
3D plotFigure 7
Errors ofx
,
y
,
z in model-aided navigation within 0.5–1.0 ×
1
0
4 s (A to B) in Figure 6.
(a)
Error inx with model-aided navigation within 0.5–1.0 ×
1
0
4 s
(b)
Error iny with model-aided navigation within 0.5–1.0 ×
1
0
4 s
(c)
Error inz with model-aided navigation within 0.5–1.0 ×
1
0
4 sFigure 8
Errors of Euler angles in model-aided navigation within 0.5–1.0×
1
0
4 s (A to B) in Figure 6.
(a)
Error inϕ with model-aided navigation within 0.5–1.0 ×
1
0
4 s
(b)
Error inθ with model-aided navigation within 0.5–1.0 ×
1
0
4 s
(c)
Error inψ with model-aided navigation within 0.5–1.0 ×
1
0
4 s


### 3.8. Navigation with Transponder Dropout and No Velocity Aiding

Navigation simulation is performed with the transponder dropout and no velocity aiding for 5000 s period. The 2D plot of the trajectory is shown in Figure9(a) and errors in x
,
y, and z are shown in Figures 9(b), 9(c), and 9(d), respectively. Note that, with no velocity aiding, the AUV quickly diverges from the assigned path resulting in high errors in x and y. Once the velocity aiding is established again (in this case the DVL reading) the AUV converges and errors in x
,
y are decreased.Figure 9
Navigation with a transponder dropout and no velocity aiding for 5000 s.
(a)
x
,
y plot of navigation with transponder dropout for 5000 s
(b)
Error inx for navigation with transponder dropout for 5000 s
(c)
Error iny for navigation with transponder dropout for 5000 s
(d)
Error inz for navigation with transponder dropout for 5000 s


### 3.9. Navigation with Incremental Transponder Dropouts

In this case, we consider a scenario where transponder dropout occurs at every 50 s and consequent loss ofx
,
y position fixes. The simulation run is limited to 10000 s. Figures 10(a) and 10(b) show the errors in range and azimuth from the AUV to the transponder position, respectively. Figures 10(c) and 10(d) show the errors in inertial x and y directions due to the transponder dropouts. It can be observed that, even with incremental loss of x
,
y position, the estimated horizontal position is maintained with low error bounds.Figure 10
Navigation with incremental transponder dropouts at every 50 s.
(a)
Range error for incremental transponder dropouts at every 50 s
(b)
Azimuth error for incremental transponder dropouts at every 50 s
(c)
Error inx for incremental transponder dropouts at every 50 s
(d)
Error iny for incremental transponder dropouts at every 50 s


### 3.10. Discussion on the AUV Navigation Results

We have performed numerous simulations using the AUV localization and navigation algorithm. The AUV localization is performed by employing an unscented Kalman filter, where the true nonlinear system is used to capture the correct mean and covariance to the 3rd order, providing better performance than standard extended Kalman filter based approaches [22]. The localization scheme yields satisfactory performance even with the transponder dropouts.In Figures5(b) and 5(c) increased errors in x
,
y positions can be observed when DVL dropout occurs. At this period the AUV failed to perform a smooth navigation. Also, in Figure 5(d) there is a small reduction of the error in inertial z at the DVL dropout period. At the DVL drop the observability matrix is not full rank. Hence, to preserve the observability the dimensions are reduced. As a result, the cross-covariance (which is introduced by DVL measurements) of filter covariance matrix is eliminated and a better representation of the uncertainty can be obtained in inertial z direction, which may cause the error reduction. However, in practice the error in z is mainly caused by the depth controller, which is mostly decoupled from the horizontal motion and makes decisions based readings from only the pressure sensor.In the model-aided navigation under DVL dropout, an increment of the error in inertialz direction can be observed at the DVL dropout period on Figure 7(c). This can be due to the fact that the new cross-covariance, which is introduced by using the model-aided information in the DVL dropout period, degrades the filter performance, hence leading to higher error in z.The rudder angle control is achieved by combining LQR with PI controller. As a result, the AUV was able to withstand the strong currents (V
C
(
m
/
s
)
=
[
0.2
0.4
0.3
]
T) and successful performances are shown in all cases.


## 3.1. Controller Implementation

The navigation system is decoupled for ease of control implementation, assuming negligible coupling effects between vertical and horizontal planes. AUV movement in these two planes is governed by the control of its elevator angle(
δ
e
) and rudder angle (
δ
r
), respectively. Two controllers are developed based on estimated states and the linearised versions of state equations to control δ
e and δ
r. Mechanical constraints on these angles are such that -
π
/
3
≤
δ
e
,
δ
r
≤
π
/
3.δ
e is controlled by C
δ
e:(18)
C
δ
e
=
L
Q
R
e
=
e
k
1
u
3
+
e
k
2
ω
2
+
e
k
3
θ
+
e
k
4
z
+
e
k
5
∫
z
e
r
r
,where L
Q
R
e is the Linear Quadratic Regulator (LQR) controller for elevator angle, e
k
1
,
…
,
e
k
5 are LQR gains, and z
e
r
r
=
z
-
W
P
z (i.e., the difference between z and current waypoint z coordinate).δ
r is controlled by C
δ
r, which combines LQR and Proportional-Integral (PI) Controllers:(19)
C
δ
r
=
L
Q
R
r
+
P
I
r
,
L
Q
R
r
=
r
k
1
u
2
+
r
k
2
ω
3
+
r
k
3
ψ
e
r
r
,
P
I
r
=
k
p
d
⊥
+
k
i
∫
d
⊥
,where r
k
1
,
r
k
2
,
r
k
3 are LQR gains and k
p
,
k
i are proportional and integral gains. Also, ψ
e
r
r
=
ψ
-
ψ
d, where(20)
ψ
d
=
t
a
n
-
1
W
P
y
-
y
W
P
x
-
x
,and d
⊥ is the perpendicular distance from the AUV position to the assigned path x
,
y plane. After several iterations to reduce settling time, overshoot, and steady state errors, the optimized controller gains were found to be(21)
e
k
1
=
0.5110
,
e
k
2
=
-
4.1867
,
e
k
3
=
-
6.6161
,
e
k
4
=
3.1698
,
e
k
5
=
0.0035
.
r
k
1
=
0.3137
,
r
k
2
=
-
0.9071
,
r
k
3
=
-
1.1180
,
k
p
=
0.01
,
k
i
=
0.00001
.AUV reaching the waypoint is determined by(22)
W
P
x
y
z
-
p
<
1
m
.


## 3.2. An Algorithm for Localization and Autonomous Navigation

Incorporating the state estimation and control strategies, an algorithm is developed for AUV localization and navigation and is shown in Algorithm1.Algorithm 1:AUV localization and navigation algorithm.
(1)    Initialize with: y
0 and its covariance P
0
(2)    for  i
∈
(
1
,
…
,
n
u
m
b
e
r
o
f
W
P
)  do
(3)        for  k
∈
(
1
,
…
,
∞
)  do
(4)            Read IMU measurements
(5)            Calculate sigma points
(6)            Compute y
k
-
,
P
k
- using y
k
-
1
+
,
P
k
-
1
+ and (14)
(7)            Redraw sigma points
(8)            if  Attitude measurements are available  then
(9)             Read Z
ϕ
θ
ψ
(10)         if  Pressure measurement is available  then
(11)          Read Z
p
r
(12)         if  DVLbottom-track is available  then
(13)          Z
u
=
Z
D
V
L, Q
u
=
Q
D
V
L
(14)         else
(15)          Z
u
=
Z
m
o
d, Q
u
=
Q
m
o
d
(16)         if  Trsignal is received by all receivers  then
(17)          Construct Z
t
(18)         Compute y
k
+
,
P
k
+
(19)         Implement C
δ
s and C
δ
r using (18) and (19)
(20)         Check the condition in (22)
(21)         if  WPis achieved  then
(22)          Exit and start from the next WPOne advantage of this approach is that gyroscope readings are not treated as measurements and henceω is not included in the state vector. Consequently, the dimension of the state vector is reduced. In fact, ω is used for the state-based controller implementation. However, since the gyros have very low noise (σ
=
0.005
r
a
d
/
s), we assume that eliminating the bias error from gyro readings will provide ω with sufficient accuracy.


## 3.3. Simulation Setup

Navigation simulations are performed assuming a2400
m
×
1000
m
×
10
m three-dimensional space. The ocean current velocity is assumed as V
C
(
m
/
s
)
=
[
0.2
0.4
0.3
]
T in x, y, and z directions, respectively. The mission is to perform a lawnmower-type navigation in the environment under five different cases:(1)
navigation without acoustic-based position fixes,
(2)
navigation with position aiding and DVL dropout,
(3)
dynamic model-aided navigation when DVL data is unavailable,
(4)
navigation with transponder dropout and no velocity aiding,
(5)
navigation with incremental transponder dropouts.The start and end positions of the mission are given as 1200 m, 0 m, and −10 m and −1200 m, 1000 m, and −10 m in inertial x
,
y
,
z directions, respectively. While navigating between these two positions, a 10 m step change occurs in vertical (z) direction.


## 3.4. Bias Estimation

The predefined bias values of the accelerometers and gyroscopes are estimated. This is depicted in Figure3. Figures 3(a)–3(c) show the estimates of accelerometer bias in directions u
1
–
u
3, respectively. Figures 3(d)–3(f) show gyro bias estimates, respectively, in ω
1
–
ω
3. All the bias estimates converged quickly within less than 30 s and remained constants thereafter.Figure 3
Estimated accelerometer and gyro biases.
(a)
Accelerometer bias inu
1
(b)
Accelerometer bias inu
2
(c)
Accelerometer bias inu
3
(d)
Gyro bias inω
1
(e)
Gyro bias inω
2
(f)
Gyro bias inω
3


## 3.5. Navigation without Acoustic-Based Position Fixes

Simulation is performed without transponder aiding forx
,
y position fixes throughout the run. However, velocity aiding is provided by DVL measurements. The results are shown in Figure 4. Figures 4(b)–4(d) show the error in AUV position estimates of inertial x
,
y
,
z directions, respectively. The errors in x
,
y positions are still low due to the accurate localization based on low noise DVL measurements. The error in z is still very low as the estimation of z is updated based on pressure sensor readings.Figure 4
Navigation with DVL support and withoutx
,
y position fixes.
(a)
2D plot of navigation with DVL support
(b)
Error inx with DVL support
(c)
Error iny with DVL support
(d)
Error inz with DVL support


## 3.6. Navigation with DVL Dropout

Navigation simulation is performed with a DVL data dropout over a 5000 s period, but transponder aiding is provided throughout the run. During the DVL dropout period, (9) and (10) are not used and the lines (12)–(15) in Algorithm 1 are not implemented in the controller. The results are shown in Figure 5. In Figure 5(a), the blue path shows the navigation in normal conditions where DVL data is available. The red path from A to B shows the time where DVL data is unavailable.Figure 5
Navigation with DVL dropout for 5000 s.
(a)
2D plot of navigation with DVL dropout from A to B
(b)
Error inx with DVL dropout for 5000 s
(c)
Error iny with DVL dropout for 5000 s
(d)
Error inz with DVL dropout for 5000 s


## 3.7. Model-Aided Navigation under DVL Dropout

In this case, the AUV’s dynamic model-based information is incorporated in the DVL dropout period. Figures6(a) and 6(b) depict the 2D and 3D plots of the trajectory, respectively. Note that from A to B the algorithm uses model-based data and successful navigation is performed. Figures 7(a)–7(c) show the errors associated with this model-aided navigation which occurs between 0.5
×
1
0
4 s and 1
×
1
0
4 s. Figures 8(a)–8(c) show the errors in Euler angles under model-aided navigation from A to B shown in Figure 6. The errors in ϕ and θ are very low and under the range of ±0.02 rad. However, error in ψ is under ±0.05 rad. The computation of Euler angles is not affected by the DVL dropout.Figure 6
2D and 3D plots under model-aided navigation from A to B.
(a)
2D plot
(b)
3D plotFigure 7
Errors ofx
,
y
,
z in model-aided navigation within 0.5–1.0 ×
1
0
4 s (A to B) in Figure 6.
(a)
Error inx with model-aided navigation within 0.5–1.0 ×
1
0
4 s
(b)
Error iny with model-aided navigation within 0.5–1.0 ×
1
0
4 s
(c)
Error inz with model-aided navigation within 0.5–1.0 ×
1
0
4 sFigure 8
Errors of Euler angles in model-aided navigation within 0.5–1.0×
1
0
4 s (A to B) in Figure 6.
(a)
Error inϕ with model-aided navigation within 0.5–1.0 ×
1
0
4 s
(b)
Error inθ with model-aided navigation within 0.5–1.0 ×
1
0
4 s
(c)
Error inψ with model-aided navigation within 0.5–1.0 ×
1
0
4 s


## 3.8. Navigation with Transponder Dropout and No Velocity Aiding

Navigation simulation is performed with the transponder dropout and no velocity aiding for 5000 s period. The 2D plot of the trajectory is shown in Figure9(a) and errors in x
,
y, and z are shown in Figures 9(b), 9(c), and 9(d), respectively. Note that, with no velocity aiding, the AUV quickly diverges from the assigned path resulting in high errors in x and y. Once the velocity aiding is established again (in this case the DVL reading) the AUV converges and errors in x
,
y are decreased.Figure 9
Navigation with a transponder dropout and no velocity aiding for 5000 s.
(a)
x
,
y plot of navigation with transponder dropout for 5000 s
(b)
Error inx for navigation with transponder dropout for 5000 s
(c)
Error iny for navigation with transponder dropout for 5000 s
(d)
Error inz for navigation with transponder dropout for 5000 s


## 3.9. Navigation with Incremental Transponder Dropouts

In this case, we consider a scenario where transponder dropout occurs at every 50 s and consequent loss ofx
,
y position fixes. The simulation run is limited to 10000 s. Figures 10(a) and 10(b) show the errors in range and azimuth from the AUV to the transponder position, respectively. Figures 10(c) and 10(d) show the errors in inertial x and y directions due to the transponder dropouts. It can be observed that, even with incremental loss of x
,
y position, the estimated horizontal position is maintained with low error bounds.Figure 10
Navigation with incremental transponder dropouts at every 50 s.
(a)
Range error for incremental transponder dropouts at every 50 s
(b)
Azimuth error for incremental transponder dropouts at every 50 s
(c)
Error inx for incremental transponder dropouts at every 50 s
(d)
Error iny for incremental transponder dropouts at every 50 s


## 3.10. Discussion on the AUV Navigation Results

We have performed numerous simulations using the AUV localization and navigation algorithm. The AUV localization is performed by employing an unscented Kalman filter, where the true nonlinear system is used to capture the correct mean and covariance to the 3rd order, providing better performance than standard extended Kalman filter based approaches [22]. The localization scheme yields satisfactory performance even with the transponder dropouts.In Figures5(b) and 5(c) increased errors in x
,
y positions can be observed when DVL dropout occurs. At this period the AUV failed to perform a smooth navigation. Also, in Figure 5(d) there is a small reduction of the error in inertial z at the DVL dropout period. At the DVL drop the observability matrix is not full rank. Hence, to preserve the observability the dimensions are reduced. As a result, the cross-covariance (which is introduced by DVL measurements) of filter covariance matrix is eliminated and a better representation of the uncertainty can be obtained in inertial z direction, which may cause the error reduction. However, in practice the error in z is mainly caused by the depth controller, which is mostly decoupled from the horizontal motion and makes decisions based readings from only the pressure sensor.In the model-aided navigation under DVL dropout, an increment of the error in inertialz direction can be observed at the DVL dropout period on Figure 7(c). This can be due to the fact that the new cross-covariance, which is introduced by using the model-aided information in the DVL dropout period, degrades the filter performance, hence leading to higher error in z.The rudder angle control is achieved by combining LQR with PI controller. As a result, the AUV was able to withstand the strong currents (V
C
(
m
/
s
)
=
[
0.2
0.4
0.3
]
T) and successful performances are shown in all cases.


## 4. Adaptive Plume Tracking

According to Algorithm2, AUV navigation is performed and information is collected about the plume boundary based onin situ fluorometer readings. Incorporating that information, an estimate on dispersion of the plume can be achieved. Table 2 depicts the methods and limitations of the state-of-the-art plume tracking strategies used in the literature. In this work, we developed an efficient adaptive plume tracking algorithm to track the plume boundary and investigate its dispersion, with path optimization. An initial bounding box is assigned based on some prior knowledge (such as remote sensing data) and it is assumed that the oil plume is located inside the bounding box. The tracking algorithm navigates the AUV through the predefined search path and fluorometer readings are incorporated for detecting the plume boundary. Once a fluorometer reading F
r
i exceeds a predefined threshold T
f, a plume can be detected with a higher concentration. At this point, the AUV reaches the first boundary point and the algorithm switches to the boundary detecting mode.Table 2
Different plume tracking strategies in literature.
Article
Number of agents
Properties
Limitations
[23]
Multiple
Uses CUSUM algorithm, presents the boundary estimation problem as a HMM, and recast as an optimization problem
Considered only ellipse-shaped, no path optimization, simulation only, intervehicle communication issues not addressed, and AUV dynamics not considered
[24]
Multiple
Decentralized, gradient-free algorithm, convergent and stable
AUV dynamics not considered, high computation cost, intervehicle communication issues not addressed, and only very simple plume shapes considered
[3]
Single, multiple
Cooperative, generating polygons to follow based on ocean model predictions, simulation, and practical implementation
Trajectory based on (roughly) approximated polygons, temporal constraints not considered, unable to react to the fast moving features, and ignoring the dynamics of the glider
[25]
Single
Behavior-based approach for plume mapping, subsumption architecture, showing experimental results
Limitations of behavior-based approach, no description on adaptive mapping, and implementing simple preplanned lawn-mower strategy
[26]
Single
Uses colored dissolved organic matter (CDOM) sensor for planed missions, adaptive planning usingin situ current, and temperature measurements, and gets the 3D track of the AUV, practical implementation
No information on path optimization and no adaptive tracking
[27]
Single
Based on peak-capture algorithm, it generates a sawtooth trajectory and uses depth information and practical implementation
No information on path optimization
[28]
Multiple
Adaptive behavior-based system, acoustic communication within AUVs, representing the plume using Fourier orders when reconstructing
No information on path optimization and communication overhead
[29]
Single
Uses a plume indicator function and real-time implementation and uses adaptive transects; transect length depends on number of consecutive samples; distance between transects is a percentage of previous samples
Less path optimization, not using an AUV, no information about the convergence, and coverage of the used algorithm
[30]
Single
Uses remote sensing data to detect hotspots, uses surface current to project plumes spatiotemporally, and runs in a lawnmower type pattern, practical implementation
No path optimization, only using predefined pattern, and no adaptive tracking
Proposed
Single
Path optimization based on gradient information, adaptive plume tracking, and centralized approach
Only simulation results, no comparison data available, relying on remote sensing data for locating the plume region initially, low performance to noisy plume boundaries, and using only one fluorometerAlgorithm 2:Plume boundary tracking algorithm.
input: Fluorometer readings F
r
i  (
i
=
1
,
…
,
n
), threshold (T
f)
output: Adaptive boundary tracking algorithm
(1)    while  F
r
i
<
T
f  do
(2)     Navigate the AUV through pre-defined path.
(3)     if  AUVreaches to lastWP  then
(4)      Could not locate the plume…! exit
(5)    while  boundary track is not complete  do
(6)     if  F
r
i
>
T
f  then
(7)      Plume boundary is detected: log
⁡
(
x
t
,
y
t
)
(8)      Move AUV until i
=
k
1 (Δ
a
∝
k
1)
(9)      Calculate the gradient g
t using (
x
t
,
y
t
) and (
x
t
-
1
,
y
t
-
1
)
(10)   Set ψ
=
g
t
(11)   Calculate section length Δ
b
∝
1
/
g
t
-
g
t
-
1 and move AUV
(12)   Set ψ
=
π
/
2 and move
(13)  if  F
r
i
<
T
f  then
(14)   Repeat from line (8) to (12)
(15)   if  i
>
k
2  then
(16)    Plume cannot be located…!
(17)    Calculate the center point (
x
c
,
y
c
) using x
1
,
y
1
,
…
,
x
t
,
y
t
(18)    Set ψ to (
x
c
,
y
c
) and move
(19)    Repeat from line (6)The proposed approach is depicted in Figure11 and the steps followed are described in Algorithm 2. Let k
1 be a predefined number of consecutive fluorometer readings to ensure the AUV is inside the plume boundary. Also, let k
2 be another predefined number of consecutive fluorometer readings, which are lower than T
f, to ensure that the plume cannot be located. The transect Δ
a is proportional to k
1. The same can be applied when the AUV is outside the boundary. Furthermore, Δ
b is used for rapid plume mapping and it helps to improve the efficiency of the algorithm. To calculate Δ
b, the system needs the two latest consecutive gradient values (as stated in line (11) of Algorithm 2). These consecutive gradient values are obtained from the four consecutive logged (x
,
y) positions. The calculation of Δ
b is mentioned in line (9) of Algorithm 2. When the plume boundary is relatively smooth, the two consecutive gradients are nearly equal and the section length Δ
b can be long. This is better for smooth sections of the plume boundary. Also when the plume boundary seems irregular, those consecutive gradients will be different and Δ
b will be short comparatively. As a result, the system can map the complex, irregular-shaped section of the boundary.Figure 11
Plume boundary tracking.Lines (15)–(18) in Algorithm2 ensure that the plume is tracked all the time. Here, k
2 controls the distance the AUV moves before it turns to the center point direction. The condition in line (5) is evaluated based on a predefined distance between start and end points (points A and B in Figure 11) and terminates the plume detection mode.One limitation of the approach is that the accuracy of plume boundary coverage entirely depends on the sensitivity of the fluorometer and the predefinedk
1
,
k
2 values. The performance on “noisy” plume boundaries may be improved by adaptively changing the k
1
,
k
2 values and adding an array of fluorometers.


### 4.1. Plume with an Ellipse-Shaped Boundary

A horizontal plume dispersion with a simple ellipse shape is considered. The major and minor axis lengths of the ellipse are 100 m and 150 m. The angle shift from the major axis isπ
/
3. Figure 12(a) shows the true and detected plume boundaries under DVL-aided and dropout (dynamic model-based) navigation. Figures 12(b) and 12(c) show the 3D view of the navigation and the error in plume detection in x
,
y
,
z directions, respectively. Point A is the place where the AUV looses the DVL data. Although instant high errors can be observed in inertial x
,
y
,
z, those errors slowly converge due to the help of model-aided navigation.Figure 12
Plume with an ellipse-shaped boundary.
(a)
Dispersion in 2D
(b)
Dispersion in 3D
(c)
Error inx
,
y
,
z. At 5000 s DVL drop occurs and model-aided navigation starts


### 4.2. Plume Boundary Modeled Using Fourier Orders

A rough estimate of the horizontal dispersion of a real plume can be obtained using Fourier orders of the form [28](23)
R
k
=
∑
i
=
0
k
A
i
cos
⁡
i
θ
+
ϕ
i
+
R
u
,where R
k is the radial distance to the plume boundary, k
(
=
10
) is the highest Fourier order of the series, R
u
(
=
50
m
) is the undisturbed plume radius, θ
(
=
[
0,2
π
)
) is the angles about the center of the plume evaluated for each degree, A
i
(
=
(
+
25
R
u
/
2
k
,
-
25
R
u
/
2
k
)
) is the radial amplitude perturbation, and ϕ
i
(
=
[
-
π
,
+
π
]
) is the phase shift of i
t
h order. Figures 13(a) and 13(b) show the dispersion of the plume and the detected plume boundary in 2D and 3D view. Figure 13(c) shows the error in plume detection in x
,
y
,
z directions. Note that the section length Δ
b is reduced due to the irregularity of the plume dispersion.Figure 13
Plume boundary modeled using Fourier orders.
(a)
Dispersion in 2D
(b)
Dispersion in 3D
(c)
Error inx
,
y
,
z


### 4.3. Discussion on the Developed Plume Tracking Algorithm

The proposed approach was able to successfully track a more realistic plume boundary demonstrating its path optimization and adaptive features. This can be observed in Figure13(a). From sections A-B where the irregularity of the plume boundary is lower, the AUV crosses the plume more loosely. Also at the places where the irregularity seems higher (sections C-D or E-F) AUV crosses the plume tightly. Also, the AUV navigation plan is not predefined and adaptive according to the shape of the plume boundary.Certainly there are lots of facts that could influence the limitations of the algorithm. The measurement uncertainty of the oil sensor is not considered and the operation is assumed as an ON/OFF type. Consequently, those uncertainties can affect the accuracy of the detection/tracking of the plume boundary. The accuracy of the plume tracking system can be improved by adding an array of fluorometers to better estimate the oil concentration. Another challenge is the noisy plume boundaries where the gradient information based on fluorometer readings cannot be established accurately enough. Consequently, the AUV could fail to generate a reliable boundary tracking. Furthermore, the accuracy of the boundary tracking specially with irregular gradients is limited by the physical AUV dynamics and actuator constraints. Moreover, due to the medium density changes in a plume area the behavior of the acoustic could be complicated. This can introduce screening effects that severely impair the acoustic system and the AUV may fail to estimatex
,
y positions accurately. However, at this work we did not model the medium density changes and its effect in acoustics. We assumed that the acoustic channel behaves the same both in and out of the plume. We believe it can be an interesting future work.


## 4.1. Plume with an Ellipse-Shaped Boundary

A horizontal plume dispersion with a simple ellipse shape is considered. The major and minor axis lengths of the ellipse are 100 m and 150 m. The angle shift from the major axis isπ
/
3. Figure 12(a) shows the true and detected plume boundaries under DVL-aided and dropout (dynamic model-based) navigation. Figures 12(b) and 12(c) show the 3D view of the navigation and the error in plume detection in x
,
y
,
z directions, respectively. Point A is the place where the AUV looses the DVL data. Although instant high errors can be observed in inertial x
,
y
,
z, those errors slowly converge due to the help of model-aided navigation.Figure 12
Plume with an ellipse-shaped boundary.
(a)
Dispersion in 2D
(b)
Dispersion in 3D
(c)
Error inx
,
y
,
z. At 5000 s DVL drop occurs and model-aided navigation starts


## 4.2. Plume Boundary Modeled Using Fourier Orders

A rough estimate of the horizontal dispersion of a real plume can be obtained using Fourier orders of the form [28](23)
R
k
=
∑
i
=
0
k
A
i
cos
⁡
i
θ
+
ϕ
i
+
R
u
,where R
k is the radial distance to the plume boundary, k
(
=
10
) is the highest Fourier order of the series, R
u
(
=
50
m
) is the undisturbed plume radius, θ
(
=
[
0,2
π
)
) is the angles about the center of the plume evaluated for each degree, A
i
(
=
(
+
25
R
u
/
2
k
,
-
25
R
u
/
2
k
)
) is the radial amplitude perturbation, and ϕ
i
(
=
[
-
π
,
+
π
]
) is the phase shift of i
t
h order. Figures 13(a) and 13(b) show the dispersion of the plume and the detected plume boundary in 2D and 3D view. Figure 13(c) shows the error in plume detection in x
,
y
,
z directions. Note that the section length Δ
b is reduced due to the irregularity of the plume dispersion.Figure 13
Plume boundary modeled using Fourier orders.
(a)
Dispersion in 2D
(b)
Dispersion in 3D
(c)
Error inx
,
y
,
z


## 4.3. Discussion on the Developed Plume Tracking Algorithm

The proposed approach was able to successfully track a more realistic plume boundary demonstrating its path optimization and adaptive features. This can be observed in Figure13(a). From sections A-B where the irregularity of the plume boundary is lower, the AUV crosses the plume more loosely. Also at the places where the irregularity seems higher (sections C-D or E-F) AUV crosses the plume tightly. Also, the AUV navigation plan is not predefined and adaptive according to the shape of the plume boundary.Certainly there are lots of facts that could influence the limitations of the algorithm. The measurement uncertainty of the oil sensor is not considered and the operation is assumed as an ON/OFF type. Consequently, those uncertainties can affect the accuracy of the detection/tracking of the plume boundary. The accuracy of the plume tracking system can be improved by adding an array of fluorometers to better estimate the oil concentration. Another challenge is the noisy plume boundaries where the gradient information based on fluorometer readings cannot be established accurately enough. Consequently, the AUV could fail to generate a reliable boundary tracking. Furthermore, the accuracy of the boundary tracking specially with irregular gradients is limited by the physical AUV dynamics and actuator constraints. Moreover, due to the medium density changes in a plume area the behavior of the acoustic could be complicated. This can introduce screening effects that severely impair the acoustic system and the AUV may fail to estimatex
,
y positions accurately. However, at this work we did not model the medium density changes and its effect in acoustics. We assumed that the acoustic channel behaves the same both in and out of the plume. We believe it can be an interesting future work.


## 5. Conclusion

In this work we have developed a multirate UKF algorithm for AUV localization in a GPS-denied undersea environment. Furthermore, an adaptive plume detection and tracking system is developed. The proposed tracking algorithm uses gradient information. The algorithms are implemented in simulations and successful results are obtained.The adaptive plume tracking system developed in this work is not limited to tracking oil plumes and it can be also used to track other types of plumes such as biological and chemical. As a future work we expect extending the proposed system to track 3D dynamic plumes and implementing it in real time to investigate most of the issues present in physical scenarios, which are not examined in detail with the current simulation work. Moreover, extending this system for multi-AUV navigation missions such as implementing a cooperative AUV network for under-ice oil plume detection and tracking would be an interesting research study. This will decrease the overall time for feature tracking and improve the usage and sharing of information, which lead to a better representation of the plume boundary.



---

*Source: 1764527-2016-03-03.xml*