Annual Reviews in Control 42 (2016) 166–176

Contents lists available at ScienceDirect

Annual Reviews in Control

journal homepage: www.elsevier.com/locate/arcontrol

Review

AUV terrain-aided navigation using a Doppler velocity loggerR

Francisco Curado Teixeira∗, João Quintas, António Pascoal
Institute for Systems and Robotics, Instituto Superior Técnico, Univ. Lisboa, Av. Rovisco Pais 1, Lisbon 1049-001, Portugal

a r t i c l e i n f o a b s t r a c t

Article history: This paper addresses the design and implementation of terrain-aided navigation (TAN) methods for small
Received 27 May 2016 autonomous underwater vehicles (AUVs) that rely on standard navigation sensors and dispense with the
Revised 19 September 2016
need for dedicated sensors for terrain data acquisition. The research described focuses on the problem of
Accepted 20 October 2016
TAN implementation in underwater scenarios characterized by smooth sea-bottom topography and very
Available online 4 November 2016
shallow water, where the terrain information available for navigation is scarce. The navigation algorithms
Keywords: and the data fusion methods whose tests are documented in the paper build upon and expand prior the-
Particle filter oretical work published by the authors; the TAN solutions adopted exploit the terrain information and the
Complementary filtering navigation data acquired with an inexpensive Doppler velocity logger (DVL) and a standard motion refer-
Doppler-based navigation ence unit, respectively. The position estimation methods analyzed include a bi-dimensional particle filter
Terrain-aided navigation (PF) and a four-dimensional Rao-Blackwellized PF that was designed to estimate the unknown Doppler
velocity measurement biases responsible for the unbound localization errors typically observed in dead-
recknoning navigation. The positioning accuracy achieved with these filters is compared with the output
of a novel method, also proposed in the paper, that mechanizes a complementary-like filter designed to
fuse the output of a TAN estimator with the velocity measurements provided by a DVL. Experimental
results obtained during field tests with an autonomous marine vehicle are reported and analyzed.
© 2016 International Federation of Automatic Control. Published by Elsevier Ltd. All rights reserved.

1. Introduction ple, terrain navigation can be integrated with dead-reckoning to

Terrain-aided navigation (TAN) systems have been proposed
during the last decades as an economical alternative to conven-
tional navigation methods for autonomous underwater vehicles
(AUVs) that rely on high-grade inertial navigation systems (INS)
and long acoustic baselines. The surge of interest in this type of
methods, witnessed in the last few years, may be explained by
the increasing demand of low-cost, multipurpose underwater vehi-
cles capable of performing long-term and long-range oceanic mis-
sions in fully autonomous mode using affordable, accurate naviga-
tion systems. TAN relies on the acquisition of range measurements
using sonar sensors installed on a vehicle, which are matched se-
quentially with the predicted ranges obtained using a prior map
of the terrain elevation to estimate the vehicles position. In princi-
implement autonomous navigation systems capable of providing
accurate estimates of position in the short term, combined with
bounded localization errors in the long run. TAN is a cost-effective
approach specially for applications that involve operations in a pre-
viously mapped area. TAN may also be an interesting approach for
missions that require prolonged operations in the same area even
if a reference map has to be acquired, since the cost of acquiring
the prior maps can be largely compensated by the investment re-
duction in terms of navigation instrumentation. Map sharing by
different users and the utilization of public bathymetric datasets,
which are currently available on a world-wide scale, may also con-
tribute to reduce the costs of TAN implementations.
2. Prior work on TAN and main contributions of the paper

R
Funding: This research was supported in part by FCT project ATLAS [PTDC/EEA- 2.1. Related problems
ELC/111095/2009], project MORPH (EU FP7 grant agreement No. 288704), and FCT
[PEst-OE/EEI/LA0 0 09/2011]. The authors gratefully acknowledge the sponsorhip of There are interesting publications on the development of TAN
the South Korean Agency for Defense Development under a collaborative research
systems for air vehicles that have inspired the application of these
agreement between KAIST and IST.
∗
Corresponding author. Fax: +351 218 499 242. concepts to underwater navigation. As a representative example,
E-mail addresses: fcurado@isr.ist.utl.pt (F. Curado Teixeira), joaogquintas@ in Bergman (1999) the authors use a Bayesian approach to terrain
tecnico.ulisboa.pt (J. Quintas), antonio@isr.ist.utl.pt (A. Pascoal). aided navigation, whereby inertial navigation system (INS) data are

http://dx.doi.org/10.1016/j.arcontrol.2016.10.002
1367-5788/© 2016 International Federation of Automatic Control. Published by Elsevier Ltd. All rights reserved.
 F. Curado Teixeira et al. / Annual Reviews in Control 42 (2016) 166–176
combined with terrain information measured by a radar altimeter.
They further use a point mass filter to iteratively propagate an ap-
propriately defined posterior probability density function so as to
estimate the position of the vehicle. The same circle of ideas is
exploited in a series of publications culminating in the work pre-
sented by Nordlund and Gustafsson (2008), where a marginalized
particle filter is used to fuse INS data with TAN estimates of posi-
tion based on radar altimeter measurements.
The increasing interest of the underwater robotics community
in the TAN approach is reflected in a series of demonstrations of
the concept through experimental trials in the water performed
recently by distinct research groups. With a few, early exceptions
(see e.g. Majumder, 2001; Newman, 1999), implementations of the
TAN methodology for underwater vehicles rely exclusively on sonar
to observe the environmental features of interest. The sensors most
frequently proposed are conventional echo-sounders as well as
pencil-beam and multi-beam sonars. In the latter case, due to the
large computational burden of the correlation methods employed
in some implementations, the navigation systems proposed include
dedicated hardware for acquisition and matching of bathymetric
features (Nygren, 2005; Nygren & Jansson, 2004). The utilization
of expensive sonar systems such as multi-beams is justified when
these sensors are used not only in navigation but also in the sci-
entific or commercial applications envisioned for the vehicle. Oth-
erwise, less expensive instruments should be chosen to implement
the navigation solution.
2.2. Estimation methods
The last two decades have witnessed a surge of applications
of navigation methods, including terrain-aided underwater navi-
gation, that resort to sequential Bayeisan estimation algorithms
based on point-mass approximations of probability distributions.
This class of algorithms includes grid based methods like the point
mass filter (PMF) and simulation methods commonly known as
particle filters (PFs); see e.g. Anonsen, Hagen, Hegrenaes, and Ha-
gen (2013); Anonsen and Hallingstad (2006) and the comprehen-
sive survey presented in Melo and Matos (2013). Among other
advantages that contribute to their widespread acceptance, PFs
and PMFs allow for accurate representations of multi-modal, non-
parametric noise distributions and support nonlinear process and
measurement models without the need for linearized approxima-
tions. Particle filters have been preferred in most applications due
to their reduced computational complexity when compared with
point-mass filters whose computational burden increases exponen-
tially with the dimension of the state-space. The superior perfor-
mance of particle filters relatively to parametric estimation ap-
proaches - e.g. Kalman-based and batch-oriented matching pro-
cesses like the Terrain Contour Matching, TERCOM (Siouris, 2004)
- is shown in several publications that address the TAN prob-
lem; see e.g. Bergman (1999), Gustafsson et al. (2002), Nordlund
(20 02), Frykman (20 03), Nygren (20 05), Xie (20 05), Morice, Veres,
and McPhail (2009), Shidan, Liguo, Xin, and Desheng (2011),
Nordlund and Gustafsson (2008), Meduna, Rock, and McEwen
(2008), Meduna, Rock, and McEwen (2010).
2.3. Proposed TAN solutions
The work of Nygren and Jansson (2004), Anonsen and
Hallingstad (2006), and Morice et al. (2009), among others,
demonstrated experimentally the potential of the TAN solution in
different types of terrain. A commercial version of a terrain naviga-
tion solution with the ability to exploit the altitude measurements
provided by a multibeam echo-sounder (MBE) or alternatively by
a Doppler velocity logger (DVL) has been delivered recently; see
167
Anonsen et al. (2013). The TAN module is integrated with the DVL-
aided navigation system of the Hugin AUV which includes a state-
of-the-art inertial measurement unit (IMU) and a high-grade DVL;
see Hagen, Anonsen, and Mandt (2010). All the above mentioned
navigation systems rely primarily on navigation-grade INS and top
quality swath sonar instrumentation that contribute to make them
too expensive. To the best of our knowledge, the first success-
ful low-cost terrain navigation solution using non-dedicated sonar
sensors and lower-accuracy inertial systems was first reported in
Meduna et al. (2008).
The TAN problem formulated in our previous work (Teixeira,
2007) assumed a sensor set-up where a Doppler velocity logger
measured the velocity of the vehicle relative to the water. It fur-
ther considered the existence of oceanic currents that could not
be measured directly. This was justified by the working scenario
and the characteristics of the high-resolution DVL employed whose
maximum range could not guarantee its operation in bottom-lock.
Consequently, the problem of estimating the position of the vehicle
posed the additional problem of estimating its drifting velocity due
to the effect of the oceanic currents. The problem that we present
now is apparently simplified by the availability of DVL measure-
ments of the velocity vector relative to the sea-bottom, thus elim-
inating the need to estimate the velocity of the oceanic currents.
However, the solution sought includes the estimation of the veloc-
ity biases inherent in Doppler measurements that, when uncom-
pensated, cause large drifts in the long run. Hence, the current
problem bears some resemblance with the previous one; in princi-
ple, it can be solved using a four-dimensional particle filter which
jointly estimates the DVL velocity bias and the 2D position of the
vehicle.
As a contribution to the development of a cost-effective ter-
rain navigation system for AUVs, in Teixeira, Quintas, and Pascoal
(2012b) we proposed an implementation based on a Doppler ve-
locity logger (DVL) integrated with standard, inexpensive motion
sensors. In the solution proposed, besides using the Doppler-based
velocity measurements for dead-reckoning, we exploited the ability
of the DVL to acquire periodically a set of altitude or slant-range
measurements that are used by TAN algorithms for terrain match-
ing. The implementation reported relied on a new particle filter
algorithm, the Prior-correction Particle Filter (PPF) described in
Teixeira, Pascoal, and Maurya (2012a) and used a complementary
filter (CF) to fuse the TAN estimates with the dead-reckoning data.
The paper showed the advantages of using the novel particle filter
and the CF data fusion mechanism in TAN applications; the perfor-
mance of the methods proposed was illustrated in computer simu-
lations using real bathymetry and simulated DVL data. In Teixeira,
Quintas, and Pascoal (2015) we validated the solutions proposed
in the above two papers, using experimental data acquired in the
water with a robotic marine vehicle. The trials were conducted in
an marine scenario characterized by a relatively smooth sea-floor
topography and very shallow water, where the terrain information
available for navigation is scarce. The results thus obtained demon-
strated the efficacy of the proposed solution even in difficult oper-
ationl conditions.
2.4. Main contributions of the paper
In the current paper we analyze in detail, both from a theoreti-
cal and practical standpoint, the limitations of particle filter formu-
lations commonly used in terrain aided navigation, including those
contributed by the authors in previous publications (e.g., Teixeira,
2007; Teixeira et al., 2012a). Here, for the first time, we avail our-
selves of experimental data - obtained in the course of field trials
- that have hitherto not been used for the above purpose. Further,
the positioning estimates obtained with those filters are compared
in terms of accuracy with the output of the complementary filter
168 F. Curado Teixeira et al. / Annual Reviews in Control 42 (2016) 166–176
that we propose to fuse the output of a TAN estimator with the
velocity measurements provided by a DVL. This analysis provides
compelling practical evidence of the superior performance of the
proposed TAN solution.
3. Basic notation, problem formulation, and TAN filters setup
In the formulation of the TAN problem and in the models that
follow {I} represents an inertial coordinate frame, {B} denotes the
body-fixed frame that moves with the vehicle, p = [x, y, z]T is the
position of the origin of {B} measured in {I}, λ = [φ , θ , ψ ]T repre-
sents roll, pitch and yaw angles that parametrize locally the orien-
tation of {B} relative to {I}, and ω = [ p, q, r]T represents the angu-
lar velocity of {B} w.r.t. {I}, expressed in {B}. Vector V = [u, v, w]T
represents the linear velocity of the origin of {B} relative to the
sea-bottom supplied by the Doppler, expressed in {B}.
Using the above notation, the TAN problem can be stated as
follows:
Given the kinematic model of an underwater vehicle, a bathymetric
map of the area of interest, and measurements of ranges and velocity
obtained by the vehicle with respect to the seabed, expressed in {B},
estimate the position and the velocity of the vehicle relative to {I}.
Although the present TAN formulation includes a component
of dead recknoning navigation which is essentially a linear prob-
lem, the terrain-aided navigation problem formulated above con-
stitutes a nonlinear estimation problem due to the nonlinear, non-
structured nature of the measurement model. This model relates
measurements provided by the acoustic sensors installed on-board
with the three-dimensional position and the orientation of the
vehicle relative to the sea-bottom represented in the map. The
problem is addressed here in the framework of nonlinear sequen-
tial Monte Carlo estimation, using the above mentioned Prior-
correction Particle Filter.
In the present implementation, and without loss of generality, it
is assumed that the AUV maintains constant depth (z) which can
be measured with high accuracy by using a pressure cell. Hence,
we treat z as an input instead of a state variable. This is adequate,
since we do not need to estimate z and has the advantage of re-
ducing the dimension of the state vector. Also, for simplicity of
analysis we assume that the vehicle is leveled horizontally and sta-
bilized in roll and pitch, i.e. φ = θ = 0 so that the angular velocity
yaw rate is given by r = ψ˙ . Given these assumptions we restrict
the current problem to the estimation of position and velocity in
the horizontal plane. In the 2D formulation that we will use hence-
forth, v = [x˙ , y˙ ]T is the vector representing the velocity of the ve-
hicle expressed in {I}.
3.1. Stochastic models and filter set-up
In what follows we borrow from Teixeira et al. (2012a) and
Teixeira et al. (2012b) the formulation of the process and mea-
surement models, and the complementary filter setup, which are
reproduced here with minor adaptations. The models are simpli-
fied in the present case, since the problem does not include the
estimation of the velocity bias introduced by oceanic currents; it
is assumed that a DVL operating in bottom-lock supplies the ve-
locities relative to the seabed. As will be shown in Section 4, un-
der these assumptions the TAN position estimates may computed
using a 2D particle filter which is computationaly less demanding
than the four-dimensional Rao-Blackwellized PF version adopted in
prior implementations.
Employing the notation usually associated with the formula-
tion of particle filters (see, e.g., Arulampalam, Maskell, Gordon, &
Clapp, 2002), x is a vector representing the system state with di-
mension nx and y is a measurement vector with dimension ny ,
N is the number of samples (particles) used by the particle fil-
ter, xti represents the ith particle (a random sample from the state
space) at time instant t, and ωti denotes the weight associated to
particle xti .
3.1.1. Process and measurement models
The discrete-time kinematic process model that we consider is
given by
xk+1 = F xk + Gu,k uk + Lζk , (1)
where F = I2x2 and L denote the state-transition and the noise
coupling matrices, respectively, x = [x, y]T is the state vector, uk =
[u, v, ψ ]T is the input vector, and ζk ∈ IRnx represents the pro-
cess noise sequence. The input coupling matrix Gu, k is a func-
tion of the orientation ψ , and the product Gu, k uk represents the
dead-reckoning incremental displacement which is as a function
of linear velocity and orientation. The discrete-time measurement
model with additive measurement noise is given by
yk = h ( pk , ψ ) + η ( pk , ψ ), (2)
where h(. ) : IR4 → IRny is a function that supplies a set of true
slant ranges measured between the vehicle and the sea-floor; in
the navigation filters it is implemented by ray-tracing in the direc-
tion of the corresponding sonar beams used to acquire the terrain
information; η ∈ IRny models map errors and range measurement
noise which is a function of the distance measured by the acous-
tic sensors; yk is a vector that represents the set of measurements
taken at each iteration. To model the acquisition of terrain eleva-
tion data using the Doppler unit, the observation model presented
is adapted to account for a set of four simultaneous range mea-
surements according to the Janus configuration (see e.g. Brokloff,
1994).
3.1.2. Noise models
The discrete-time process noise sequences represented in ζ k are
assumed mutually independent and Gaussian, with intensity noise
represented by matrix Q. Given information on the vehicle posi-
tion and orientation, the measurement noise variables represented
in the vector η are considered mutually independent and are char-
acterized by the time-varying measurement noise intensity matrix
Rk ; see Teixeira (2007). Notice that in the measurement model, the
time-varying error is a function of the sonar resolution, slant range,
and local terrain gradient; please refer to Teixeira (2007) and con-
sult the main noise parameters in Table 3.
4. TAN implementation with particle filters
4.1. Particle filter setup
Particle filters are nonlinear state estimators of hidden Markov
processes that implement state prediction and correction (update)
steps as proposed originally by Ho and Lee (1964) using the im-
portance sampling principle; see, e.g. Tanner (1996). The sequen-
tial importance sampling (SIS) algorithm mechanizes a standard PF
version and forms the basis of several particle filter implementa-
tions; Arulampalam et al. (2002). The algorithm uses the following
recursive procedure:
 F. Curado Teixeira et al. / Annual Reviews in Control 42 (2016) 166–176 169

Initialization 4.2. TAN/DVL marginalized particle filter

 N
Draw a set of particles xik from the prior, xik ∼ p0 (x )
 i i=1
N In the measurement model (2) the non-linear function h(.) only
with assigned weights w0 equal to 1/N. uses part of the state vector x, i.e. h(x ) = h(x, y ). Since model (1) is
i=1
Iteration linear in the states and the additive noise is Gaussian, this allows
Repeat the following two steps for k = 1, .., T : Rao-Blackwellization of the PF, decomposing the state vector into
Prediction two parts: x = [xpf , xkf ]T , where x p f = [x, y]T represents the part
 N
Draw a set of sample states xik from the prior of the state vector estimated by the PF and xk f = [bx , by ]T is the
i=1
distribution: part of the state vector estimated by a Kalman Filter (KF). We de-
xik ∼ p(xik |xik−1 ). compose similarly the state transition matrix F, the input coupling
Update matrix Gu, k , the noise coupling matrix L, the input and noise vec-
Weight the particles using the likelihood of the tors uk and ζ k , and the discrete-time process noise intensity ma-
measurements: trix Q. In the sequel we use uk = [uk , vk , ψk ]T = [Vk , ψk ]T . The RBPF
wik = wik−1 p(yk |xik ). formulation becomes:
Prediction
xkp+1
f,i
= xkp f,i + F p f xkk f,i + Gu,k
pf
Vk + ζkp f (3)
The above procedure implements a simple and intuitive expres-
sion for the correction stage where new samples drawn from the  
prior have their weights updated by the likelihood function. It has xkk+1
f,i
= F k f xˆ kk|f,i
k−1
+ Kk νk (4)
been demonstrated that this formulation presents limitations that
where Kk denotes the KF gain, νk = xk+1 − xk
p f,i p f,i k f,i
can be circumvented by appropriate choices of the sampling and − (F p f xˆ k|k−1 +
filtering distribution used by the filter and the inclusion of a re- pf
Gu,kVk ), and
sampling step. These considerations led to the development of new
PF versions such as the Sequential Importance Sampling with Re-
  T  T
sampling (SISR), Auxiliary Particle Filter (APF), and Likelihood PF ζkp f ∼ N 0, F p f Pkk|fk−1 F p f + L p f Q p f L p f . (5)
(LPF); see Pitt and Shephard (1999), Doucet, Godsill, Andrieu, and
Update
filtering (20 0 0), Arulampalam et al. (20 02). Given the widespread
The standard particle filter updates the weights according to
utilization of the SISR algorithm at the core of most particle fil-
ter implementations, we refer to it as the standard PF algorithm. wik = wik−1 p(yk |xik ),
In our implementations we adopt the Prior-corrected Particle Filter
while the PPF formulation uses the expression
(PPF) proposed by Teixeira et al. (2012a) that has shown superior
performance in TAN applications. Although not as severe as in the wik = wik−1 p(xk |xik−1 ) p(yk |xik ). (6)
case of point mass filters, a well-known limitation of particle filters Point estimates
is the escalate of their computational complexity with the dimen- A point estimate of the current state xˆ k and the associated co-
sion of the state-space. For this reason, until recently particle filters variance matrices Pk can be obtained from the following equations:
have been limited to estimation problems in two or three dimen-
sions. An efficient solution to the dimensionality problem consists  

N
xkp f,i
in using a marginalized version of the filter, also known as Rao- xˆ M MS
 ωki xik = ωki (7)
Blackwellized particle filter (RBPF). This approach combines the ad-
k
i i
xkk f,i
vantage of reduced filter dimensionality with increased estimation
accuracy since it benefits from the efficiency principle stated in the

N   T
Pkp f = ωki xkp f,i − xˆ kp f,MMS · xkp f,i − xˆ kp f,MMS (8)
Rao-Blackwell theorem, Blackwell (1947). i
The terrain-aided navigation problem formulated in Section 3 is
essentially a nonlinear problem due to the nonlinear, non-

N   T
structured nature of the measurement model. This model relates Pkk f = Pkk|fk + ωki xˆ kk|f,ik − xˆ kk f,MMS xˆ kk|f,ik − xˆ kk f,MMS . (9)
measurements provided by the acoustic sensors installed on-board i

with the three-dimensional position and the orientation of the
vehicle relative to the sea-bottom represented in the map. Due
to the high-dimentionality of the state-space model, which in-
cludes the 2D coordinates of the vehicle position and the 2D ve-
locity components of the velocity bias, a direct PF implementa-
tion of the state estimator incurs in a high computational bur-
den. In order to make the problem tractable, current implementa-
tions marginalize the probability distribution of the states and ap-
ply a Kalman filter to solve the linear part of the estimation prob-
lem; see, e.g. Teixeira (2007), Nordlund and Gustafsson (2009). We
present next the realization of a Rao-Blackwellized particle filter
(RBPF) that has been used in previous works to solve the current
problem. In this presentation we eschew the details of the deriva-
tion of particle filter (PF) algorithms that have been covered in de-
tail in the literature. The interested reader can find in e.g. Liu and
Chen (1998), Arulampalam et al. (2002), and Cappé, Godsill, and
Moulines (2007) comprehensive and in-depth expositions of the
different algorithms and extensive lists of references on the sub-
ject. A novel PF formulation with application to TAN is presented
in Teixeira et al. (2012a).
Although the Rao-Blackwellized PF has shown to provide accu-
rate position estimates in the presence of unknown velocity biases
of significant magnitude, using the RBPF formulation does not con-
tribute to improve the estimation accuracy when the magnitude of
the velocity bias is very small, as in the case under analysis. Ac-
cording to the results of our tests, it is preferable to resort to the
bi-dimensional version of the filter which achieves a similar esti-
mation accuracy with a much lower computational cost. The filter
performance thus achieved may still be improved, as is shown in
the forthcoming sections, through the use of a properly designed
data fusion mechanism. In the simplified 2D estimator formulation
adopted, the expressions used by the PPF at the prediction and up-
date stages of the filter reduce to the PF components of the RBPF
formulation, i.e., to Eqs. (3) and (6)–(8).
4.3. Data fusion using the Rao-Blackwellized particle filter
The objective of the forthcoming analysis is to clarify the mech-
anism by which the marginalized particle filter integrates DVL
measurements with TAN estimates in a stochastic framework; this
170 F. Curado Teixeira et al. / Annual Reviews in Control 42 (2016) 166–176
Fig. 1. Conceptual model of the data fusion filter implemented by the RBPF.
study provided us with valuable insight to develop the new fusion
filter described in the next section.
The structure of the Rao-Blackwellized particle filter presented
above has been successfully applied to navigation problems; see
e.g. Gustafsson et al. (20 02), Karlsson and Gustafsson (20 03),
Frykman (2003), Nordlund and Gustafsson (2008). In Teixeira
(2007), we implemented a PPF with this structure to solve the
TAN problem. In spite of the success achieved with RBPF imple-
mentations in terms of long-term estimation stability, the results
obtained are characterized by position estimates affected by high-
frequency noise. Clear illustrations of this problem can be found
in the above cited references. We show next that this undesirable
effect is a consequence of the structure of the RBPF which mech-
anizes a data fusion filter in a stochastic framework. To address
this problem we propose an alternative implementation that uses
a complementary filter to fuse the Doppler navigation data with
the TAN estimates taking in account the complementary spectral
characteristics of the signals supplied by the TAN and DVL mod-
ules.
Careful inspection of Eqs. (3) to (5) reveals some details of the
RBPF implementation that will be useful in the analysis that fol-
lows. The observation used in the Kalman estimator consists of the
p f,i
distance between the position represented by xk and the posi-
p f,i
tion represented in its immediate successor, xk+1 as predicted by
the PF in the time interval of one iteration. This observation is
used by the Kalman filter in the estimation of the velocity bias,
the KF part of the state vector. The innovation ν k is the difference
p f,i p f,i
between the observed distance xk+1 − xk and the predicted dis-
tance traveled obtained by dead-reckoning with two components:
p f,i
the measured velocity, Gu,k Vk and the velocity bias estimated by
k f,i
the filter F p f xˆ k|k−1 . Notice that both terms are expressed in frame
{I} after the transformation of Vk from {B} to {I} implemented by
matrix Gu, k , which represents the closed form integral of the ro-
tation executed in the time interval of one iteration, assuming a
constant yaw rate. The covariance Pk|k and the Kalman gain Kk are
the same for all particles, a simplification that greatly reduces the
computational effort involved in the calculations for large sets of
particles.
For the purpose of the present analysis let v denote the true
velocity of the vehicle expressed in {I} and notice that the variable
Vk used in the filter equations denotes a noisy measurement of V
which is expressed in frame {B}. Let eV represent the measurement
error of the velocity vector V obtained by the Doppler; notice that
the DVL measurement bias included in eV is constant in {B} but
not in {I}. Additionally, consider the following variables expressed
in frame {I} that are represented in Figs. 1 and 3: eˆ V denotes the
Kalman estimation of the Doppler velocity bias; pD is the dead-
reckoning position estimation obtained by integration of the DVL
velocity measurement corrected of the bias term; the residual er-
ror affecting pD is denoted eD ; pT is the TAN position estimate and
eT is the corresponding estimation error.
One of the disadvantages of the filter realization under analy-
sis is the estimation of the velocity bias in the inertial reference
frame, {I}. This prevents the bias estimate from reaching steady
state when the vehicle modifies its orientation. Another limita-
tion of this filter structure is related to the nature of the parti-
cle filter estimator embodied in the Rao-Blacwellized particle fil-
ter. Eqs. (7) and (3) reveal that RBPF position estimates consist of
the weighted average of the particles predicted by dead-reckoning,
spread by the addition of process noise, ζk , and filtered by the
pf
measurement likelihood. Hence, the variability of this output re-
flects the process noise intensity that in common particle filter im-
plementations is artificially increased to solve typical problems of
particle degeneracy and collapse; see e.g. Nordlund and Gustafs-
son (2008), eq. (66). It further reflects the effects of measurement
noise and the uncertainty introduced by terrain symmetries that
may cause the measurement model to weight some particles in-
correctly and lead to the acceptance of outliers. This problem has
been analyzed in some detail in Teixeira et al. (2012a), Maurya,
Teixeira, and Pascoal (2012). The RBPF filter output is thus in-
evitably affected by significant noise intensity with a maximum
frequency equal to the particle filter update frequency.
The interpretation of the Rao-Blackwellized PF as a data fusion
filter has been introduced in former publications but without a de-
tailed exposition of the fusion structure; see e.g. Frykman (2003),
Nordlund and Gustafsson (2008). For the sake of clarity, we present
in Fig. 1 a conceptual model of the filter discussed above. Abstract-
ing from the details of the different filters found in the literature,
this model evidences the general structure of common RBPF imple-
mentations: the dead-reckoning data is used to aid the TAN filter
which in turn outputs the position estimates. This is in contrast
with typical procedures used for example in aided inertial naviga-
tion where GPS is used as an aiding source to INS. We state that
 F. Curado Teixeira et al. / Annual Reviews in Control 42 (2016) 166–176
Fig. 2. Conceptual model of the system that provides the signals to be filtered.
in principle, the fusion filter should use the signal with predomi-
nantly low-frequency noise, the DVL in the present case, to provide
the reference trajectory against which the aiding data supplied by
TAN should be compared.
5. Complementary filter for terrain-aided Doppler navigation
5.1. TAN and DVL signals
The design of stochastic data fusion filters requires knowledge
of the covariances that model the correlation between signals. In
many practical problems it is difficult to characterize the signals
as random processes and to model their correlation by the re-
spective covariances. Covariance intersection (Julier & Uhlmann,
1997) is a method that addresses this problem when the corre-
lations are difficult to quantify but uses covariance representations
that tend to be too conservative. When the signals have comple-
mentary spectral characteristics, it is often advantageous to fuse
them by resorting to complementary filtering (CF), a data fusion
methodology with roots in Wiener filter theory; see e.g. Brown and
Hwang (1985). Complementary filtering is applicable when redun-
dant measurements of a same signal or some of its time deriva-
tives are available with measurement disturbances in complemen-
tary frequency regions. The problem to be solved consists in fus-
ing all the information in order to reject the instrumentation er-
rors without distorting the signal. This approach has witnessed
widespread application in navigation problems especially in the in-
tegration of inertial navigation systems with navigation data from
aiding sources.
The problem that we tackle here is that of fusing the estimates
of position supplied by a TAN filter with DVL velocity measure-
ments. A related navigation problem presented in Pascoal, Kaminer,
and Oliveira (20 0 0) is that of complementing position information
available from GPS with the velocity data provided by a Doppler
sonar system.
5.2. Complementary filter design
In the present work we borrow from Pascoal et al. (20 0 0) some
of the methods used in the design of a time-varying complemen-
tary filter. We also reproduce here with due adaptations the com-
plementary filter setup derived in Teixeira et al. (2012b). The sys-
171
the vehicle expressed in {I} and notice that the variables uk and
vk used in the particle filter equations denote noisy measurements
of the velocity expressed in frame {B}. R(t) is the time-varying ro-
tation matrix from {B} to {I}. Additionally, consider the following
variables expressed in frame {I} that are represented in Fig. 3: pD
is the dead-reckoning position estimation obtained by integration
of the DVL velocity measurement corrected of the bias term; the
residual error affecting pD is denoted eD ; pT is the TAN position
estimate and eT is the corresponding estimation error.
Let p(s) and v(s) denote the Laplace transforms of p and v re-
spectively and consider the decomposition
s+k k s
p (s ) = p (s ) = p (s ) + p ( s ),
s+k s+k s+k
with k=const. Using the relationship v(s ) = sp(s ) we obtain
p(s ) = Fp (s )p(s ) + Fv (s )v(s ) = Fp (s )p(s ) + Fv (s )sp(s ),
where Fp (s ) = s+k k , Fv (s ) = s+1 k , and Fp (s ) + Fv (s ) = I.
The former equations suggest a filter with the following struc-
ture that preserves the signal p:
pˆ = Fp pT + Fv vD ,
where Fp and Fv are linear time-invariant operators with trans-
fer functions Fp (s) and Fv (s ), respectively. Denoting by Tp and Tv
the linear time-invariant operators with transfer functions Tp (s ) =
Fp (s ) and Tv (s ) = I − Tp (s ), it is easy to show that
pˆ = (T p + Tv )p + Fp eT + Fv eV .
Noting that Tp (s) is a low-pass filter, the previous equation re-
veals the following characteristics of the filter operation:
• The filter fuses the data provided by TAN at low frequency with
the measurements available from the Doppler in the comple-
mentary region of the spectrum.
• The filter output consists of an undistorted copy of the original
signal p plus corrupting terms that depend on the disturbances
eT and eV .
To achieve steady-state rejection of the Doppler bias, the filter
is augmented with an integrator leading to the filter structure rep-
resented in Fig. 3 that admits the following realization:
tem that generates the signals available for filtering is represented     
by the conceptual model depicted in Fig. 2. In the data-fusion fil- x˙ 1 −K1 I2x2 R(t ) x1
=
ter formulation described below let v denote the true velocity of x˙ 2 −K2 R(t )−1 02x2 x2
172 F. Curado Teixeira et al. / Annual Reviews in Control 42 (2016) 166–176

Fig. 3. Structure of the complementary filter proposed to integrate TAN with DVL.

  
R(t )
6
x 10
K1 I2x2 pT
+ (10) Ground−truth. GPS−RTK

K2 R(t )−1 02x2 vD 4.2909 Dead−reckon. DVL

Dead−reckon. DVL+External IMU

 
 x1 4.2908

pˆ = I2x2 02x2 (11)

x2 ,
4.2908
Y [North] − Local Coordimates (UTM)[m]

where x1 = [x, y]T and x2 = [bx , by ]T , with K1 and K2 denoting the 4.2908
CF filter gains.
Eqs. (10) and (11) are represented in compact form as
4.2907

x˙ = A(t )x + B(t )U (12)

4.2907

4.2906
y = Cx, (13)
T
with x = [x1 , x2 ] , U = [pT , vD ]T , y = pˆ. A(t) and B(t) correspond to 4.2905

the time-varying matrices that multiply vectors x and U in Eq. (10), 4.917 4.9175 4.918 4.9185 4.919 4.9195 4.92 4.9205 4.921
X [East] − Local Coordimates (UTM)[m]
and C =[I2x2 02x2 ].
5
x 10

The filter state dynamics in discrete time is given by: Fig. 4. Aerial view of the trial site. Superimposed on the photo are the true tra-
 t
jectory followed by the vehicle (blue line starting at the NE corner) and the dead-
reckoning estimates obtained with different motion sensor units (red and magenta).
x(t ) =
(t, t0 )x(t0 ) +
(t, τ )B(τ )Udτ ,
t0

where
(t, t0 ) is the state transition matrix for the time inter-
˙ t, τ ) =
(t, τ )A(τ ). This matrix may be
val [t0 , t] that satisfies
(
computed using differential equation solvers available in standard
calculation packages such as Matlab.
6. TAN experiments with in-water collected data
In the present section, we illustrate the application of the above
derived complementary filter to the navigation of an autonomous
marine vehicle.
6.1. Site Description and TAN Map
The site chosen for the experimental tests is a very shallow-
water lake with an area of approximately 300m x 200m located at
Doca do Oceanário - Parque das Nações (38.765°N, 9.093°W), in Lis-
bon; see Fig. 4. The prior map used by the TAN algorithms was ob-
tained by interpolation of bathymetric data in a regular grid with
0.5m spacing. The topography of the site, shown in Fig. 5, is very
smooth except on the boundaries and in the Northern part of the
lake where a ramp facilitates the access to the water.
6.2. Configuration of the TAN experiments
6.2.1. Robotic plataform
The robotic platform used in the experimental tests is an au-
tonomous vehicle of the class Medusa developed by the Institute
for Systems and Robotics of Instituto Superior Técnico (ISR/IST).
The Medusa can be configured as autonomous underwater vehicles
(AUVs) or autonomous surface vehicles (ASVs); see Fig. 6. The ASV
configuration used in the present trials includes an external GPS
antenna which was used only to track the vehicle using real-time
kinematics (RTK) GPS; the positioning data so obtained is used as
ground truth to determine the navigation errors of TAN and dead-
reckoning. In the trials, the ASV, used as a proxy to an AUV, moved
with a surge velocity of 0.5m/s. The real path followed by the ve-
hicle is shown in Fig. 4, together with the dead-reckoning esti-
mates based on DVL velocity measurements. In this experiment,
the Doppler was used in a bottom-locking mode, thus providing
measurement of the velocity with respect to the bottom.
6.2.2. Sensors specifications
The DVL used in this experiments is a LinkQuest NavQuest 600
Micro Doppler velocity logger with a classical Janus four beam con-
figuration. This DVL is a relatively inexpensive system which is
 F. Curado Teixeira et al. / Annual Reviews in Control 42 (2016) 166–176 173

Depth (m) Table 1

−0.5 Specifications of the DVL used in the trials.

LinkQuest-NavQuest 600 MicroDVL

Sensor function Velocity sensor and altimeter

600 Frequency 600kHz
−1
Maximum range 110m
Range accuracy 0.1m
Beam-width 22° (4-beam)

500 −1.5

6.2.3. TAN filters configurations

In the present context, the method of navigation by dead-
Northing (0.5m)

400 −2 reckoning based exclusively on Doppler velocity measurements is

designated DVL-Nav. In what follows we compare the results of
TAN navigation using three implementations:

300 −2.5 • TAN/DVL-RBPF, Rao-Blackwellized particle filter: this version

mechanizes a data fusion procedure using a marginalized par-
ticle filter. The filter estimates the 2D position as well as the
2D components of the DVL velocity bias. Its structure is repre-
200 −3 sented by the diagram in Fig. 1.
• TAN/DVL-PF, Doppler based TAN filter: this filter uses Doppler
velocity and range measurements relative to the sea-bottom to
perform terrain-based navigation. The DVL velocity is input to
100 −3.5
the kinematic model without explicit integration of TAN and
DVL data.
• TAN/DVL-CF, TAN aided Doppler navigation using complemen-
−4
tary filtering: the TAN estimates based on range Doppler mea-
0 100 200 300 400 482 surements are fused with Doppler velocity data using a com-
Easting (0.5m) plementary filter. The filter structure is depicted in Fig. 3.

Fig. 5. Prior bathymetric map of the TAN trial area. The error plots in Fig. 7 and the histograms of the errors pre-
sented in Fig. 8 compare the estimation accuracies achieved by
the TAN/DVL-PF and the TAN/DVL-RBPF. As shown by these plots,
there is no advantage in the utilization of the marginalized parti-
cle filter which is significantly more demanding in terms of com-
putationl resources. Hence, in the next sections we compare only
the TAN/DVL-PF and the TAN/DVL-CF in terms of the position esti-
mation accuracy achieved with the real data acquired in the ter-
rain navigation experiments. The reader is invited to check the
filter configuration parameters in Table 3 and refer to Teixeira
(2007) and Teixeira et al. (2012a) for a detailed analysis of the per-
formance of different PF algorithms applied to the TAN problem.
In order to assess the computational effort imposed by the dif-
ferent algorithms, we employ as metrics the maximum amount of
random-access memory (RAM) used during the simulations and
the average time of CPU spent in the execution of a filter itera-
tion. The algorithms, whose parameters are summarized in Table 3,
were executed with a Toshiba Satellite A100 laptop with the fol-
lowing characteristics: CPU Intel T2400@1.83GHz; 2GB of RAM;
operating system Windows XP Pro SP3. We notice that the RAM
capacity required to execute the algorithms was always below
1 Mbyte and the largest iteration time observed was smaller than
25 miliseconds. These results, in conjuction with the average val-
Fig. 6. The Medusa ASV in the water at the test site.
ues presented in Table 4, may be used as indicators of the com-
putational resources required to run the corresponding algorithms
on-board the marine robotic vehicle.

installed as a standard navigation instrument of the Medusa ASV.
Besides providing velocity measurements used for dead-reckoning,
the unit can acquire four depth measurements at each ping, with
an approximate accuracy of 10cm; the DVL is also equipped with
a motion sensor which provides relatively noisy measurements of
the sensor attitude. A separate motion reference unit, that provides
more accurate measurements of attitude and angular velocities, is
installed on-board the vehicle: a VectorNav VN-100 miniature IMU;
see equipment specifications in Tables 1 and 2.
6.3. Presentation and discussion of experimental results
6.3.1. Navigation data issues and fault detection
The analysis of the navigation data acquired in the trials re-
veals a heading bias which is not constant along the experiment;
actually it is practically absent during some legs of the path fol-
lowed by the vehicle. In our opinion, the bias is due to the in-
duced effects of the ferro-magnetic materials incorporated in the
walls and the nearby buildings which enclose the area where the
174 F. Curado Teixeira et al. / Annual Reviews in Control 42 (2016) 166–176

Table 2
Specifications of motion sensors used in the trials.

VectorNav LinkQuest
VN-100 NavQuest

Sensor function Motion sensor

√ DVL built in motion sensor
Ang. rate noise roll, pitch, yaw 0.0035◦ /s/ Hz NA
Head. accuracy: Static, Dynamic 2°, 2° 2°, NA
Roll, pitch acc.: Static, Dynamic 0.5°, 1° 0.5°, NA

Localization error of TAN PF algorithms

40
TAN error: 2D PF
TAN error: 4D RBPF
30
error (m)

20

10

0
0 200 400 600 800 1000 1200 1400 1600 1800 2000
t (sec)
Fig. 7. Absolute value of the localization errors obtained with TAN/DVL-RBPF (4D) and TAN/DVL-PF (2D).

3000 Table 4
TAN 2D PF RAM and CPU time per iteration used by the algorithms.
TAN RBPF
TAN/DVL-RBPF TAN/DVL-PF TAN/DVL-CF
2500
RAM (kB) 522 490 920
CPU (sec) 0.02129 0.01425 0.01630

2000
Frequency

1500
experiments were conducted. The very large magnetic anomalies
induced by these infrastructures are associated to very large gradi-
1000
ents of the magnetic field intensity that exceed the gradient toler-
ances of most magnetic sensors. In particular, the strong magnetic
anomaly and the extremely large gradient of the total magnetic
500 field caused by the Oceanarium building at the Southern part of
the lake (Fig. 4), constitute the most plausible cause of the large
bias on the heading measured in that area by the DVL built-in
0
−2 0 2 4 6 8 10 12 14 16 18 20 22 24 26 28 30 32 34
compass. Because these biases are transient and their magnitude is
Magnitude of position error (m) unpredictable, they become impossible to calibrate. The orientation
errors affect not only the estimation of the pose of the vehicle by
Fig. 8. Histograms of the magnitude of the position estimation errors obtained with dead-reckoning, but also the prediction of altitude measurements
TAN/DVL-RBPF (4D) and TAN/DVL-PF (2D).
used by the TAN algorithm; these measurements are predicted by
Table 3 simulating the intersection of the acoustic beams with the bottom
Parameters used in the tests. surface represented in the reference map. Despite the large estima-
Parameter value units tion error caused by this deficiency, the TAN filter was able to con-
verge approximately to the true position when the vehicle reached
Filter update frequency 10 Hz
PF: num. of standard particles, N 500
the area of larger terrain slope observed in the Northern part of
PF: num. of extra particles, Nextra 100 the lake; see the trajectory estimated by TAN with this sensor con-
RBPF: num. of standard particles, N 20 0 0 figuration in Fig. 9. Recovery from filter divergence was achieved in
RBPF: num. of extra particles, Nextra 200 this experiment through the execution of a failure detection mech-
Complementary filter gains: K1 , K2 0.15, 0.01
anism embodied in the TAN filter that monitors the sum of the
DVL range measurements std.dev, σ r 0.05 m
DVL vel. noise std.dev, σu = σv 5x10−3 m/sec weights of the particles, w . A value of w which holds smaller
DVL static bias, bs 2x10−3 m/sec than a pre-defined threshold for several iterations is an indicator
DVL dynamic bias, bv 0.4%V m/sec that the filter has diverged; the threshold used in the current tests
Filter update frequency, T −1 2 Hz
is presented in Table 3. In this situation, the filter is re-initialized
Intensity of PF synthetic noise, diag(Qadd ) [1, 1]T m
Threshold on w 10−15
by distributing the particles uniformly over a large area that may
correspond to the whole map.
 F. Curado Teixeira et al. / Annual Reviews in Control 42 (2016) 166–176 175

Depth Localization error: DVL+External MRU dead−reckoning vs. TAN

True (GPS−RTK) 50
DVL−NAV (dead−reckon.) DVL−NAV
600 TAN/DVL−PF TAN/DVL−PF
45
Depth (m) TAN/DVL−CF
−0.5
40
500
−1 35
Northing (0.5 meters)

400 30
−1.5

error (m)
25
300 −2
20
−2.5
200
15

−3 10
100
5
−3.5

0
50 100 150 200 250 300 350 400 450 −4
0 500 1000 1500 2000
Easting (0.5 meters) t (sec)

Fig. 9. True trajectory and estimated trajectories obtained by TAN/DVL-PF and Fig. 11. Absolute value of the localization errors obtained with TAN/DVL-PF,
dead-reckoning using the DVL built-in motion sensor. TAN/DVL-CF, and dead-reckoning performed with the DVL and the external MRU
data.

Depth
True (GPS−RTK)
DVL−NAV (dead−reck. DVL+MRU) trajectory where TAN benefits from the large terrain gradient ob-
TAN/DVL−PF
TAN/DVL−CF
served in the area of the ramp.
600
Depth (m)
6.3.3. TAN/DVL-CF results
−0.5
In the above presented results it is possible to notice a com-
500
mon deficiency of the output of terrain navigation algorithms: the
−1
short-term variability of position estimates that can be observed in
Northing (0.5meters)

400
300
200
100
50 100 150 200 250 300 350 400
Easting (0.5meters)
Fig. 10. True trajectory and dead-reckoning estimate obtained with the DVL plus
the external motion sensor, compared with the trajectories estimated by TAN/DVL-
PF and TAN/DVL-CF. The window overimposed on the right side of the figure shows
a zoom of the small box inset with the trajectories estimated by the TAN and the
Complementary filter to permit a comparison of the character of the solutions ob-
tained with the two methods.
Due to the large positioning errors introduced by the motion
sensor integrated in the DVL, in the remainder of this subsection
we will only discuss in detail the results obtained with the exter-
nal attitude unit. During the tests it was also observed that one of
the DVL beams (beam1) failed frequently and introduced large er-
rors in the altitude measurements. For this reason, the utilization
of this beam was inhibited in the TAN filter mechanization which
is programmed to deal with this type of sensor failures.
6.3.2. TAN/DVL-PF results
As can be observed in Fig. 10, the TAN-estimates follow closely
the true trajectory. The larger deviation occurs in the Southern leg
where the topography does not convey enough information to en-
able the correction of the drift accumulated in the center of the
lake. Fig. 11 shows the error decreasing in the final part of the
−1.5
the plots of Figs. 10 and 11. To mitigate this problem the comple-
mentary filter described in Section 5 was applied to fuse the TAN
−2
and DVL navigation data. As can be observed in Fig. 11, in general
the output of the CF is characterized by smaller estimation errors
−2.5 and reduced short-term variability; the smoothness of the comple-
mentary filter output, in contrast with the ’wiggled’ characteristics
−3 of the conventional TAN filter, is also evidenced by the plots of the
estimated trajectories zoomed in Fig. 10. These advantages are ev-
−3.5 idenced especially in the second half of the trajectory (starting ap-
proximately at iteration number 0.9x104 ) where, due to insufficient
450
−4 terrain information, the single TAN filter builds a bias in the along-
track direction that is attenuated by the complementary filter. This
result demonstrates the importance of incorporating conveniently
designed data fusion filters in TAN applications Fig. 12.
7. Conclusions
The paper described a class of navigation and data-fusion fil-
ters developed by the authors and illustrated their application us-
ing real data acquired in experimental trials in the water. The key
contributions of the paper included a simplified sensor configura-
tion based on a single Doppler sonar and the set-up of a comple-
mentary filter for fusion of terrain-based estimates of position with
Doppler velocity measurements. The navigation methods proposed
showed their efficacy when using low-resolution altitude sensors
and proved to be robust in the presence of large orientation bi-
ases and missing range measurements. The results obtained with
experimental data evidenced the superior performance of the com-
plementary filter in terms of position estimation accuracy, demon-
strating the potential improvement of classical TAN implementa-
tions that can be achieved through the application of properly de-
signed data fusion filters.
Due to the high diversity of terrain scenarios and system con-
figurations used in the works reported in the literature and in our
experimental trials, it is not possible to make a fair comparison
176 F. Curado Teixeira et al. / Annual Reviews in Control 42 (2016) 166–176
12000
DVL−NAV
TAN/DVL−PF
10000 TAN/DVL−CF
8000
Frequency
6000
4000
2000
0 Meduna, D., Rock, S., & McEwen, R. (2010). Closed-loop terrain relative navigation
0 2 4 6 8 10 12 14 16 18 20 22 24 26 28 30 32 34
Magnitude of position error (m)
Fig. 12. Histograms of the magnitude of the position estimation errors obtained
with DVL-NAV, TAN/DVL-PF, and TAN/DVL-CF.
between the navigation accuracy obtained with the present TAN
solution and that achieved by the above mentioned state-of-the-
art navigation systems. We consider however that the present re-
sults evidence the ability of TAN to provide accurate localization
estimates even in poorly informative terrain and using standard,
relatively inexpensive navigation sensors.
The exploration of these methods in terrain navigation solutions
which include complementary sources of terrain information, such
as the geomagnetic field, is the subject of ongoing research.
Acknowledgements
We are indebted to our colleagues at ISR/IST who offered untir-
ing support and many useful suggestions to carry out the experi-
mental tests.
References
Anonsen, K. B., Hagen, O. K., Hegrenaes, O., & Hagen, P. E. (2013). The HUGIN AUV
terrain navigation module. In Oceans-2013, San Diego, CA (pp. 1–8).
Anonsen, K. B., & Hallingstad, O. (2006). Terrain aided underwater navigation us-
ing point mass and particle filters. In IEEE/ION position location and navigation
symposium, San Diego, CA (pp. 1027–1035).
Arulampalam, S., Maskell, S., Gordon, N. J., & Clapp, T. (2002). A tutorial on particle
filters for on-line nonlinear/non-Gaussian Bayesian tracking. IEEE Transactions
on Signal Processing, 50(2), 174–188.
Bergman, N. (1999). Recursive Bayesian estimation - navigation and tracking appli-
cations. Sweden: Linköping University PhD Thesis.
Blackwell, D. (1947). Conditional expectation and unbiased sequential estimation.
Annals of Mathematical Statistics, 18(1), 105–110.
Brokloff, N. (1994). Matrix algorithm for doppler sonar navigation. IEEE/MTS
Oceans’94, Brest, France.
Brown, R. G., & Hwang, P. Y. (1985). Introduction to random signals and applied
Kalman filtering. Wiley.
Cappé, O., Godsill, S. J., & Moulines, E. (2007). An overview of existing methods and
recent advances in sequential Monte Carlo. Proceedings of the IEEE, 95(5).
Doucet, A., Godsill, S., & Andrieu, C. (20 0 0). On sequential Monte Carlo sampling
methods for Bayesian filtering. Statistics and Computing, 10(3), 197–208.
Frykman, P. (2003). Applied particle filters in integrated aircraft navigation. Sweden:
Linköping University MSc Thesis.
Gustafsson, F., Gunnarsson, F., Bergman, N., Forssell, U., Jansson, J., Karlsson, R., &
Nordlund, P. J. (2002). Particle filters for positioning, navigation and tracking.
IEEE Transactions on Signal Processing, 50(2).
Hagen, O. K., Anonsen, K. B., & Mandt, M. (2010). The HUGIN real-time terrain nav-
igation system. OCEANS, 2010, 1–7.
Ho, Y., & Lee, R. (1964). A Bayesian approach to problems in stochastic estimation
and control. IEEE Transaction on Automatic Control, AC-9(4), 333–339.
Julier, S., & Uhlmann, J. K. (1997). A non-divergent estimation algorithm in the pres-
ence of unknown correlations. American Control Conference, 4, 2369–2373.
Karlsson, R., & Gustafsson, F. (2003). Particle filter for underwater terrain navigation.
Technical Report LiTH-ISY-R-2530. Sweden: Linköping University.
Liu, J. S., & Chen, R. (1998). Sequential Monte Carlo methods for dynamic systems.
Journal of the American Statistical Association, 93(443), 1032–1044.
Majumder, S. (2001). Sensor fusion and feature based navigation for subsea robots.
Australia: University of Sydney PhD Thesis.
Maurya, P., Teixeira, F. C., & Pascoal, A. (2012). Complementary terrain/single bea-
con-based AUV navigation. IFAC workshop on navigation, guidance and control
of underwater vehicles (NGCUV-2012), Porto, Portugal.
for AUVs with non-inertial grade navigation sensors. In Autonomous underwa-
ter vehicles (AUV), 2010 IEEE/OES, Monterey, CA (pp. 1–8).
Meduna, D. K., Rock, S. M., & McEwen, R. (2008). Low-cost terrain relative naviga-
tion for long-range AUVs. OCEANS’2008. MTS/IEEE oceans conferences, Quebec,
CA.
Melo, J., & Matos, A. (2013). On the use of particle filters for terrain based navigation
of sensor-limited AUVs. In OCEANS 2013 MTS/IEEE (pp. 294–301). Bergen: IEEE.
Morice, C., Veres, S., & McPhail, S. a. (2009). Terrain referencing for autonomous
navigation of underwater vehicles. Oceans 2009 - Europe, Bremen, Germany.
Newman, P. M. (1999). On the structure and solution of the simultaneous localisa-
tion and map building problem. Australia: University of Sydney PhD Thesis.
Nordlund, P. J. (2002). Sequential Monte Carlo filters and integrated navigation.
Sweden: Linköping University Licenciate thesis.
Nordlund, P. J., & Gustafsson, F. (2008). Research on an improved terrain aided posi-
tioning model. Technical Report LiTH-ISY-R-2870. Sweden: Linköping University.
Nordlund, P. J., & Gustafsson, F. (2009). Marginalized particle filter for accurate and
reliable terrain-aided navigation. IEEE Transaction on Aerospace and Electronic
Systems, 5(4), 1385–1399.
Nygren, I. (2005). Terrain navigation for underwater vehicles. Sweden: Royal Insti-
tute of Technology (KTH) PhD Thesis.
Nygren, I., & Jansson, M. (2004). Terrain navigation for underwater vehicles using
the correlator method. IEEE Journal of Oceanic Engineering, 29(3), 906–915.
Pascoal, A., Kaminer, I., & Oliveira, P. (20 0 0). Navigation system design using time–
varying complementary filters. IEEE Transaction on Aerospace and Electronic
Systems, 36(4), 1099–1115.
Pitt, M., & Shephard, N. (1999). Filtering via simulation: auxiliary particle filters.
Journal of the American Statistical Association, 94(446), 590–599.
Shidan, L., Liguo, S., Xin, L., & Desheng, W. (2011). Research on an improved terrain
aided positioning model. Journal of Software, 6(5).
Siouris, G. M. (2004). Missile guidance and control systems. Springer.
Tanner, M. A. (1996). Tools for statistical inference. Springer Series on Statistics.
Springer-Verlag.
Teixeira, F. C. (2007). Terrain-aided navigation and geophysical navigation of au-
tonomous underwater vehicles. Lisbon, Portugal: Instituto Superior Técnico PhD
Thesis.
Teixeira, F. C., Pascoal, A., & Maurya, P. (2012a). A novel particle filter formulation
with application to terrain-aided navigation. IFAC workshop on navigation, guid-
ance and control of underwater vehicles (NGCUV’ 2012), Porto, Portugal.
Teixeira, F. C., Quintas, J., & Pascoal, A. (2012b). AUV terrain-aided Doppler naviga-
tion using complementary filtering. 9th IFAC conference on manoeuvring and
control of marine craft (MCMC-2012), Arenzano, Italy.
Teixeira, F. C., Quintas, J., & Pascoal, A. (2015). AUV terrain-aided navigation using a
Doppler velocity logger. IFAC workshop on navigation guidance and control of
underwater vehicles (NGCUV’2015), Girona, Spain.
Xie, Y. (2005). Terrain aided navigation. Sweden: Royal Institute of Technology (KTH)
MSc Thesis.