- Email: [email protected]
AUV Terrain-Aided Navigation using a Doppler Velocity Logger★
Proceedings of the IFAC Workshop on Navigation, Guidance Proceedings of the IFAC Workshop on Navigation, Guidance and Control ofofUnderwater Vehicles on Proceedings the IFAC IFAC Workshop Workshop on Navigation, Navigation, Guidance Guidance Proceedings the and Control ofofUnderwater Vehicles Available online at www.sciencedirect.com April 28-30, 2015. Girona, Spain and of Vehicles and Control Control of Underwater Underwater Vehicles April 28-30, 2015. Girona, Spain April April 28-30, 28-30, 2015. 2015. Girona, Girona, Spain Spain
ScienceDirect
AUV AUV AUV
IFAC-PapersOnLine 48-2 (2015) 137–142
Terrain-Aided Navigation using Terrain-Aided Terrain-Aided Navigation Navigation ⋆using using ⋆⋆ Doppler Velocity Logger Doppler Velocity Logger Doppler Velocity Logger
a a a
∗ ∗ ∗ Francisco Curado Teixeira a o Quintas o nio Pascoal ∗ Jo˜ ∗ Ant´ ∗ Francisco Curado Teixeira a o Quintas o nio Pascoal ∗ Jo˜ ∗ Ant´ ∗ ∗ ∗ Francisco Curado Teixeira Jo˜ a o Quintas Ant´ o nio Pascoal Francisco Curado Teixeira Jo˜ ao Quintas Ant´ onio Pascoal ∗ ∗ ∗ Institute of Systems and Robotics and Dept. Electrical Engineering, Institute of Systems and Robotics and Dept. Electrical Engineering, ∗ ∗ Institute of and and Dept. Electrical Instituto Superior T´ eecnico, Lisboa, Portugal(e-mail: Institute of Systems Systems and Robotics Robotics and Dept. Electrical Engineering, Engineering, Instituto Superior T´ cnico, Lisboa, Portugal(e-mail: Instituto Lisboa, [email protected]; [email protected]; Instituto Superior Superior T´ T´eecnico, cnico, Lisboa, Portugal(e-mail: Portugal(e-mail: [email protected]; [email protected]; [email protected]; [email protected]; [email protected]). [email protected]; [email protected]; [email protected]). [email protected]). [email protected]).
Abstract: The potential of terrain-aided navigation (TAN) of autonomous underwater vehicles Abstract: The potential of terrain-aided navigation (TAN) of autonomous underwater vehicles Abstract: The potential of terrain-aided navigation (TAN) of autonomous underwater vehicles (AUVs) has been demonstrated in the last ten years in a series of trials where high localization Abstract: The potential of terrain-aided navigation (TAN) of autonomous underwater vehicles (AUVs) has been demonstrated in the last ten years in a series of trials where high localization (AUVs) has been demonstrated in the last ten years in a series of trials where high localization accuracy was obtained during long periods of time in different types of terrain. Despite these (AUVs) has been demonstrated in the last ten years in a series of trials where high localization accuracy was obtained during long periods of time in different types of terrain. Despite these accuracy was obtained during long periods of time in different types of terrain. Despite these recent advances, it is recognized by the navigation community that further research is necessary accuracy was obtained during long periods of timecommunity in different types of terrain. Despite these recent advances, it is recognized by the navigation that further research is necessary recent advances, it is isinto recognized by the the navigation communityThe that present further research research is necessary necessary to transform TAN a mature navigation methodology. paper describes the recent advances, it recognized by navigation community that further is to transform TAN into a mature navigation methodology. The present paper describes the to transform TAN into a mature navigation methodology. The present paper describes the work developed towards the development of terrain navigation methods for small AUVs, relying to transform TAN into a mature navigation methodology. The present paper describes the work developed towards the development of terrain navigation methods for small AUVs, relying work developed towards the development of terrain navigation methods for small AUVs, relying on standard navigation sensors and dispensing with the need of dedicated sensors for terrain work developed towards the development of terrain navigation methods for small AUVs, relying on standard navigation sensors and dispensing with the need of dedicated sensors for terrain on navigation sensors and with the need dedicated sensors data acquisition. The research addresses implementation of terrain on standard standard navigation sensors described and dispensing dispensing withthe theproblem need of of of dedicated sensors for for data acquisition. The research described addresses the problem of implementation of terrain data acquisition. acquisition. The research research described addresses the problem of implementation implementation ofand terrain navigation in underwater scenarios characterized by smooth sea-bottom topography very data The described addresses the problem of of terrain navigation in underwater scenarios characterized by smooth sea-bottom topography and very navigation in underwater scenarios characterized by smooth sea-bottom topography and very shallow water, where the terrain information available for navigation is scarce. The TAN navigation in underwater scenarios characterized by smooth sea-bottom topography and very shallow water, where the terrain information available for navigation is scarce. The TAN shallow water, where the terrain information available for navigation is scarce. The TAN algorithms and the data fusion methods whose tests are documented in the paper build upon shallow water, where the terrain information available for navigation is scarce. The TAN algorithms and the data fusion methods whose tests are documented in the paper build upon algorithms and data fusion methods whose tests are in the build upon prior theoretical work published the authors. experimental results were obtained algorithms and the the data fusion by methods whoseThe tests are documented documented inreported the paper paper build upon prior theoretical work published by the authors. The experimental results reported were obtained prior theoretical theoretical work published by the the authors. The The experimental results reported were obtained recently through trials in the water performed with an autonomous surface vehicle acting as aa prior work published by authors. experimental results reported were obtained recently through trials in the water performed with an autonomous surface vehicle acting as recently through trials in the water performed with an autonomous surface vehicle acting as aa proxy to an AUV. recently through trials in the water performed with an autonomous surface vehicle acting as proxy to an AUV. proxy to an AUV. proxy to an AUV. © 2015, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Particle Keywords: Particle filters; filters; complementary complementary filtering; filtering; Doppler-based Doppler-based navigation; navigation; terrain-aided terrain-aided Keywords: Particle filters; complementary filtering; Doppler-based navigation; navigation Keywords: Particle filters; complementary filtering; Doppler-based navigation; terrain-aided terrain-aided navigation navigation navigation 1. run. The TAN solution which was first successfully applied 1. INTRODUCTION INTRODUCTION run. The TAN solution which was first successfully applied 1. run. The TAN solution which was first successfully applied in air vehicles, has received increased attention from the 1. INTRODUCTION INTRODUCTION run. The TAN solution which was first successfully applied in air vehicles, has received increased attention from the in air vehicles, has received increased attention from the underwater robotics community in the last ten years. This Conventional navigation systems currently in use by auin air vehicles, has received increased attention from the robotics community in the last ten years. This Conventional navigation systems currently in use by au- underwater underwater robotics community in the last ten years. This Conventional navigation systems currently in use by auis reflected in a series of demonstrations of the concept tonomous underwater vehicles (AUVs) are overly expenunderwater robotics community in the last ten years. This Conventional navigation systems currently in use by auis reflected in a series of demonstrations of the concept tonomous underwater vehicles (AUVs) are overly expen- is reflected in a series of demonstrations of the concept tonomous underwater vehicles (AUVs) overly through experimental in the water by sive severely their and reflected in a series trials of demonstrations ofperformed the concept tonomous underwater (AUVs) are areautonomy, overly expenexpenthrough experimental trials in the water performed by sive and and limit limit severelyvehicles their versatility, versatility, autonomy, and is through experimental trials in the water performed by distinct research groups. The work of Nygren and Jansson sive and limit severely their versatility, autonomy, and range of operation. To cope with the increasing demand through experimental trials in the water performed by sive and limit severely their versatility, autonomy, and research groups. The work of Nygren and Jansson range of operation. To cope with the increasing demand distinct distinct research groups. The work of Nygren and Jansson range of operation. To cope with the increasing demand (2004), Anonsen and Hallingstad (2006), and Morice et al. of low-cost, multipurpose underwater vehicles capable of distinct research groups. The work of Nygren and Jansson range of operation. To cope with the increasing demand and (2006), and al. of low-cost, multipurpose underwater vehicles capable of (2004), (2004), Anonsen Anonsen and Hallingstad Hallingstad (2006), and Morice Morice et etthe al. of multipurpose underwater vehicles capable of (2009), among others, demonstrated experimentally performing and long-range Anonsen and Hallingstad (2006), and Morice et al. of low-cost, low-cost, long-term multipurpose vehicles missions capable in of (2004), (2009), among others, demonstrated experimentally the performing long-term and underwater long-range oceanic oceanic missions in (2009), among others, demonstrated experimentally the potential of the TAN solution in different types of terrain. performing long-term and long-range oceanic missions in fully autonomous mode, it is necessary to develop a new (2009), among others, demonstrated experimentally the performing long-term and long-range oceanic missions in of the TAN solution in different types of terrain. fully autonomous mode, it is necessary to develop a new potential potential of TAN in types of fully autonomous mode, it is to commercial of aa terrain navigation solution with class of precise systems. potential of the theversion TAN solution solution in different different types of terrain. terrain. fully is necessary necessary to develop develop aa new new A A commercial version of terrain navigation solution with class autonomous of affordable, affordable,mode, preciseitnavigation navigation systems. A commercial version of a terrain navigation solution with class of affordable, precise navigation systems. the ability to exploit the altitude measurements provided A commercial version the of aaltitude terrain navigation solution with class terrain-aided of affordable, navigation precise navigation systems. the ability to exploit measurements provided The (TAN) approach has been theaability ability to exploit exploit the altitude altitude measurements provided by multibeam echo-sounder (MBE) or alternatively by a The terrain-aided navigation (TAN) approach has been the to the measurements provided by a multibeam echo-sounder (MBE) or alternatively by a The terrain-aided navigation (TAN) approach has been proposed during last as an alThe terrain-aided navigation (TAN) has been aa multibeam echo-sounder (MBE) or alternatively by a Doppler velocity logger (DVL) has been delivered recently; proposed during the the last decades decades as approach an economical economical al- by by multibeam echo-sounder (MBE) or alternatively by a Doppler velocity logger (DVL) has been delivered recently; proposed during the last decades as an economical alternative to the conventional navigation of AUVs based proposed during the last decades as an economical alDoppler velocity logger (DVL) has been delivered recently; see Anonsen et al. (2013). The TAN module is integrated ternative to the conventional navigation of AUVs based Doppler velocity logger (DVL) has been delivered recently; see Anonsen et al. (2013). The TAN module is integrated ternative to conventional navigation of AUVs based on high-grade inertial navigation systems (INS) and long ternative to the the conventional navigation AUVs see et The module is integrated the DVL-aided navigation system of the AUV on high-grade inertial navigation systems of (INS) andbased long with see Anonsen Anonsen et al. al. (2013). (2013). The TAN TAN module isHugin integrated with the DVL-aided navigation system of the Hugin on high-grade inertial navigation systems (INS) and long acoustic baselines. TAN relies on matching a set of range on high-grade inertial navigation systems (INS) and long with the the DVL-aided navigation system system of the Hugin Hugin AUV AUV which includes a state-of-the-art inertial measurement unit acoustic baselines. TAN relies on matching a set of range with DVL-aided navigation of the AUV which includes aa state-of-the-art inertial measurement unit acoustic baselines. TAN relies on a set measurements acquired sensors in acoustic baselines. TAN with reliessonar on matching matching set of of range range includes state-of-the-art inertial measurement unit (IMU) and a high-grade DVL, see Hagen et al. (2010). All measurements acquired with sonar sensors ainstalled installed in aa which which includes a state-of-the-art inertial measurement unit (IMU) and a high-grade DVL, see Hagen et al. (2010). All measurements acquired with sonar sensors installed in a vehicle with a previously acquired digital elevation map measurements acquired with sonar sensors installed in a (IMU) and a high-grade DVL, see Hagen et al. (2010). All the above mentioned navigation systems rely primarily on vehicle with a previously acquired digital elevation map (IMU) and a high-grade DVL, see Hagen et al. (2010). All the above mentioned navigation systems rely primarily on vehicle with a previously acquired digital elevation map (DEM) of the terrain to estimate position. In principle, vehicle with previously acquired position. digital elevation map navigation-grade the above mentioned navigation systems rely primarily on INS and top quality swath sonar instru(DEM) of thea terrain to estimate In principle, the above mentioned navigation systems rely sonar primarily on navigation-grade INS and top quality swath instru(DEM) of the terrain to estimate position. In principle, terrain navigation can be integrated with dead-reckoning (DEM) of the terrain to estimate position. In principle, navigation-grade INS and and to topmake quality swath sonar instruinstrumentation that contribute them too expensive. To terrain navigation can be integrated with dead-reckoning navigation-grade INS top quality swath sonar mentation that contribute to make them too expensive. To terrain navigation can be integrated with dead-reckoning to autonomous navigation systems capable terrain navigation can be integrated dead-reckoning that contribute make them too To the best of our knowledge, the first successful low-cost terto implement implement autonomous navigationwith systems capable of of mentation mentation that contribute to to make them too expensive. expensive. To the best of our knowledge, the first successful low-cost terto implement autonomous navigation systems capable of providing accurate estimates of position in the short term to implement autonomous navigation systems capable of the best of our knowledge, the first successful low-cost terrain navigation solution using non-dedicated sonar sensors providing accurate estimates of position in the short term the best of our knowledge, the first successful low-cost terrain navigation solution using non-dedicated sonar sensors providing estimates of in short term combined with localization the long providing accurate accurate estimates of position positionerrors in the thein rain solution using non-dedicated sonar sensors lower-accuracy inertial was first reported in combined with bounded bounded localization errors inshort the term long and rain navigation navigation solution usingsystems non-dedicated sonar sensors and lower-accuracy inertial systems was first reported in combined with bounded localization errors in the long combined with bounded localization errors in the long ⋆ and lower-accuracy inertial systems was first reported in Meduna et al. (2008). This research was supported in part by FCT project ATLAS and lower-accuracy inertial systems was first reported in ⋆ This research was supported in part by FCT project ATLAS Meduna et al. (2008). ⋆ Meduna et al. (2008). [PTDC/EEA-ELC/111095/2009], MORPH (EU FP7 grant This research research was was supported supported in inproject part by by FCT project project ATLAS ⋆ Meduna et al. (2008). This part FCT ATLAS [PTDC/EEA-ELC/111095/2009], project MORPH (EU FP7 grant As aa contribution to the development of aa cost-effective agreement No. 288704), and FCT [PEst-OE/EEI/LA0009/2011]. [PTDC/EEA-ELC/111095/2009], project MORPH As contribution to the development of cost-effective [PTDC/EEA-ELC/111095/2009], project MORPH (EU (EU FP7 FP7 grant grant agreement No. 288704), and FCT [PEst-OE/EEI/LA0009/2011]. As a contribution to the development of cost-effective terrain navigation system for AUVs, in et al. As a contribution to the development of aaTeixeira cost-effective agreement No. 288704), and FCT [PEst-OE/EEI/LA0009/2011]. The authorsNo. gratefully acknowledge the sponsorhip of the South terrain navigation system for AUVs, in Teixeira et al. agreement 288704), and FCT [PEst-OE/EEI/LA0009/2011]. The authors gratefully acknowledge the sponsorhip of the South terrain navigation system for AUVs, in Teixeira et al. (2012b) we proposed an implementation based on a Korean Agency for Defense Development under a collaborative The authors gratefully acknowledge the sponsorhip of the South terrain navigation system for AUVs, in Teixeira et al. The authors gratefully acknowledge the sponsorhip the South (2012b) we proposed an implementation based on a Korean Agency for Defense Development under a of collaborative (2012b) we proposed an implementation based on a Doppler velocity logger (DVL) integrated with standard, research agreement between KAIST and IST on the related theme Korean Agency for Defense Development under a collaborative (2012b) we proposed an implementation based on a Korean Agency for Defense Development under a collaborative Doppler velocity logger (DVL) integrated with standard, research agreement between KAIST and IST on the related theme Doppler velocity logger (DVL) integrated with standard, of Development of Terrain-Referenced and Geomagnetic Navigation research agreement between KAIST and IST on the related theme inexpensive motion sensors. The solution proposed, beDoppler velocity logger (DVL) integrated with standard, research agreement between KAIST and on the related theme of Development of Terrain-Referenced andIST Geomagnetic Navigation inexpensive motion sensors. The solution proposed, beTechniques for Enhanced Navigation Performance of UUVs. of Development of and Navigation inexpensive motion motion sensors. sensors. The The solution solution proposed, proposed, bebeof Development of Terrain-Referenced Terrain-Referenced and Geomagnetic Geomagnetic Navigation inexpensive Techniques for Enhanced Navigation Performance of UUVs. Techniques Techniques for for Enhanced Enhanced Navigation Navigation Performance Performance of of UUVs. UUVs.
Copyright IFAC 2015 137 Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright © 2015, IFAC 2015 137 Copyright IFAC 2015 137 Peer review© of International Federation of Automatic Copyright ©under IFAC responsibility 2015 137Control. 10.1016/j.ifacol.2015.06.022
138
Francisco Curado Teixeira et al. / IFAC-PapersOnLine 48-2 (2015) 137–142
sides using the Doppler-based velocity measurements for dead-reckoning, exploits the ability of the DVL to acquire periodically a set of altitude or slant-range measurements that are used by TAN algorithms for terrain matching. The implementation reported relies on a new particle filter algorithm, the Prior-correction Particle Filter (PPF) described in Teixeira et al. (2012a) and uses a complementary filter (CF) to fuse the TAN estimates with the deadreckoning data. The paper shows the advantages of using the novel particle filter and the CF data fusion mechanism in TAN applications; the performance of the methods proposed is illustrated in computer simulations using real bathymetry and simulated DVL data. In the current paper we present experimental results of terrain-aided navigation obtained recently in the scope of the research project ATLAS (http://atlas-geo.web.ua.pt). The project aimed at assessing the efficacy of TAN methods in different underwater scenarios, including relatively smooth sea-bottom topography and very shallow water, where the terrain information available for navigation is scarce. Another objective of the project was the implementation of terrain-aided navigation of AUVs using affordable navigation sensors and relying on non-dedicated sonar equipment for real-time acquisition of bathymetric data. For online acquisition of altitude data it was proposed to use the range measurements provided by a Doppler velocity logger which is a standard instrument used for dead-reckoning navigation in underwater robotics. The site used in the experimental trials is a very shallow-water lake located at Doca do Ocean´ ario - Parque das Na¸c˜oes, in Lisbon; see Figure 3. The results obtained in this scenario demonstrate the efficacy of the TAN solution and the robustness of the proposed estimation methods even in difficult operational conditions. 2. BASIC NOTATION, PROBLEM FORMULATION, AND TAN FILTERS SETUPS 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 represents roll, pitch and yaw angles that parametrize locally the orientation of {B} relative to {I}, and ω = [p, q, r]T represents the angular 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 problem, the terrain-aided navigation problem formulated above constitutes 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 138
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 sequential Monte Carlo estimation, using the above mentioned Prior-correction Particle Filter. 2.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 measurement models which are reproduced here with minor adaptations. The models are simplified 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 velocities relative to the seabed. As such, the TAN position estimates are computed by a 2D particle filter, instead of the four-dimensional Rao-Blackwellized PF version adopted in prior implementations. The notation used in the filter formulation is the following: x is a vector representing the system state with dimension nx and y is a measurement vector with dimension ny . N is the number of samples (particles) used by the particle filter, xit represents the ith particle (a random sample from the state space) at time instant t, and ωti denotes the weight associated to particle xit . Process and measurement models The discretetime process model is: 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, and ζk ∈ IRnx represents the process noise sequence. The input coupling matrix Gu,k is a function of the input vector uk = [vu , vv , vw , ψ, r, z]T and the product Gu,k uk represents the dead-reckoning incremental displacement which is as a function of linear and angular velocities, and orientation. The discrete-time measurement model with additive measurement noise is yk = h(xk ) + η(pk ), (2) where h(.) : IR2 → IRny represents the elevation map and η ∈ IRny models maps erros and range measurement noise which is a function of the 3D position, pk ; yk is a vector that represents the simultaneous measurements taken at each iteration. To model the acquisition of terrain elevation data using the Doppler unit, the observation model presented is adapted to account for a set of four simultaneous range measurements according to the Janus configuration (see e.g. Brokloff (1994)). 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 position and orientation, the measurement noise variables represented in the vector η are considered mutually independent and are characterized by the time-varying measurement noise intensity matrix Rk ; see Teixeira (2007). TAN/DVL-PPF: Particle filter set-up The simplified 2D estimator formulation uses a reduced input vector uk = [vu , vv , ψ, r, z]T where [vu , vv ]T denotes the 2D
Francisco Curado Teixeira et al. / IFAC-PapersOnLine 48-2 (2015) 137–142
velocity vector measured by the Doppler. The expressions used by the PPF at the prediction and update stages of the filter are: Prediction xik+1 = F xik + Gu,k uk + Lζk
,
(3)
where ζk ∼ N (0, Q).
139
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 respectively and consider the decomposition s+k k s p(s) = p(s) = p(s) + p(s), s+k s+k s+k
Update
with k=const. Using the relationship v(s) = sp(s) we obtain
The standard particle filter updates the weights as i wki = wk−1 p(yk |xik ),
p(s) = Fp (s)p(s) + Fv (s)v(s) = Fp (s)p(s) + Fv (s)sp(s),
while the PPF formulation in Teixeira et al. (2012a) uses
where Fp (s) =
i wki = wk−1 p(xk |xik−1 )p(yk |xik ),
where the likelihood p(yk |xk ) implicit in (2) is p(yk |xk ) = pη (yk − h(xk )) and the transition kernel embodied in (1) is p(xk |xik−1 ) = pζ (xk − F xik−1 − Gu,k−1 uk−1 ), with pη and pζ denoting the PDFs of the respective error models. Point estimates ˆ k and the associated A point estimate of the current state x covariance matrices Pk can be obtained trough: N x ˆMMS ωki xik (4) ≃ k i
Pk =
N i
i T ˆMMS . · xk − x ˆMMS ωki xik − x k k
(5)
2.2 Complementary filter for Terrain-Aided Doppler navigation Motivation Despite the important advantage of achieving bounded estimation error, TAN algorithms produce position estimates which are generally characterized by high-frequency noise. Due to space limitations we avoid discussing this issue here and encourage the interested reader to consult Teixeira et al. (2012b), where the causes of short-term variability of the TAN estimates are analyzed in detail. To address the problem, we proposed in the above mentioned work the fusion of the Doppler velocity data with the TAN position estimates by using a complementary filter (CF). This approach takes in account the complementary spectral characteristics of the signals supplied by the TAN and DVL modules. TAN/DVL-CF: Complementary filter setup We reproduce here with due adaptations the complementary filter setup derived in Teixeira et al. (2012b). The system that generates the signals available for filtering is represented by the conceptual model depicted in Figure 1. In the data-fusion filter formulation described below let v denote the true velocity of the vehicle expressed in {I} and notice that the variables vu and vv used in the particle filter equations denote noisy measurements of the velocity expressed in referential {B}. R(t) is the timevarying rotation matrix from {B} to {I}. Additionally, consider the following variables expressed in referential {I} that are represented in Figure 2: pD is the dead-reckoning position estimation obtained by integration of the DVL velocity measurement corrected of the bias term; the residual 139
k s+k ,
Fv (s) =
1 s+k ,
and Fp (s) + Fv (s) = I.
The former equations suggest a filter with the following structure that preserves the signal p: pˆ = Fp pT + Fv vD , where Fp and Fv are linear time-invariant operators with transfer 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ˆ = (Tp + Tv )p + Fp eT + Fv eV . Noting that Tp (s) is a low-pass filter, the previous equation reveals 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 complementary 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 represented in Figure 2 that admits the following realization: −K1 I2x2 R(t) x˙ 1 x1 = x˙ 2 x2 −K2 R(t)−1 02x2 K1 I2x2 R(t) pT (6) + vD K2 R(t)−1 02x2 x1 (7) pˆ = [ I2x2 02x2 ] x2 , where x1 = [x, y]T and x2 = [bx , by ]T , with K1 and K2 denoting the CF filter gains. Equations (6) and (7) are represented in compact form as x˙ = A(t)x + B(t)U (8) y = Cx, (9) with x = [x1 , x2 ]T , U = [pT , vD ]T , y = pˆ. A(t) and B(t) correspond to the time-varying matrices that multiply vectors x and U in equation (6), and C = [ I2x2 02x2 ]. The filter state dynamics in discrete time is given by: x(t) = Φ(t, t0 )x(t0 ) +
t
t0
Φ(t, τ )B(τ )Udτ
,
Francisco Curado Teixeira et al. / IFAC-PapersOnLine 48-2 (2015) 137–142
140
where Φ(t, t0 ) is the state transition matrix for the time ˙ τ ) = Φ(t, τ )A(τ ). This mainterval [t0 , t] that satisfies Φ(t, trix may be computed using differential equation solvers available in standard calculation packages such as Matlab.
6
x 10
Ground−truth. GPS−RTK Dead−reckon. DVL Dead−reckon. DVL+External IMU
4.2909
Y [North] − Local Coordimates (UTM)[m]
4.2908
4.2908
4.2908
4.2907
4.2907
4.2906
4.2905
4.917
4.9175
4.918
Fig. 1. Conceptual model of the system that provides the signals to
4.9185 4.919 4.9195 X [East] − Local Coordimates (UTM)[m]
4.92
4.9205
4.921 5
x 10
be filtered.
Fig. 3. Aerial view of the trial site. Superimposed on the photo are the true trajectory 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). Depth (m) −0.5
Fig. 2. Structure of the complementary filter proposed to integrate
600
−1
500
−1.5
400
−2
300
−2.5
200
−3
100
−3.5
Northing (0.5m)
TAN with DVL.
3. TAN EXPERIMENTS WITH IN-WATER COLLECTED DATA 3.1 Site Description and TAN Map The site chosen for the experimental tests is a very shallowwater lake with an area of approximately 300m x 200m located at Doca do Ocean´ ario - Parque das Na¸c˜ oes (38.765◦N , 9.093◦ W ), in Lisbon; see Figure 3. The prior map used by the TAN algorithms was obtained by interpolation of bathymetric data in a regular grid with 0.5m spacing. The topography of the site, shown in Figure 4, is very smooth except on the boundaries and in the Northern part of the lake where a ramp facilitates the access to the water.
0
100
200
300
400
482
−4
Easting (0.5m)
Fig. 4. Prior bathymetric map of the TAN trial area.
3.2 Configuration of the TAN experiments Robotic plataform The robotic platform used in the experimental tests is an autonomous vehicle of the class Medusa developed by the Institute for Systems and Robotics of Instituto Superior T´ecnico (ISR/IST). The Medusa can be configured as autonomous underwater vehicles (AUVs) or autonomous surface vehicles (ASVs); see Figure 5. 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 deadreckoning. In the trials, the ASV navigated with a surge 140
velocity of 0.5m/s. The real path followed by the vehicle is shown in Figure 3, together with the dead-reckoning estimates 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. Sensors Specifications The DVL used in this experiments is a LinkQuest NavQuest 600 Micro Doppler velocity logger with a classical Janus four beam configuration. This DVL is a relatively inexpensive system which is installed as a standard navigation instrument of the Medusa ASV. Besides providing velocity measurements
Francisco Curado Teixeira et al. / IFAC-PapersOnLine 48-2 (2015) 137–142
141
Table 3. Parameters used in TAN Filters Parameter Filter update frequency:
value 4
Number of standard particles: N
500 100
Number of extra particles: Nextra Complementary filter gains, K1 , K2
Fig. 5. The Medusa ASV in the water at the test site. 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. Table 1. Specifications of the DVL used in the trials Sensor function
LinkQuest-NavQuest 600 MicroDVL Velocity sensor and altimeter
Frequency
600kHz
Maximum range
110m
Range accuracy
0.1 22◦ (4-beam)
Beam-width
Table 2. Specs. of motion sensors used in the trials
Ang. rate noise roll, pitch, yaw Head. accuracy: Static, Dynamic Roll, pitch acc.: Static, Dynamic
LinkQuest NavQuest DVL built in motion sensor
2◦ , 2◦
2◦ , NA
0.15, 0.01
sensor; in one of the legs, the heading measurements were biased in excess of 30 degrees. The orientation errors affect not only the estimation of the pose of the vehicle by deadreckoning, but also the prediction of altitude measurements used by the TAN algorithm; these measurements are predicted by simulating the intersection of the acoustic beams with the bottom surface represented in the reference map. Despite the large estimation error caused by this deficiency, the TAN filter was able to converge approximately to the true position when the vehicle reached the area of larger terrain slope observed in the Northern part of the lake; see the trajectory estimated by TAN with this sensor configuration in Figure 6. Recovery from filter divergence was achieved through the execution of a failure detection mechanism embodied in the TAN filter that monitors the sum of the weights of the particles, Σw . An extremely low value of Σw which holds for several iterations, is an indicator that the filter has diverged. In this situation, the filter is re-initialized by distributing the particles uniformly over a large area that may correspond to the whole map. 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 external attitude unit. During the tests it was also observed that one of the DVL beams (beam1) failed frequently and introduced large errors in the altitude measurements. For this reason, the utilization of this beam was inhibited in the TAN filter which is programmed to deal with this type of sensor failures.
Dead−reckon.(DVL) True (GPS−RTK) Estimated (TAN) Depth
600
Depth (m) −0.5
NA
500 −1
0.5◦ ,
1◦
0.5◦ ,
Northing (0.5 meters)
Sensor function
VectorNav VN-100 Motion sensor √ 0.0035◦ /s/ Hz
units Hz
NA
TAN Filters Configurations The filters selected to execute the terrain navigation experiments are the TAN/DVL-PPF and TAN/DVL-CF described in the previous sections. See filters configurations parameters in Table 3. A detailed analysis of the performance of different PF algorithms applied to the TAN problem, can be found in Teixeira (2007) and Teixeira et al. (2012a). 3.3 Presentation and discussion of experimental results
400
−1.5
−2
300
−2.5 200 −3 100 −3.5
50
100
150
200 250 300 Easting (0.5 meters)
350
400
450
−4
Fig. 6. True trajectory and estimated trajectories obtained by TAN and dead-reckoning using the DVL built-in motion sensor.
Navigation data issues and fault detection The analysis of the navigation data acquired in the trials revealed the poor accuracy of the DVL built-in motion 141
TAN/DVL-PPF results As can be observed in Figure 7, the TAN-estimates follow closely the true trajectory.
Francisco Curado Teixeira et al. / IFAC-PapersOnLine 48-2 (2015) 137–142
142
The larger deviation occurs in the Southern leg where the topography does not convey enough information to enable the correction of the drift accumulated in the center of the lake. Figure 8 shows the error decreasing in the final part of the trajectory where TAN benefits from the large terrain gradient observed in the area of the ramp.
Localization Error: Dead reckon. (DVL+ExternalMRU) vs. TAN/DVL−PPF vs. TAN/DVL−CF 50 45 Dead reckon.(DVL+MRU) DVL/TAN−CF DVL/TAN−PPF
40 35
error (m)
30 Depth True (GPS−RTK) TAN/DVL−PPF TAN/DVL−CF Dead−reckoning
600
20 15 10
Depth (m) −0.5
Northing (0.5meters)
500
25
5 0
−1 400
0
0.5
1 iteration number
1.5
−1.5 300
Fig. 8. Absolute value of the localization errors obtained with
−2
TAN/DVL-PPF, TAN/DVL-CF, and dead-reckoning performed with the DVL and the external MRU data.
−2.5
200
above mentioned state-of-the-art navigation systems. We consider however that the present results evidence the ability of TAN to provide accurate localization estimates even in poorly informative terrain and using standard, relatively inexpensive navigation sensors.
−3 100 −3.5
50
100
150
200 250 300 Easting (0.5meters)
350
400
450
2 4
x 10
−4
ACKNOWLEDGEMENTS Fig. 7. True trajectory and dead-reckoning estimate obtained with the DVL plus the external motion sensor, compared with the trajectories estimated by TAN/DVL-PPF and TAN/DVL-CF.
TAN/DVL-CF results In the above presented results it is possible to notice a common deficiency of the output of terrain navigation algorithms: the short-term variability of position estimates that can be observed in the plots of Figures 7 and 8. To mitigate this problem the complementary filter described in Section 2.2 was applied to fuse the TAN and DVL navigation data. As can be observed in Figure 8, in general the output of the CF is characterized by smaller estimation errors and reduced short-term variability. These advantages are evidenced especially in the second half of the trajectory (starting approximately at iteration number 0.9x104 ) where, due to insufficient 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. 4. CONCLUSIONS The paper described a class of navigation and datafusion filters developed by the authors and illustrated their application using real data acquired in experimental trials in the water. The navigation methods proposed showed their efficacy when using low-resolution altitude sensors and proved to be robust in the presence of large orientation biases and missing DVL range measurements. Due to the high diversity of terrain scenarios and system configurations used in the works reported in the literature and in our experimental trials, it is not possible to make a fair comparison between the navigation accuracy obtained with the present TAN solution and that achieved by the 142
We are indebted to our colleagues at ISR/IST who offered untiring support and many useful suggestions to carry out the experimental tests. REFERENCES Anonsen, K.B., Hagen, O.K., Hegrenaes, O., and Hagen, P.E. (2013). The HUGIN AUV terrain navigation module. In IEEE/MTS OCEANS’13. San Diego, CA. Anonsen, K.B. and Hallingstad, O. (2006). Terrain aided underwater navigation using point mass and particle filters. In IEEE/ION Position Location and Navigation Symposium, 1027– 1035. San Diego, CA. Brokloff, N. (1994). Matrix algorithm for Doppler sonar navigation. In IEEE/MTS OCEANS’94. Brest, FR. Hagen, O.K., Anonsen, K.B., and Mandt, M. (2010). The HUGIN real-time terrain navigation system. In IEEE/MTS OCEANS’10. Seattle, USA. Meduna, D.K., Rock, S.M., and McEwen, R. (2008). Low-cost terrain relative navigation for long-range AUVs. In MTS/IEEE OCEANS’08 Quebec, CA. Morice, C., Veres, S., and McPhail, S. (2009). Terrain referencing for autonomous navigation of underwater vehicles. In MTS/IEEE OCEANS’09 - Europe. Bremen, GE. Nygren, I. and Jansson, M. (2004). Terrain navigation for underwater vehicles using the correlator method. IEEE Journal of Oceanic Engineering, 29(3), 906–915. Teixeira, F.C., Pascoal, A., and Maurya, P. (2012a). A novel particle filter formulation with application to terrain-aided navigation. In IFAC Workshop on Navigation, Guidance and Control of Underwater Vehicles (NGCUV’ 2012). Porto, PT. Teixeira, F.C., Quintas, J., and Pascoal, A. (2012b). AUV terrainaided Doppler navigation using complementary filtering. In 9th IFAC Conference on Manoeuvring and Control of Marine Craft (MCMC-2012). Arenzano, IT. Teixeira, F.C. (2007). Terrain-Aided Navigation and Geophysical Navigation of Autonomous Underwater Vehicles. Ph.D. thesis, Instituto Superior T´ ecnico. Lisbon.
ScienceDirect
AUV AUV AUV
IFAC-PapersOnLine 48-2 (2015) 137–142
Terrain-Aided Navigation using Terrain-Aided Terrain-Aided Navigation Navigation ⋆using using ⋆⋆ Doppler Velocity Logger Doppler Velocity Logger Doppler Velocity Logger
a a a
∗ ∗ ∗ Francisco Curado Teixeira a o Quintas o nio Pascoal ∗ Jo˜ ∗ Ant´ ∗ Francisco Curado Teixeira a o Quintas o nio Pascoal ∗ Jo˜ ∗ Ant´ ∗ ∗ ∗ Francisco Curado Teixeira Jo˜ a o Quintas Ant´ o nio Pascoal Francisco Curado Teixeira Jo˜ ao Quintas Ant´ onio Pascoal ∗ ∗ ∗ Institute of Systems and Robotics and Dept. Electrical Engineering, Institute of Systems and Robotics and Dept. Electrical Engineering, ∗ ∗ Institute of and and Dept. Electrical Instituto Superior T´ eecnico, Lisboa, Portugal(e-mail: Institute of Systems Systems and Robotics Robotics and Dept. Electrical Engineering, Engineering, Instituto Superior T´ cnico, Lisboa, Portugal(e-mail: Instituto Lisboa, [email protected]; [email protected]; Instituto Superior Superior T´ T´eecnico, cnico, Lisboa, Portugal(e-mail: Portugal(e-mail: [email protected]; [email protected]; [email protected]; [email protected]; [email protected]). [email protected]; [email protected]; [email protected]). [email protected]). [email protected]).
Abstract: The potential of terrain-aided navigation (TAN) of autonomous underwater vehicles Abstract: The potential of terrain-aided navigation (TAN) of autonomous underwater vehicles Abstract: The potential of terrain-aided navigation (TAN) of autonomous underwater vehicles (AUVs) has been demonstrated in the last ten years in a series of trials where high localization Abstract: The potential of terrain-aided navigation (TAN) of autonomous underwater vehicles (AUVs) has been demonstrated in the last ten years in a series of trials where high localization (AUVs) has been demonstrated in the last ten years in a series of trials where high localization accuracy was obtained during long periods of time in different types of terrain. Despite these (AUVs) has been demonstrated in the last ten years in a series of trials where high localization accuracy was obtained during long periods of time in different types of terrain. Despite these accuracy was obtained during long periods of time in different types of terrain. Despite these recent advances, it is recognized by the navigation community that further research is necessary accuracy was obtained during long periods of timecommunity in different types of terrain. Despite these recent advances, it is recognized by the navigation that further research is necessary recent advances, it is isinto recognized by the the navigation communityThe that present further research research is necessary necessary to transform TAN a mature navigation methodology. paper describes the recent advances, it recognized by navigation community that further is to transform TAN into a mature navigation methodology. The present paper describes the to transform TAN into a mature navigation methodology. The present paper describes the work developed towards the development of terrain navigation methods for small AUVs, relying to transform TAN into a mature navigation methodology. The present paper describes the work developed towards the development of terrain navigation methods for small AUVs, relying work developed towards the development of terrain navigation methods for small AUVs, relying on standard navigation sensors and dispensing with the need of dedicated sensors for terrain work developed towards the development of terrain navigation methods for small AUVs, relying on standard navigation sensors and dispensing with the need of dedicated sensors for terrain on navigation sensors and with the need dedicated sensors data acquisition. The research addresses implementation of terrain on standard standard navigation sensors described and dispensing dispensing withthe theproblem need of of of dedicated sensors for for data acquisition. The research described addresses the problem of implementation of terrain data acquisition. acquisition. The research research described addresses the problem of implementation implementation ofand terrain navigation in underwater scenarios characterized by smooth sea-bottom topography very data The described addresses the problem of of terrain navigation in underwater scenarios characterized by smooth sea-bottom topography and very navigation in underwater scenarios characterized by smooth sea-bottom topography and very shallow water, where the terrain information available for navigation is scarce. The TAN navigation in underwater scenarios characterized by smooth sea-bottom topography and very shallow water, where the terrain information available for navigation is scarce. The TAN shallow water, where the terrain information available for navigation is scarce. The TAN algorithms and the data fusion methods whose tests are documented in the paper build upon shallow water, where the terrain information available for navigation is scarce. The TAN algorithms and the data fusion methods whose tests are documented in the paper build upon algorithms and data fusion methods whose tests are in the build upon prior theoretical work published the authors. experimental results were obtained algorithms and the the data fusion by methods whoseThe tests are documented documented inreported the paper paper build upon prior theoretical work published by the authors. The experimental results reported were obtained prior theoretical theoretical work published by the the authors. The The experimental results reported were obtained recently through trials in the water performed with an autonomous surface vehicle acting as aa prior work published by authors. experimental results reported were obtained recently through trials in the water performed with an autonomous surface vehicle acting as recently through trials in the water performed with an autonomous surface vehicle acting as aa proxy to an AUV. recently through trials in the water performed with an autonomous surface vehicle acting as proxy to an AUV. proxy to an AUV. proxy to an AUV. © 2015, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Particle Keywords: Particle filters; filters; complementary complementary filtering; filtering; Doppler-based Doppler-based navigation; navigation; terrain-aided terrain-aided Keywords: Particle filters; complementary filtering; Doppler-based navigation; navigation Keywords: Particle filters; complementary filtering; Doppler-based navigation; terrain-aided terrain-aided navigation navigation navigation 1. run. The TAN solution which was first successfully applied 1. INTRODUCTION INTRODUCTION run. The TAN solution which was first successfully applied 1. run. The TAN solution which was first successfully applied in air vehicles, has received increased attention from the 1. INTRODUCTION INTRODUCTION run. The TAN solution which was first successfully applied in air vehicles, has received increased attention from the in air vehicles, has received increased attention from the underwater robotics community in the last ten years. This Conventional navigation systems currently in use by auin air vehicles, has received increased attention from the robotics community in the last ten years. This Conventional navigation systems currently in use by au- underwater underwater robotics community in the last ten years. This Conventional navigation systems currently in use by auis reflected in a series of demonstrations of the concept tonomous underwater vehicles (AUVs) are overly expenunderwater robotics community in the last ten years. This Conventional navigation systems currently in use by auis reflected in a series of demonstrations of the concept tonomous underwater vehicles (AUVs) are overly expen- is reflected in a series of demonstrations of the concept tonomous underwater vehicles (AUVs) overly through experimental in the water by sive severely their and reflected in a series trials of demonstrations ofperformed the concept tonomous underwater (AUVs) are areautonomy, overly expenexpenthrough experimental trials in the water performed by sive and and limit limit severelyvehicles their versatility, versatility, autonomy, and is through experimental trials in the water performed by distinct research groups. The work of Nygren and Jansson sive and limit severely their versatility, autonomy, and range of operation. To cope with the increasing demand through experimental trials in the water performed by sive and limit severely their versatility, autonomy, and research groups. The work of Nygren and Jansson range of operation. To cope with the increasing demand distinct distinct research groups. The work of Nygren and Jansson range of operation. To cope with the increasing demand (2004), Anonsen and Hallingstad (2006), and Morice et al. of low-cost, multipurpose underwater vehicles capable of distinct research groups. The work of Nygren and Jansson range of operation. To cope with the increasing demand and (2006), and al. of low-cost, multipurpose underwater vehicles capable of (2004), (2004), Anonsen Anonsen and Hallingstad Hallingstad (2006), and Morice Morice et etthe al. of multipurpose underwater vehicles capable of (2009), among others, demonstrated experimentally performing and long-range Anonsen and Hallingstad (2006), and Morice et al. of low-cost, low-cost, long-term multipurpose vehicles missions capable in of (2004), (2009), among others, demonstrated experimentally the performing long-term and underwater long-range oceanic oceanic missions in (2009), among others, demonstrated experimentally the potential of the TAN solution in different types of terrain. performing long-term and long-range oceanic missions in fully autonomous mode, it is necessary to develop a new (2009), among others, demonstrated experimentally the performing long-term and long-range oceanic missions in of the TAN solution in different types of terrain. fully autonomous mode, it is necessary to develop a new potential potential of TAN in types of fully autonomous mode, it is to commercial of aa terrain navigation solution with class of precise systems. potential of the theversion TAN solution solution in different different types of terrain. terrain. fully is necessary necessary to develop develop aa new new A A commercial version of terrain navigation solution with class autonomous of affordable, affordable,mode, preciseitnavigation navigation systems. A commercial version of a terrain navigation solution with class of affordable, precise navigation systems. the ability to exploit the altitude measurements provided A commercial version the of aaltitude terrain navigation solution with class terrain-aided of affordable, navigation precise navigation systems. the ability to exploit measurements provided The (TAN) approach has been theaability ability to exploit exploit the altitude altitude measurements provided by multibeam echo-sounder (MBE) or alternatively by a The terrain-aided navigation (TAN) approach has been the to the measurements provided by a multibeam echo-sounder (MBE) or alternatively by a The terrain-aided navigation (TAN) approach has been proposed during last as an alThe terrain-aided navigation (TAN) has been aa multibeam echo-sounder (MBE) or alternatively by a Doppler velocity logger (DVL) has been delivered recently; proposed during the the last decades decades as approach an economical economical al- by by multibeam echo-sounder (MBE) or alternatively by a Doppler velocity logger (DVL) has been delivered recently; proposed during the last decades as an economical alternative to the conventional navigation of AUVs based proposed during the last decades as an economical alDoppler velocity logger (DVL) has been delivered recently; see Anonsen et al. (2013). The TAN module is integrated ternative to the conventional navigation of AUVs based Doppler velocity logger (DVL) has been delivered recently; see Anonsen et al. (2013). The TAN module is integrated ternative to conventional navigation of AUVs based on high-grade inertial navigation systems (INS) and long ternative to the the conventional navigation AUVs see et The module is integrated the DVL-aided navigation system of the AUV on high-grade inertial navigation systems of (INS) andbased long with see Anonsen Anonsen et al. al. (2013). (2013). The TAN TAN module isHugin integrated with the DVL-aided navigation system of the Hugin on high-grade inertial navigation systems (INS) and long acoustic baselines. TAN relies on matching a set of range on high-grade inertial navigation systems (INS) and long with the the DVL-aided navigation system system of the Hugin Hugin AUV AUV which includes a state-of-the-art inertial measurement unit acoustic baselines. TAN relies on matching a set of range with DVL-aided navigation of the AUV which includes aa state-of-the-art inertial measurement unit acoustic baselines. TAN relies on a set measurements acquired sensors in acoustic baselines. TAN with reliessonar on matching matching set of of range range includes state-of-the-art inertial measurement unit (IMU) and a high-grade DVL, see Hagen et al. (2010). All measurements acquired with sonar sensors ainstalled installed in aa which which includes a state-of-the-art inertial measurement unit (IMU) and a high-grade DVL, see Hagen et al. (2010). All measurements acquired with sonar sensors installed in a vehicle with a previously acquired digital elevation map measurements acquired with sonar sensors installed in a (IMU) and a high-grade DVL, see Hagen et al. (2010). All the above mentioned navigation systems rely primarily on vehicle with a previously acquired digital elevation map (IMU) and a high-grade DVL, see Hagen et al. (2010). All the above mentioned navigation systems rely primarily on vehicle with a previously acquired digital elevation map (DEM) of the terrain to estimate position. In principle, vehicle with previously acquired position. digital elevation map navigation-grade the above mentioned navigation systems rely primarily on INS and top quality swath sonar instru(DEM) of thea terrain to estimate In principle, the above mentioned navigation systems rely sonar primarily on navigation-grade INS and top quality swath instru(DEM) of the terrain to estimate position. In principle, terrain navigation can be integrated with dead-reckoning (DEM) of the terrain to estimate position. In principle, navigation-grade INS and and to topmake quality swath sonar instruinstrumentation that contribute them too expensive. To terrain navigation can be integrated with dead-reckoning navigation-grade INS top quality swath sonar mentation that contribute to make them too expensive. To terrain navigation can be integrated with dead-reckoning to autonomous navigation systems capable terrain navigation can be integrated dead-reckoning that contribute make them too To the best of our knowledge, the first successful low-cost terto implement implement autonomous navigationwith systems capable of of mentation mentation that contribute to to make them too expensive. expensive. To the best of our knowledge, the first successful low-cost terto implement autonomous navigation systems capable of providing accurate estimates of position in the short term to implement autonomous navigation systems capable of the best of our knowledge, the first successful low-cost terrain navigation solution using non-dedicated sonar sensors providing accurate estimates of position in the short term the best of our knowledge, the first successful low-cost terrain navigation solution using non-dedicated sonar sensors providing estimates of in short term combined with localization the long providing accurate accurate estimates of position positionerrors in the thein rain solution using non-dedicated sonar sensors lower-accuracy inertial was first reported in combined with bounded bounded localization errors inshort the term long and rain navigation navigation solution usingsystems non-dedicated sonar sensors and lower-accuracy inertial systems was first reported in combined with bounded localization errors in the long combined with bounded localization errors in the long ⋆ and lower-accuracy inertial systems was first reported in Meduna et al. (2008). This research was supported in part by FCT project ATLAS and lower-accuracy inertial systems was first reported in ⋆ This research was supported in part by FCT project ATLAS Meduna et al. (2008). ⋆ Meduna et al. (2008). [PTDC/EEA-ELC/111095/2009], MORPH (EU FP7 grant This research research was was supported supported in inproject part by by FCT project project ATLAS ⋆ Meduna et al. (2008). This part FCT ATLAS [PTDC/EEA-ELC/111095/2009], project MORPH (EU FP7 grant As aa contribution to the development of aa cost-effective agreement No. 288704), and FCT [PEst-OE/EEI/LA0009/2011]. [PTDC/EEA-ELC/111095/2009], project MORPH As contribution to the development of cost-effective [PTDC/EEA-ELC/111095/2009], project MORPH (EU (EU FP7 FP7 grant grant agreement No. 288704), and FCT [PEst-OE/EEI/LA0009/2011]. As a contribution to the development of cost-effective terrain navigation system for AUVs, in et al. As a contribution to the development of aaTeixeira cost-effective agreement No. 288704), and FCT [PEst-OE/EEI/LA0009/2011]. The authorsNo. gratefully acknowledge the sponsorhip of the South terrain navigation system for AUVs, in Teixeira et al. agreement 288704), and FCT [PEst-OE/EEI/LA0009/2011]. The authors gratefully acknowledge the sponsorhip of the South terrain navigation system for AUVs, in Teixeira et al. (2012b) we proposed an implementation based on a Korean Agency for Defense Development under a collaborative The authors gratefully acknowledge the sponsorhip of the South terrain navigation system for AUVs, in Teixeira et al. The authors gratefully acknowledge the sponsorhip the South (2012b) we proposed an implementation based on a Korean Agency for Defense Development under a of collaborative (2012b) we proposed an implementation based on a Doppler velocity logger (DVL) integrated with standard, research agreement between KAIST and IST on the related theme Korean Agency for Defense Development under a collaborative (2012b) we proposed an implementation based on a Korean Agency for Defense Development under a collaborative Doppler velocity logger (DVL) integrated with standard, research agreement between KAIST and IST on the related theme Doppler velocity logger (DVL) integrated with standard, of Development of Terrain-Referenced and Geomagnetic Navigation research agreement between KAIST and IST on the related theme inexpensive motion sensors. The solution proposed, beDoppler velocity logger (DVL) integrated with standard, research agreement between KAIST and on the related theme of Development of Terrain-Referenced andIST Geomagnetic Navigation inexpensive motion sensors. The solution proposed, beTechniques for Enhanced Navigation Performance of UUVs. of Development of and Navigation inexpensive motion motion sensors. sensors. The The solution solution proposed, proposed, bebeof Development of Terrain-Referenced Terrain-Referenced and Geomagnetic Geomagnetic Navigation inexpensive Techniques for Enhanced Navigation Performance of UUVs. Techniques Techniques for for Enhanced Enhanced Navigation Navigation Performance Performance of of UUVs. UUVs.
Copyright IFAC 2015 137 Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright © 2015, IFAC 2015 137 Copyright IFAC 2015 137 Peer review© of International Federation of Automatic Copyright ©under IFAC responsibility 2015 137Control. 10.1016/j.ifacol.2015.06.022
138
Francisco Curado Teixeira et al. / IFAC-PapersOnLine 48-2 (2015) 137–142
sides using the Doppler-based velocity measurements for dead-reckoning, exploits the ability of the DVL to acquire periodically a set of altitude or slant-range measurements that are used by TAN algorithms for terrain matching. The implementation reported relies on a new particle filter algorithm, the Prior-correction Particle Filter (PPF) described in Teixeira et al. (2012a) and uses a complementary filter (CF) to fuse the TAN estimates with the deadreckoning data. The paper shows the advantages of using the novel particle filter and the CF data fusion mechanism in TAN applications; the performance of the methods proposed is illustrated in computer simulations using real bathymetry and simulated DVL data. In the current paper we present experimental results of terrain-aided navigation obtained recently in the scope of the research project ATLAS (http://atlas-geo.web.ua.pt). The project aimed at assessing the efficacy of TAN methods in different underwater scenarios, including relatively smooth sea-bottom topography and very shallow water, where the terrain information available for navigation is scarce. Another objective of the project was the implementation of terrain-aided navigation of AUVs using affordable navigation sensors and relying on non-dedicated sonar equipment for real-time acquisition of bathymetric data. For online acquisition of altitude data it was proposed to use the range measurements provided by a Doppler velocity logger which is a standard instrument used for dead-reckoning navigation in underwater robotics. The site used in the experimental trials is a very shallow-water lake located at Doca do Ocean´ ario - Parque das Na¸c˜oes, in Lisbon; see Figure 3. The results obtained in this scenario demonstrate the efficacy of the TAN solution and the robustness of the proposed estimation methods even in difficult operational conditions. 2. BASIC NOTATION, PROBLEM FORMULATION, AND TAN FILTERS SETUPS 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 represents roll, pitch and yaw angles that parametrize locally the orientation of {B} relative to {I}, and ω = [p, q, r]T represents the angular 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 problem, the terrain-aided navigation problem formulated above constitutes 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 138
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 sequential Monte Carlo estimation, using the above mentioned Prior-correction Particle Filter. 2.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 measurement models which are reproduced here with minor adaptations. The models are simplified 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 velocities relative to the seabed. As such, the TAN position estimates are computed by a 2D particle filter, instead of the four-dimensional Rao-Blackwellized PF version adopted in prior implementations. The notation used in the filter formulation is the following: x is a vector representing the system state with dimension nx and y is a measurement vector with dimension ny . N is the number of samples (particles) used by the particle filter, xit represents the ith particle (a random sample from the state space) at time instant t, and ωti denotes the weight associated to particle xit . Process and measurement models The discretetime process model is: 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, and ζk ∈ IRnx represents the process noise sequence. The input coupling matrix Gu,k is a function of the input vector uk = [vu , vv , vw , ψ, r, z]T and the product Gu,k uk represents the dead-reckoning incremental displacement which is as a function of linear and angular velocities, and orientation. The discrete-time measurement model with additive measurement noise is yk = h(xk ) + η(pk ), (2) where h(.) : IR2 → IRny represents the elevation map and η ∈ IRny models maps erros and range measurement noise which is a function of the 3D position, pk ; yk is a vector that represents the simultaneous measurements taken at each iteration. To model the acquisition of terrain elevation data using the Doppler unit, the observation model presented is adapted to account for a set of four simultaneous range measurements according to the Janus configuration (see e.g. Brokloff (1994)). 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 position and orientation, the measurement noise variables represented in the vector η are considered mutually independent and are characterized by the time-varying measurement noise intensity matrix Rk ; see Teixeira (2007). TAN/DVL-PPF: Particle filter set-up The simplified 2D estimator formulation uses a reduced input vector uk = [vu , vv , ψ, r, z]T where [vu , vv ]T denotes the 2D
Francisco Curado Teixeira et al. / IFAC-PapersOnLine 48-2 (2015) 137–142
velocity vector measured by the Doppler. The expressions used by the PPF at the prediction and update stages of the filter are: Prediction xik+1 = F xik + Gu,k uk + Lζk
,
(3)
where ζk ∼ N (0, Q).
139
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 respectively and consider the decomposition s+k k s p(s) = p(s) = p(s) + p(s), s+k s+k s+k
Update
with k=const. Using the relationship v(s) = sp(s) we obtain
The standard particle filter updates the weights as i wki = wk−1 p(yk |xik ),
p(s) = Fp (s)p(s) + Fv (s)v(s) = Fp (s)p(s) + Fv (s)sp(s),
while the PPF formulation in Teixeira et al. (2012a) uses
where Fp (s) =
i wki = wk−1 p(xk |xik−1 )p(yk |xik ),
where the likelihood p(yk |xk ) implicit in (2) is p(yk |xk ) = pη (yk − h(xk )) and the transition kernel embodied in (1) is p(xk |xik−1 ) = pζ (xk − F xik−1 − Gu,k−1 uk−1 ), with pη and pζ denoting the PDFs of the respective error models. Point estimates ˆ k and the associated A point estimate of the current state x covariance matrices Pk can be obtained trough: N x ˆMMS ωki xik (4) ≃ k i
Pk =
N i
i T ˆMMS . · xk − x ˆMMS ωki xik − x k k
(5)
2.2 Complementary filter for Terrain-Aided Doppler navigation Motivation Despite the important advantage of achieving bounded estimation error, TAN algorithms produce position estimates which are generally characterized by high-frequency noise. Due to space limitations we avoid discussing this issue here and encourage the interested reader to consult Teixeira et al. (2012b), where the causes of short-term variability of the TAN estimates are analyzed in detail. To address the problem, we proposed in the above mentioned work the fusion of the Doppler velocity data with the TAN position estimates by using a complementary filter (CF). This approach takes in account the complementary spectral characteristics of the signals supplied by the TAN and DVL modules. TAN/DVL-CF: Complementary filter setup We reproduce here with due adaptations the complementary filter setup derived in Teixeira et al. (2012b). The system that generates the signals available for filtering is represented by the conceptual model depicted in Figure 1. In the data-fusion filter formulation described below let v denote the true velocity of the vehicle expressed in {I} and notice that the variables vu and vv used in the particle filter equations denote noisy measurements of the velocity expressed in referential {B}. R(t) is the timevarying rotation matrix from {B} to {I}. Additionally, consider the following variables expressed in referential {I} that are represented in Figure 2: pD is the dead-reckoning position estimation obtained by integration of the DVL velocity measurement corrected of the bias term; the residual 139
k s+k ,
Fv (s) =
1 s+k ,
and Fp (s) + Fv (s) = I.
The former equations suggest a filter with the following structure that preserves the signal p: pˆ = Fp pT + Fv vD , where Fp and Fv are linear time-invariant operators with transfer 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ˆ = (Tp + Tv )p + Fp eT + Fv eV . Noting that Tp (s) is a low-pass filter, the previous equation reveals 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 complementary 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 represented in Figure 2 that admits the following realization: −K1 I2x2 R(t) x˙ 1 x1 = x˙ 2 x2 −K2 R(t)−1 02x2 K1 I2x2 R(t) pT (6) + vD K2 R(t)−1 02x2 x1 (7) pˆ = [ I2x2 02x2 ] x2 , where x1 = [x, y]T and x2 = [bx , by ]T , with K1 and K2 denoting the CF filter gains. Equations (6) and (7) are represented in compact form as x˙ = A(t)x + B(t)U (8) y = Cx, (9) with x = [x1 , x2 ]T , U = [pT , vD ]T , y = pˆ. A(t) and B(t) correspond to the time-varying matrices that multiply vectors x and U in equation (6), and C = [ I2x2 02x2 ]. The filter state dynamics in discrete time is given by: x(t) = Φ(t, t0 )x(t0 ) +
t
t0
Φ(t, τ )B(τ )Udτ
,
Francisco Curado Teixeira et al. / IFAC-PapersOnLine 48-2 (2015) 137–142
140
where Φ(t, t0 ) is the state transition matrix for the time ˙ τ ) = Φ(t, τ )A(τ ). This mainterval [t0 , t] that satisfies Φ(t, trix may be computed using differential equation solvers available in standard calculation packages such as Matlab.
6
x 10
Ground−truth. GPS−RTK Dead−reckon. DVL Dead−reckon. DVL+External IMU
4.2909
Y [North] − Local Coordimates (UTM)[m]
4.2908
4.2908
4.2908
4.2907
4.2907
4.2906
4.2905
4.917
4.9175
4.918
Fig. 1. Conceptual model of the system that provides the signals to
4.9185 4.919 4.9195 X [East] − Local Coordimates (UTM)[m]
4.92
4.9205
4.921 5
x 10
be filtered.
Fig. 3. Aerial view of the trial site. Superimposed on the photo are the true trajectory 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). Depth (m) −0.5
Fig. 2. Structure of the complementary filter proposed to integrate
600
−1
500
−1.5
400
−2
300
−2.5
200
−3
100
−3.5
Northing (0.5m)
TAN with DVL.
3. TAN EXPERIMENTS WITH IN-WATER COLLECTED DATA 3.1 Site Description and TAN Map The site chosen for the experimental tests is a very shallowwater lake with an area of approximately 300m x 200m located at Doca do Ocean´ ario - Parque das Na¸c˜ oes (38.765◦N , 9.093◦ W ), in Lisbon; see Figure 3. The prior map used by the TAN algorithms was obtained by interpolation of bathymetric data in a regular grid with 0.5m spacing. The topography of the site, shown in Figure 4, is very smooth except on the boundaries and in the Northern part of the lake where a ramp facilitates the access to the water.
0
100
200
300
400
482
−4
Easting (0.5m)
Fig. 4. Prior bathymetric map of the TAN trial area.
3.2 Configuration of the TAN experiments Robotic plataform The robotic platform used in the experimental tests is an autonomous vehicle of the class Medusa developed by the Institute for Systems and Robotics of Instituto Superior T´ecnico (ISR/IST). The Medusa can be configured as autonomous underwater vehicles (AUVs) or autonomous surface vehicles (ASVs); see Figure 5. 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 deadreckoning. In the trials, the ASV navigated with a surge 140
velocity of 0.5m/s. The real path followed by the vehicle is shown in Figure 3, together with the dead-reckoning estimates 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. Sensors Specifications The DVL used in this experiments is a LinkQuest NavQuest 600 Micro Doppler velocity logger with a classical Janus four beam configuration. This DVL is a relatively inexpensive system which is installed as a standard navigation instrument of the Medusa ASV. Besides providing velocity measurements
Francisco Curado Teixeira et al. / IFAC-PapersOnLine 48-2 (2015) 137–142
141
Table 3. Parameters used in TAN Filters Parameter Filter update frequency:
value 4
Number of standard particles: N
500 100
Number of extra particles: Nextra Complementary filter gains, K1 , K2
Fig. 5. The Medusa ASV in the water at the test site. 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. Table 1. Specifications of the DVL used in the trials Sensor function
LinkQuest-NavQuest 600 MicroDVL Velocity sensor and altimeter
Frequency
600kHz
Maximum range
110m
Range accuracy
0.1 22◦ (4-beam)
Beam-width
Table 2. Specs. of motion sensors used in the trials
Ang. rate noise roll, pitch, yaw Head. accuracy: Static, Dynamic Roll, pitch acc.: Static, Dynamic
LinkQuest NavQuest DVL built in motion sensor
2◦ , 2◦
2◦ , NA
0.15, 0.01
sensor; in one of the legs, the heading measurements were biased in excess of 30 degrees. The orientation errors affect not only the estimation of the pose of the vehicle by deadreckoning, but also the prediction of altitude measurements used by the TAN algorithm; these measurements are predicted by simulating the intersection of the acoustic beams with the bottom surface represented in the reference map. Despite the large estimation error caused by this deficiency, the TAN filter was able to converge approximately to the true position when the vehicle reached the area of larger terrain slope observed in the Northern part of the lake; see the trajectory estimated by TAN with this sensor configuration in Figure 6. Recovery from filter divergence was achieved through the execution of a failure detection mechanism embodied in the TAN filter that monitors the sum of the weights of the particles, Σw . An extremely low value of Σw which holds for several iterations, is an indicator that the filter has diverged. In this situation, the filter is re-initialized by distributing the particles uniformly over a large area that may correspond to the whole map. 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 external attitude unit. During the tests it was also observed that one of the DVL beams (beam1) failed frequently and introduced large errors in the altitude measurements. For this reason, the utilization of this beam was inhibited in the TAN filter which is programmed to deal with this type of sensor failures.
Dead−reckon.(DVL) True (GPS−RTK) Estimated (TAN) Depth
600
Depth (m) −0.5
NA
500 −1
0.5◦ ,
1◦
0.5◦ ,
Northing (0.5 meters)
Sensor function
VectorNav VN-100 Motion sensor √ 0.0035◦ /s/ Hz
units Hz
NA
TAN Filters Configurations The filters selected to execute the terrain navigation experiments are the TAN/DVL-PPF and TAN/DVL-CF described in the previous sections. See filters configurations parameters in Table 3. A detailed analysis of the performance of different PF algorithms applied to the TAN problem, can be found in Teixeira (2007) and Teixeira et al. (2012a). 3.3 Presentation and discussion of experimental results
400
−1.5
−2
300
−2.5 200 −3 100 −3.5
50
100
150
200 250 300 Easting (0.5 meters)
350
400
450
−4
Fig. 6. True trajectory and estimated trajectories obtained by TAN and dead-reckoning using the DVL built-in motion sensor.
Navigation data issues and fault detection The analysis of the navigation data acquired in the trials revealed the poor accuracy of the DVL built-in motion 141
TAN/DVL-PPF results As can be observed in Figure 7, the TAN-estimates follow closely the true trajectory.
Francisco Curado Teixeira et al. / IFAC-PapersOnLine 48-2 (2015) 137–142
142
The larger deviation occurs in the Southern leg where the topography does not convey enough information to enable the correction of the drift accumulated in the center of the lake. Figure 8 shows the error decreasing in the final part of the trajectory where TAN benefits from the large terrain gradient observed in the area of the ramp.
Localization Error: Dead reckon. (DVL+ExternalMRU) vs. TAN/DVL−PPF vs. TAN/DVL−CF 50 45 Dead reckon.(DVL+MRU) DVL/TAN−CF DVL/TAN−PPF
40 35
error (m)
30 Depth True (GPS−RTK) TAN/DVL−PPF TAN/DVL−CF Dead−reckoning
600
20 15 10
Depth (m) −0.5
Northing (0.5meters)
500
25
5 0
−1 400
0
0.5
1 iteration number
1.5
−1.5 300
Fig. 8. Absolute value of the localization errors obtained with
−2
TAN/DVL-PPF, TAN/DVL-CF, and dead-reckoning performed with the DVL and the external MRU data.
−2.5
200
above mentioned state-of-the-art navigation systems. We consider however that the present results evidence the ability of TAN to provide accurate localization estimates even in poorly informative terrain and using standard, relatively inexpensive navigation sensors.
−3 100 −3.5
50
100
150
200 250 300 Easting (0.5meters)
350
400
450
2 4
x 10
−4
ACKNOWLEDGEMENTS Fig. 7. True trajectory and dead-reckoning estimate obtained with the DVL plus the external motion sensor, compared with the trajectories estimated by TAN/DVL-PPF and TAN/DVL-CF.
TAN/DVL-CF results In the above presented results it is possible to notice a common deficiency of the output of terrain navigation algorithms: the short-term variability of position estimates that can be observed in the plots of Figures 7 and 8. To mitigate this problem the complementary filter described in Section 2.2 was applied to fuse the TAN and DVL navigation data. As can be observed in Figure 8, in general the output of the CF is characterized by smaller estimation errors and reduced short-term variability. These advantages are evidenced especially in the second half of the trajectory (starting approximately at iteration number 0.9x104 ) where, due to insufficient 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. 4. CONCLUSIONS The paper described a class of navigation and datafusion filters developed by the authors and illustrated their application using real data acquired in experimental trials in the water. The navigation methods proposed showed their efficacy when using low-resolution altitude sensors and proved to be robust in the presence of large orientation biases and missing DVL range measurements. Due to the high diversity of terrain scenarios and system configurations used in the works reported in the literature and in our experimental trials, it is not possible to make a fair comparison between the navigation accuracy obtained with the present TAN solution and that achieved by the 142
We are indebted to our colleagues at ISR/IST who offered untiring support and many useful suggestions to carry out the experimental tests. REFERENCES Anonsen, K.B., Hagen, O.K., Hegrenaes, O., and Hagen, P.E. (2013). The HUGIN AUV terrain navigation module. In IEEE/MTS OCEANS’13. San Diego, CA. Anonsen, K.B. and Hallingstad, O. (2006). Terrain aided underwater navigation using point mass and particle filters. In IEEE/ION Position Location and Navigation Symposium, 1027– 1035. San Diego, CA. Brokloff, N. (1994). Matrix algorithm for Doppler sonar navigation. In IEEE/MTS OCEANS’94. Brest, FR. Hagen, O.K., Anonsen, K.B., and Mandt, M. (2010). The HUGIN real-time terrain navigation system. In IEEE/MTS OCEANS’10. Seattle, USA. Meduna, D.K., Rock, S.M., and McEwen, R. (2008). Low-cost terrain relative navigation for long-range AUVs. In MTS/IEEE OCEANS’08 Quebec, CA. Morice, C., Veres, S., and McPhail, S. (2009). Terrain referencing for autonomous navigation of underwater vehicles. In MTS/IEEE OCEANS’09 - Europe. Bremen, GE. Nygren, I. and Jansson, M. (2004). Terrain navigation for underwater vehicles using the correlator method. IEEE Journal of Oceanic Engineering, 29(3), 906–915. Teixeira, F.C., Pascoal, A., and Maurya, P. (2012a). A novel particle filter formulation with application to terrain-aided navigation. In IFAC Workshop on Navigation, Guidance and Control of Underwater Vehicles (NGCUV’ 2012). Porto, PT. Teixeira, F.C., Quintas, J., and Pascoal, A. (2012b). AUV terrainaided Doppler navigation using complementary filtering. In 9th IFAC Conference on Manoeuvring and Control of Marine Craft (MCMC-2012). Arenzano, IT. Teixeira, F.C. (2007). Terrain-Aided Navigation and Geophysical Navigation of Autonomous Underwater Vehicles. Ph.D. thesis, Instituto Superior T´ ecnico. Lisbon.