UNIVERSITA DI PISA

Facolta di Ingegneria

Laurea Specialistica in Ingegneria dell’Automazione

Tesi di Laurea

An Underwater Acoustic Positioning SystemBased on Buoys with GPS

Candidato:

Andrea De Vito

Relatori:

Prof. Andrea CaitiProf. Antonio PascoalProf. Mario Innocenti

Sessione di Laurea del 02/10/2007Anno Accademico 2006/2007

Consultazione consentita

Abstract

This thesis proposes several solutions to the problem of estimating the position of an un-derwater vehicle. The commercial system adopted for collecting data consists of four buoysthat compute the times of arrival (TOA) of the acoustic signals emitted periodically by apinger installed on-board the target (so called G.I.B. system).

The solutions discussed are grouped in two family of algorithms: instantaneous posi-tioning (static) algorithms and filtering (dynamic) algorithms. For the static group, theTrilateration problem is discussed in depth, yielding closed form and iterative solutions. Adetailed discussion of the estimation accuracy of the algorithms is presented.

For the dynamic group, an EKF based on a simple kinematic model of the target wasintroduced. In order to test the algorithms proposed with real data from sea trials aninitialization procedure was implemented. The procedure provides a initial fix that liesinside a certain confidence region. The filtering algorithms can be dramatically affected byoutliers. Thus, an on-line measurement validation was also introduced.

A Matlab-based graphical user interface was designed in order to test the algorithmsdescribed with real data obtained during sea experiments.

i

Sommario

Questa tesi propone alcune soluzioni al problema della stima di posizione di un veicolosottomarino. Il sistema commerciale adottato per acquisire dati e composto da quattro boeche calcolano i tempi di arrivo (TOA) di un segnale acustico emesso periodicamente da unpinger installato a bordo del target (per questo chiamto sistema G.I.B.).

Le soluzioni discusse possono essere suddivise in due famiglie di algoritmi: algoritmi dipositionamento istantanei (statici) e filtri (dinamici). Per la famiglia degli algoritmi statici,e stato discussa approfonditamente la trilaterazione, ottenendo sia soluzioni in forma chisuache in modo iterativo. Inoltre e fornita una discussione dettagliata delle performace deglialgoritmi presentati

Per il gruppo degli algoritmi dinamici, e stato sviluppato un filtro di Kalman estesobasato su un semplice modello cinematico del target. Per testare gli algortimi proposti condati reali e stato progettato un algoritmo di inizializzazione. L’algoritmo proposto sceglie ilpunto iniziale se risiede all’interno di una certa regione di confidenza. I filtri possono esseredrammaticamente interessati dal fenomeno degli outliers quindi, per porre rimedio a taleproblema e stata introdotta una procedura di validazione delle misure.

E stata progettata un interfaccia grafica in Matlab per consentire il testing degli algoritmicon i dati acquisti durante gli esperimenti in mare.

ii

Acknowledgement

This Master Thesis was carried out at the Dynamical Systems and Ocean Robotics Lab ofthe Instituo Superior Tecnico in Lisbon, Portugal.

I’d like to thank my examiner Prof. Dr. Andrea Caiti for giving me the opportunity ofdoing this thesis.

I’d like to thank all teachers and instructors at DSOR, especially Prof. Dr. AntonioPascoal for allowing me doing this thesis.

Furthermore, I want to thank Dr. Alex Alcoer for his help, support and all the discussionsthroughout this project. It was a pleasure to work with him.

Thanks to all the guys at the “Lab”, especially to Andre, Bruno G., Bruno C., Luıs,Joao, Jose Maria, Manuel, Pedro, Reza, Rita, and Vahid for the enjoyable and nice timethere.

iii

Ringraziamenti

Con queste pagine si chiude un capitolo della mia vita, caratterizzata dalle visite notturneal Rossi, da chi diceva che a Pisa piove solo due volte all’anno (mah!), da il “Deh!” sempree ovunque, dai “Dassoloni” di mensa, da “Bice e le sue amice”, dalla COOP e le sue offertesocio.

In questi anni molte sono le persone che ho incontrato, ma solo un piccolo gruppo ha resoindimenticabile questa esperienza. Con loro, ho consumato il mio “Mitico ed InsuperabileTiramisu” ogni 3 Dicembre (tranne quella volta che ho cercato di avvelenarli con il dolce alburro, grazie per non avermi ucciso). Con loro ho viaggiato.... come dimenticare il viaggiodella speranza in Salento a 170 Km/h in Panda, l’HackIT di Genova e Lubeck. Con loro hobevuto, memorabile l’ultima festa delle matricole al Sant’Anna e le cene a casa Russo.

Inutile continuare, non basterebbero 100 pagine per ricordare tutte le avventure e soprat-tutto le disavventure condivise, percio mi limitero solo a ringraziarli uno per uno, ricordandoad Anna Maria che l’ordine e puramente casuale.

Il primo di loro l’ho conosciuto per telefono, ed ora siamo divenuti anche mezzi parenti,ne e passata di acqua sotto i ponti e molti sono i soprannomi che si e visto assegnare daAllievo, Pelato, Venticello, Lito Lito (da me anche Cognatiiiiiinoooo), con lui ho imparato“...quanto e buona la roba di giu...”, e a “menare” la birra (rigorosamente Peroni, meglio sedello stabilimento di Bari) ed ora e in Norvegia per una nuova esperienza, in bocca al lupoRocco.

Valerio, conosciuto sul fondo schiena di Ilaria (Amore uno) il primo giorno di precorsi, fidocompagno di mensa, di studio e pedinamenti, con lui ho ampliato i miei orizzonti musicali, masoprattutto ho visto la finale di Champions League Juventus-Milan del 2003, io Juventuinoe lui Milanista.... fortuna che non sono un appassionato di calcio.

Rocco, ingegnere d.o.c e barista da strapazzo che con un cocktail di Gin e scorze diarancia ha quasi ucciso due persone, lui mi ha insegnato a vedere le cose da un altro puntodi vista.... dal lunotto posteriore della Panda.

Luca, con lui ho condiviso casa per tutti questi anni di universita, indimenticabile la suapasta ai wurstel, i capelli da cugino di campagna e l’attrazione per la cultura Giapponese.

Daniele (Landucci), il ciociaro amante della distro universale di Linux, cultore di Perl edel open source, ma soprattutto il piu grosso organizzatore di partitelle di calcetto al DLF,e adesso dottorando a Delft... bella pe te, Danie!

Anna Maria e Manuela salentine convinte, la prima paranoica aspirante assistente sociale,ma anche ragazza dal grande cuore, sempre pronta ad ascoltare e aiutare il prossimo; con laseconda mi sono confrontato su argomenti “importanti”, con le nostre discussioni basate sul

iv

Ringraziamenti

“e dai, e dai....” e calici di vino rosso.Letizia, con la sua avversione al computer e tutto quello che riguarda l’informatica, storica

vicina di casa in via di Cisanello ... mi chiedo ancora: con tutto il frastuono che provenivadal mio appartamento come non abbia mai fatto a lamentarsi... un saluto anche a mammaDonatella.

Veronica, compagna di Valerio, e tutte le ore che mi ha torturato con le paranoie suPandorino a casa Mad House, dove ho avuto l’onore di conoscere Francesa, Valeria e Kammycon la sua disastrosa padrona Elisqui, nonche fidanzata di quel Hondista di Gianluca.

Mara e Stefano, lei avvenente ingegnere delle telecomunicazioni dalla parlantina devas-tante, lui talentuoso avvocato servo della gleba.

Non posso dimenticare Casa Guelfi, e le lunghe chiaccerate con Peppino su Scienze delleCostruzioni, le canzoni degli Afterhours scandite dalle note di Torelli e della sua chitarra, leintrusioni del Roscio e ultimo ma non meno importante Danielone.

Grazie ancora per i bei momenti trascorsi assime a Paola, Donatella, Valentina, Claudio,a quel gruppo di Ingegneri che si facevano chiamare Birillo Boys (Besh, Toshi, Favollo,Fabrizio, Gabriele, Nello e Giampaolo), e anche a Gianni, al Nikki, a Sofia e a Pamela.

Alla fine ho lasciato le persone a me piu care, per loro vale un discorso particolare dettatodal legame di amore che ci lega indissolubilemente nonostante tutte le incomprensioni elitigate non mi hanno fatto mai mancare il loro supporto. Se adesso ho raggiunto questotraguardo lo devo solamente a loro, a tutti i sacrifici e agli sforzi che hanno fatto ogni giorno:non vi saro mai sufficientemente grato, grazie Babbo e grazie Mamma.

Grazie anche ai miei nonni, in particolare a Nonno Beppe e Nonna Vilma con cui nonposso condividere la gioia di questo momento.

Un ringraziamento particolare va ad Elisabetta per essermi stata sempre vicina e peraver condiviso con me questi ultimi due anni, per aver sempre sopportato i miei momenti didepressione e per tutto l’amore che mi trasmette quotidianamente.

In fine un grazie dovuto alla Pandina, fedele compagna di tante avventure, che si eimmolata sulle strade che portano al Salento alla veneranda eta di quattordici anni; e allabimba rossa che da tempo attende in garage il mio ritorno.

v

List of notations

| · | absolute value‖ · ‖ norm operator(·)† matrix pseoudoinverse(·)T matrix transpose(·)−1 matrix inversetr(·) matrix trace∇(·) cost function gradient∇2(·) cost function hessianE{·} expectationexp{·} exponential(−) predicted valueIm identity matrix of order m1m column vector of m onesδk direction of minimization on kth iterationσ standard deviationτ times of arrival (TOA)vs sound velocityd vector of actual ranges

d vector of estimated rangesr vector of measured rangesω Gaussian measurement noiseν Gaussian process noisep actual target positionp estimated target positionpBi position vector of the ith buoy

vi

Contents

Abstract i

Sommario ii

Acknowledgement iii

Ringraziamenti iv

List of notations vi

Contents ix

List of figures xi

List of tables xii

1 Introduction 11.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 System Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.3 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2 Acoustic Positioning Systems 52.1 Principles of Sound Propagation . . . . . . . . . . . . . . . . . . . . . . . . . 52.2 Conventional Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.2.1 Ultra Short or Super Short Baseline . . . . . . . . . . . . . . . . . . . 82.2.2 Short Baseline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.2.3 Positioning Modes for USLB/SBL . . . . . . . . . . . . . . . . . . . . 10

Transponder . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10Free Running Pinger . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.2.4 Long Baseline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.2.5 Positioning Modes for LBL . . . . . . . . . . . . . . . . . . . . . . . . 10

Direct Ranging . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11Intelligent Acoustic Remote . . . . . . . . . . . . . . . . . . . . . . . 11Relay . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11Pseudo Ranging . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

vii

CONTENTS

2.3 Latest Architecture Developments . . . . . . . . . . . . . . . . . . . . . . . . 122.3.1 System Integration . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122.3.2 Inverted architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.4 G.I.B. System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

3 Instantaneous Positioning Algorithms 163.1 Trilateration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

3.1.1 3D Trilateration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163.1.2 2D Trilateration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

Weighted Trilateration . . . . . . . . . . . . . . . . . . . . . . . . . . 193.2 Maximum Likelihood Estimation . . . . . . . . . . . . . . . . . . . . . . . . 21

3.2.1 MLE: Squared ranges . . . . . . . . . . . . . . . . . . . . . . . . . . . 223.2.2 MLE: Pure ranges . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233.2.3 Stepsize: Armijo rule . . . . . . . . . . . . . . . . . . . . . . . . . . . 243.2.4 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

3.3 Cramer-Rao Lower Bound . . . . . . . . . . . . . . . . . . . . . . . . . . . . 313.3.1 Buoy Geometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 323.3.2 Algorithm Comparison . . . . . . . . . . . . . . . . . . . . . . . . . . 32

4 Filtering Methods 364.1 Linear Estimation Problem . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

4.1.1 The Discrete Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . 374.1.2 Uncertainty Ellipsoid . . . . . . . . . . . . . . . . . . . . . . . . . . . 374.1.3 Alternative formulation . . . . . . . . . . . . . . . . . . . . . . . . . . 384.1.4 Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

Linear system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 394.2 Nonlinear Estimation Problem . . . . . . . . . . . . . . . . . . . . . . . . . . 40

4.2.1 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . 414.2.2 EKF Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

Nonlinear System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43Jacobians . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

5 Initialization and Outlier rejection algorithms 475.1 Initialization algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 475.2 Outlier Rejection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

6 Graphical User Interface 51

7 Experimental Results 547.1 Experimental scenario . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 547.2 Results analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

viii

CONTENTS

8 Conclusions and future work 648.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 648.2 Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

Bibliography 66

A MLE: Gradient and Hessian derivation 69A.1 MLE: Squared ranges . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69A.2 MLE: Pure ranges . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

B Chi-Square distribution 74

C Coordinate Transformations 76C.1 Earth Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76C.2 ECEF coordinates from Longitude and Latitude . . . . . . . . . . . . . . . . 77C.3 NED coordinates from ECEF coordinates . . . . . . . . . . . . . . . . . . . . 78

ix

List of Figures

1.1 Multipaths propagation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.2 The G.I.B. system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.1 Typical variable characteristics . . . . . . . . . . . . . . . . . . . . . . . . . 62.2 Typical sound velocity profile . . . . . . . . . . . . . . . . . . . . . . . . . . 62.3 Sound propagation in isovelocity water . . . . . . . . . . . . . . . . . . . . . 72.4 Sound propagation in positive gradient water . . . . . . . . . . . . . . . . . . 72.5 Sound propagation in negative gradient water . . . . . . . . . . . . . . . . . 72.6 Layer Depth Phenomenon . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.7 Ultra Short Baseline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.8 Short Baseline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.9 Long Baseline and Direct Ranging . . . . . . . . . . . . . . . . . . . . . . . . 112.10 USBL/INS integration scheme, from [16] . . . . . . . . . . . . . . . . . . . . 132.11 Inverted USBL, from [32] . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132.12 Inverted LBL, from [32] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142.13 GIB system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142.14 GIB pinger (up), GIB buoy (left) and GIB central unit (right) . . . . . . . . 152.15 Buoy deployment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.1 Step size determination by Armijo Rule. . . . . . . . . . . . . . . . . . . . . 253.2 Gradient and pure ranges. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.3 Newton and pure ranges. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 273.4 Gradient and squared ranges. . . . . . . . . . . . . . . . . . . . . . . . . . . 283.5 Newton and squared ranges. . . . . . . . . . . . . . . . . . . . . . . . . . . . 293.6 Local and global minimums for pure ranges log-likelihood function. . . . . . 303.7 Local and global minimums for squared ranges log-likelihood function. . . . . 303.8 Configuration of 4 buoys in square shape. . . . . . . . . . . . . . . . . . . . . 333.9 Configuration of 4 buoys in rhombus shape. . . . . . . . . . . . . . . . . . . 333.10 Configuration of 3 buoys in triangle shape. . . . . . . . . . . . . . . . . . . . 343.11 Algorithm accuracy compared with CRLB. . . . . . . . . . . . . . . . . . . . 35

4.1 Gaussian probability density for K = 2. . . . . . . . . . . . . . . . . . . . . . 384.2 Kalman Filter simulation results. . . . . . . . . . . . . . . . . . . . . . . . . 404.3 Uncertainty ellipsoid graph. . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

x

LIST OF FIGURES

4.4 Ranges components. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 434.5 EKF simulation results. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 454.6 State error evolutions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

5.1 Integration of EKF, initialization and outlier rejection procedures. . . . . . . 50

6.1 GUI logical scheme. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 526.2 Read Binary Data block. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

7.1 Experimental trajectory. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 567.2 Times of Arrival of acoustic pulses at each buoy. . . . . . . . . . . . . . . . . 577.3 Details of TOA at buoys. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 587.4 State vector evolution. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 597.5 Experimental trajectory of second set of data. . . . . . . . . . . . . . . . . . 607.6 Detailed of TOA validation in the time range [300 − 340]. . . . . . . . . . . . 617.7 Times of Arrival of acoustic pulses at each buoy (second set of data). . . . . 627.8 State vector evolution (second set of data) . . . . . . . . . . . . . . . . . . . 63

B.1 χ2 distribution. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74

C.1 Earth shape model. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76C.2 ECEF coordinate frame with z-axis along Earth’s rotation axis. . . . . . . . 78

xi

List of Tables

3.1 MLE: Simulation parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

4.1 Kalman Filter Matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 404.2 EKF: Simulation parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

7.1 Experimental filter parameters . . . . . . . . . . . . . . . . . . . . . . . . . . 54

B.1 Table of critical values of χ2 distribution. . . . . . . . . . . . . . . . . . . . . 75

C.1 WGS-84 ellipsoid constants. . . . . . . . . . . . . . . . . . . . . . . . . . . . 77

xii

Chapter 1

Introduction

1.1 Background

Autonomous underwater vehicles (AUV) are receiving increasing interest from marine scien-tists, who use them to perform missions involving underwater object inspection, geophysicalfield measurements, and environmental assessment and monitoring. The research trends inunderwater robotics have emphasized minimizing the need of human interaction by increas-ing the autonomy of such vehicles. Central to the operation of these vehicles is the availabilityof accurate navigation and positioning systems. It is important that the navigation systembe robust against sensor measurement errors and outliers.

A common approach to estimate the position of an AUV underwater is based on the useof Inertial Navigation System (INS). Inertial Navigation relies on the integration over time ofvelocity or/and acceleration data provided by a Inertial Measurement Unit (IMU). INSs havelimitations imposed by drift phenomena. In practical applications, sensor measurements arecorrupted by noise. These errors are amplified by integration, introducing a position errorthat increases with time.

In aerial navigation, a solution to deal with drift phenomena is afforded by GPS/INSintegration that updates the vehicle’s position using two different kinds of measurements.However the use of aerial and terrestrial positioning techniques in underwater applicationsis difficult. This is due to the fact that GPS signals do not penetrate below the sea surface.This problem can be alleviated if the AUV can emerge periodically to establish a GPSinterrogation and compensate the position error by taking GPS fixes. This method is validfor applications without any constraints on the diving time.

A solution to the diving time constraints is given by Doppler-based navigation. This un-derwater positioning system merges information provided by a Doppler Velocity Log (DVL)sensor with that of a INS (attitude data). The Doppler sensor measures the vehicle’s ve-locity by with respect to the water or the seabottom. There are two operative modes for aDoppler Sensor: in the first, known as Bottom-lock, the signals are reflected off the seafloorand the maximum operative altitude respect to the seafloor is fixed by the frequency of thetransmission; in the second, called Water-lock, the signals are reflected by the particles inthe water (plankton, etc...). Unfortunately this approach is affected by biases in the velocity

1

Chapter 1. Introduction

measurements due to the presence of water currents. The DVL/INS does not compensate forthe effects of the currents (when measuring velocity with respect to the water), but avoidssome of the problems caused by accelerometer biases because it relies on the integration ofvelocity, rather than acceleration.

A further underwater positioning system is based on the emission and reception of acous-tic pulses and is known as Acoustic Positioning. This method uses only the sound propa-gation to determine the ranges between an AUV and a set of beacons on the sea bed or atthe surface. The AUV position is estimated as the intersection of multiple direction lines in3D space. The acoustic positioning systems are not affected by drift phenomena. However,numerous sources of noise may affect this navigation approach, the most common of whichare multipath effects. Multipath typically originates from acoustic signals being reflected onthe sea bed or at the sea surface and may produce huge errors if not properly detected. Con-sidering a multipath as a ”plausible” range introduces a significant error in fix estimationsbecause the propagation time of multipath is longer than the direct one (see figure 1.1). Twostrategies are usually used to solve the multipath issue: the simplest one considers multipatheffects as outliers. This has the advantage of a low complexity, and the drawback of poornavigation precision; the second approach considers a model of multipath propagation andtakes it into account them in the estimation procedure.

The algorithms for acoustic positioning systems can be classified in two groups: instan-taneous positioning (static) algorithms and filtering (dynamic) algorithms. The static algo-rithms group contains the most common procedure, known as Trilateration. Trilaterationprovides the AUV position as the intersection of multiple direction lines in 3D space, andthe estimate is evaluated only with the current measurements. Typical implementations ofthe trilateration procedure are based on the Least Square Estimator, Maximum LikelihoodEstimator, and Set-Membership approach. The dynamic algorithms group bases the estima-tion process not only on current measurement information but also on dynamic/kinematicproperty of the AUV. Due to the non linear nature of the estimation problem the underwaterpositioning algorithms are based on Extended Kalman Filter or more sophisticated methodsas Unscented Kalman Filter (UKF) and Sequential Monte Carlo (SMC).

������

������

Sea bed

Sea Surface

ObjectReflecting

Multipath

Multipath

RX

TX

Direct Range

Figure 1.1: Multipaths propagation.

2

Chapter 1. Introduction

1.2 System Overview

The subject of this thesis is present and analyze algorithms to process data from a commercialG.I.B. (GPS Intelligent Buoys) underwater positioning system. This system is composedof surface buoys equipped with GPS receivers and submerged hydrophones. This systemprovides the position of surface beacon (buoys) form GPS receivers deployed on the buoys.The buoys provide their relative distance to the underwater target equipped with an acoustictransmiter as time of propagation of a certain signal. Each buoy transmits a data packet toa central station, where the AUV trajectory is computed.

Figure 1.2: The G.I.B. system.

Before estimating the AUV trajectory the GPS buoys positions have to be transformedinto a coordinate system and the AUV relative position have to be converted into distance.GPS receivers provide buoy positions in terms of Latitude, Longitude and Altitude. Inthe case of the buoys, the measurements are codified as time of arrivals (TOAs). Theemission time is known and the buoys count the time employed from the signal to reach eachbuoy. The measurements must be transformed into relative distances. The buoy positionsare transformed into a North East Down reference frame, then the assumed displacementbetween the hydrophones and the GPS receivers are added.

There are two main approaches on position estimation, static algorithms and dynamicalgorithms. In this thesis a dynamic approach is proposed in order to introduce some kine-matic constraints to estimate the AUV velocity components. The TOAs are used to drivean Extended Kalman Filter, including a simple kinematic model of the underwater vehicle.

3

Chapter 1. Introduction

1.3 Thesis Outline

This thesis is divided into eight chapters:

• Chapter 2 gives a brief overview on common underwater acoustic positioning system.Introducing, also, the main difficulties real application. A detailed description of theACSA G.I.B system is given;

• Chapter 3 describes the estimation process with static algorithms. A closed formsolutions for Least Square Estimator and iterative solution for Maximum LikelihoodEstimator are presented. A limit on algorithm performance is given by Cramer-RaoLower Bound;

• Chapter 4 introduces a futher class of algorithm based on Kalman Filter theory. Thelinear and non linear case are discussed. For the linear 2D case the uncertainty ellip-soid are introduced. An Extended Kalman Filter algorithm for the G.I.B. system isdesigned.

• Chapter 5 describes in depth the initialization and outlier rejection algorithms;

• Chapter 6 introduces the logical structure of Graphical User Interface for G.I.B. system;

• Chapter 7 presents the experimental results of the acoustic positioning algorithmsproposed, using data collected by the G.I.B. system in sea tests;

• Chapter 8 presents conclusion and future works.

4

Chapter 2

Acoustic Positioning Systems

2.1 Principles of Sound Propagation

In underwater positioning problem, the medium (communication channel) is represented bysea water, thus the conventional aerial techniques of communications are not available. Forinstance light and magnetic signatures have limited communication range (< 100m) andradio frequencies do not propagate well in the water. Which kind of energy source shouldbe used in an underwater positioning problem? An answer to this question can be found inNature: sound energy. Sound under water is very important for animals: it allows them tonavigate, to hear approaching predators and prey, and is a way of communicating with othermembers of the same species. The sound energy in the water, has a speed of propagationaround five times greater than in the air. In a positioning problem, this characteristic isuseful to cover long distance in a short period of time between the vehicle emitter and thereceiver. The sound velocity is dependent upon various factors including water temperature,pressure and salinity. The influence of each variable is shown in figure 2.1.

Sea water is not an homogeneous body, it could be depicted as a stratified one where eachlayer summarizes the state of the three main variables (temperature, salinity and pressure).The sound path can be described as a ray by the Snell’s Law. The sound, when crossing alayer, deflects his path passing through the slowest propagation zone. The Speed VelocityProfile (SVP) is usually measured by a expendable bathythermograph probe, a typical SVPis shown in figure 2.2.

The SVP reveals some common structure to the ocean. The water can be divided intothree vertical regions.The Surface layer is at the top and is the most variable part. As thename suggests, the profile will changes depending on the time of day and the season. TheMain thermocline connects the surface layer with the uniformly cold water found deep inthe ocean. Below about 500 m, all of the world’s oceans are at about 34◦F . The positivegradient in the deep isothermal region is solely due to the pressure effect.

In accord with temperature and sound velocity characteristic, three sea water conditionscan be defined as follow:

• Isovelcity where sound propagation can be depicted as a long straight line, temperaturedecreases with depth and sound speed is constant (figure 2.3);

5

Chapter 2. Acoustic Positioning Systems

Salinity

Dep

th

Pressure Temperature

Dep

th

Dep

th

Figure 2.1: Typical variable characteristics

Sound Velocity

Dep

th

Surface Layer

Deep IsothermalLayer

Main Thermocline

Figure 2.2: Typical sound velocity profile

• Positive gradient where sound is bended in surface direction, thus the sound can notreach the seafloor. This effect is called Surface Duct. Postive gradient is characterizedby warm water over cool water (figure 2.4);

6

Chapter 2. Acoustic Positioning Systems

• Negative gradient where sound is bended in seafloor direction introducing shadowzones, in this condition, temperature and sound speed decrease with depth (figure2.5).

��������

��������

Dep

th

C

P

T

Dep

th

Figure 2.3: Sound propagation in isovelocity water

��������

��������

Dep

th

T

Dep

th

C

ShadowZone

Figure 2.4: Sound propagation in positive gradient water

��������

��������

Dep

th

T

Dep

th

C

ShadowZone

Figure 2.5: Sound propagation in negative gradient water

In practical applications, the most common sea configuration is depicted by a first layerwith positive gradient over a second layer with negative gradient. In this condition occursthe Layer Depth phenomenon defined as the depth where the sound velocity is maximum.Layer depth introduce a critical ray (sound propagation path) split, which creates a shadowzone as shown in figure 2.6, more sea water configuration are described on [29].

7

Chapter 2. Acoustic Positioning Systems

��������

��������

Dep

th

T

Dep

th

C

ZoneShadow

Figure 2.6: Layer Depth Phenomenon

2.2 Conventional Systems

Several commercial underwater acoustic positioning system are available. They are usuallyclassified according to the concept of Baseline, which is defined as the distance between theactive sensing elements. There are three common types, as define in [31]:

• Ultra Short or Super Short Baseline (USBL or SSBL)

• Short Baseline (SBL)

• Long Baseline (LBL)

This dissertation is focused on AUV positioning problem, however the presented architecturescan be also used for ROV, diver, and twofish tracking applications ([27],[31],[32]).

2.2.1 Ultra Short or Super Short Baseline

A complete USBL system consists of a transceiver rigidly mounted on the vessel bottom,and an emitter (Transponder or Responder) deployed on the underwater vehicle. Usually,the transceiver is composed by three or more transducers separated by a baseline of 10cm orless. A typical interrogation can be described as an acoustic pulse transmitted by the vesseltransceiver, which is detected by the underwater vehicle sensing device, which replies withan acoustic pulse. The vessel transceiver catches the signal and it computes the range inaccord with the elapsed time. A USBL system provides both the orientation and positionbetween vessel transceiver and underwater vehicle emitter. Orientation is determined bya method called phase-differencing, which makes a phase comparison on arriving ping onvessel transceiver elements. Orientation and position determined by USBL are referred tothe vessel reference frame thus the system needs a good accuracy level on vessel position.

Low system complexity makes USBL systems very convenient to use. Moreover, there isno need to deploy transponders on the seafloor. In order to obtain a good level of accuracywith an USBL systems based on Time of Flight (TOF) interrogation, the USBL sensingdevice need an precise instalaltion on the vessel, and a good absolute vessel position accuracyis needed, also.

8

Chapter 2. Acoustic Positioning Systems

Figure 2.7: Ultra Short Baseline

2.2.2 Short Baseline

A complete SBL system consists in a set of transceivers (≥ 3) rigidly mounted on the vesselbottom, and an emitter (Transponder or Responder) deployed on underwater vehicle. Thetransponders are separated by a baseline between 20 to 50 meters. The position determinedwith the SBL system is referred to the transceivers mounted on the ship hull.

Figure 2.8: Short Baseline

Low system complexity makes SBL system an easy tool to use moreover there is no needdeploy transponders on seafloor, spatial redundancy and a good range accuracy with ToFsystem. In order to obtain that accuracy, SBL system needs large baselines in deep water(> 40m), detailed offshore calibration and a number of pole bigger than 3. As shown forUSBL, AUV fixes are perform respect to vessel frame thus SBL require a very good vessel

9

Chapter 2. Acoustic Positioning Systems

positioning system.

2.2.3 Positioning Modes for USLB/SBL

In order to find a solution of AUV positioning problem USBL/SBL architectures offer twopossible implementation:

• Transponder

• Free running pinger

Transponder

Transponder mode relies on a bidirectional acoustic communication, the surface transceiverinterrogates the vehicle device on a Interrogation Frequency (IF) and it waits for a responseon a Replay Frequency (RF) from the vehicle emitter.

Free Running Pinger

A free running pinger (a beacon that constantly transmits at a known frequency with knownrepetition rate) is installed on the vehicle to be tracked, the depth information is given by adepth sensor. This approach is the least accurate mode for USBL/SBL architectures due tothe inaccuracy of the estimate of depth, the clock drift, and sycronization errors. Howeverthis method provides the fastest update rate, which can have significant advantages whenthe positioning system is interfaced into a position control system, because the introduceddelayes are smaller then the other positioning systems.

2.2.4 Long Baseline

Long Baseline systems are composed by three o more seafloor stations equipped with sensingdevice (Transponder or Responder), whose catch signals emitted by a transceiver mountedon the underwater vehicle. The position is determined using three o more time of flightranges generated in the vehicle/stations communications. The fix generated is referred withrespect to relative or absolute seafloor coordinates, thus a LBL system does not require avessel positioning system. The seafloor stations are typically separated by a baseline between100 to 6000 meters.

LBL system is capable of determing a position with a good of accuracy, independentof water depth and relative good accuracy for large areas. However the system complex-ity requires expert operators to deploy and recover the seafloor stations. Moreover, thedeployment requires a comprehensive calibration and expensive equipments.

2.2.5 Positioning Modes for LBL

Conventional positioning modes for LBL architecture are:

• Direct Ranging

10

Chapter 2. Acoustic Positioning Systems

• Intelligent Acoustic Remote

• Relay

• Pseudo Ranging

Figure 2.9: Long Baseline and Direct Ranging

Direct Ranging

In direct ranging mode the vehicle transceiver interrogates the seafloor stations, it waits forthe array replays and the on board processing derives a vehicle position. The interrogationprocess can be sequential or simultaneous. Sequential approach is based on a IndividualInterrogation Frequency (IIF) and a Common Replay Frequency (CRF), insted simultaneousapproach is based on a Common Interrogation Frequency (CIF) and a Individual ReplayFrequency (IRF).

Intelligent Acoustic Remote

Intelligent acoustic remote mode is similar to direct ranging with simultaneous interrogation,but in this configuration, the initial time of interrogation is given by an acoustic commandemitted from a surface control station. Also, the position is performed on surface controlstation, thus it is necessary a further communication between underwater vehicle and surfacestation. This solution is not reliable because the acoustic communication cycle plays a fun-damental role in the position update rate, which is slower then the direct ranging. About thefix accuracy, Direct Ranging and Intelligent Acoustic Remote have the same performances.

Relay

Relay mode is a variant of previous one, in this solution the surface station interrogatesthe AUV transceiver on a Relay Interrogation Frequency, the vehicle device interrogates the

11

Chapter 2. Acoustic Positioning Systems

seafloor array by a CIF. The array replays and CIF are detected bye surface station, whichknows his position respect to seafloor beacons thus it can perform the underwater vehicleposition. Relay approach is more reliable than Intelligent Acoustic Remote, otherwise thefixes accuracy is afflicted by the inaccuracy of vertical range detection.

Pseudo Ranging

Pseudo Ranging is based on a synchronous communication between AUV transceiver andseafloor beacon. This solution needs a very low drift clock AUV pinger, which interrogates onCIF the array. The seafloor beacons replay on IRF’s, whose are kept by the surface controlstation which determines the underwater vehicle position. Fixes are estimated on time oftransmission between the pinger interrogation and surface range detection. The drawback ofthis solution is the clock drift that introduces inaccuracy on fixes determinations, moreoververtical range detections introduce a further source of error. This implementation is rarelyused.

2.3 Latest Architecture Developments

Starting from the architectures shown into previous sections, two are the main developmentstrends in acoustic positioning systems ([31]):

• System Integration

• Inverting

2.3.1 System Integration

In system integration approach the conventional LBL/SBL/USBL are coupled with externalsensors in order to improve fixes estimation accuracy. This solution is usually based in aKalman Filter integration in fashion with aerial aided navigation. The advantage of thismethod concerns redundancy, more measurements are available in the system period, moresensor in the same operative area. Performance improvement are strictly relate with growingsystem complexity. Example of USBL/INS integration are described in [16].

2.3.2 Inverted architecture

The inverting approach is based on inversion of the conventional acoustic system, mostcommon inverted system are derived from USBL an LBL. In Inverted USBL approach themain advantage concerns signal noise ratio (SNR) in deep water, because in this configurationbeacons are deployed on the seafloor close to the AUV operative area. Drawback of thisconfiguration concern interelements coupling, for instance transducer and attitude sensordrift related to pressure, moreover power consumption when the system has to communicatewith surface station. A brief introduction of this approach is given on [26].

12

Chapter 2. Acoustic Positioning Systems

Figure 2.10: USBL/INS integration scheme, from [16]

Figure 2.11: Inverted USBL, from [32]

In Inverted LBL, seafloor stations are replaced by a surface beacons (usually buoys)equipped with a GPS system able to dertemine beacon positions. A free synchronized pinger(or a transponder) is installed on AUV, this device emit a pulse, which is kept from surfacestations and it is transmit, by aerial communication, to main station to calculate AUVposition. The estimate fixes are not transmit back to AUV, thus inverted LBL is limitedto vehicle tracking. The commercial inverted LBL used in this thesis is ACSA/ORCA GPSIntelligent Buoy known as G.I.B., in next paragraph a detailed description is given.

2.4 G.I.B. System

The G.I.B. (GPS Intelligent Buoy) system is composed by a set of 4 to 12 buoys equippedwith GPS receivers and submerged hydrophones, an acoustic pinger with a frequency rangefrom 8 to 50 kHz, and a control and display unit. The buoy network measures the Times ofFlight (TOA) of acoustic signal emitted from the pinger to the buoys, at a known rate. Thepinger is synchronized, prior to system deployment, with the GPS time.

During an estimation cycle, the pinger emits two consecutive pulses: the first is synchro-nized with GPS time and the second is delayed by an amount of time that is proportional

13

Chapter 2. Acoustic Positioning Systems

Figure 2.12: Inverted LBL, from [32]

Figure 2.13: GIB system

to the depth measured by a built-in pressure sensor. Each surface buoy transmits to thecentral unit the two measurements concerning the pulses emitted by pinger and their GPSpositions.

The central unit converts the TOA in distances multiplying times by sound speed prop-agation in the water, which is assumed known (and constant). The acoustic source positioncan be then determined, resorting to trilateration or other estimation approaches. The mainadvantages of GIB system are relatively simple operation costs, no need to deploy or calibratebeacons on the sea bed, and independence of vehicle type.

14

Chapter 2. Acoustic Positioning Systems

Figure 2.14: GIB pinger (up), GIB buoy (left) and GIB central unit (right)

Figure 2.15: Buoy deployment

15

Chapter 3

Instantaneous Positioning Algorithms

Instantaneous positioning algorithms compute a position fix given a set of ranges correspond-ing to a single instant of time. This is what is commonly referred to as Trilateration.

3.1 Trilateration

Trilateration is a method of determining the relative position by using the measured rangesbetween the target and a set of reference points with known locations. This approach needsat least three (no collinear) reference points to determine a unique relative fix in a 2Denvironment (see for instance [23]). A common algorithm to solve the problem is based onLeast Square Estimator (LSE) (see for instance [1], [23]) or Maximum Likelihood Estimator(MLE) (see for instance [2], [4], [3]) where the first approach relied on squaring the rangesand make a linearization to estimate fixes, whereas the MLE exploits the maximizationof a certain density probability function. A further approach is based on Set-Membershipapproach, as discussed on [8] which proposes an algorithm able to determine position fixes,under the assumption of unknown-but-bounded measurement noise.

3.1.1 3D Trilateration

In this section a brief introduction on LSE implementation of trilateration method is done.Assuming that the target is synchronized with the positioning system and the sound velocity(vs) is constant and known, the measured times of arrival (TOA) τi can be converted todistance’s through ri = vsτi = di + wi where wi is assumed to be Gaussian, zero mean,disturbance and di is a vector containing the actual ranges. The elements of di are definedas follows:

di = ‖p − pBi‖ =

√

(px − pBxi)2 + (py − pByi

)2 + (pz − pBzi)2 (3.1)

16

Chapter 3. Instantaneous Positioning Algorithms

where p = [x y z]T ∈ R3 denotes the position of the target with respect to some inertial

reference frame. Let pBi= [pBxi

pByipBzi

]T ; i = 1, . . . , m (where m is the number of availablemeasurements). Using the vector notation the equations can be written in compact form

r = d + w, E{wwT} = R ∈ Rm×m (3.2)

Note that by taking the square of ris, a nonlinear equation in the unknown vector p isobtained.

r2

i = d2

i + w2

i + 2widi

= (p − pBi)2 + w2

i + 2widi

= ‖p‖2 + ‖pBi‖2 − 2pT

Bip + εi, (3.3)

where the new disturbance is defined as

εi = ω2

i + 2ωidi, (3.4)

under the assumption di ≫ ωi, εi is Gaussian zero mean:

εi ≈ 2ωidi, (3.5)

and the vector ε = [ε1..εm] has covariance:

E{(p − p)(p − p)T} ≈ E{WεεTW T}

= WE{εεT}W T

= 4WDRDW T (3.6)

Rewriting the m squared ranges observations in matricial form:

2pTB1...

2pTBm

︸ ︷︷ ︸

A

p =

‖pB1‖2 − r2

1

...‖pBm

‖2 − r2m

︸ ︷︷ ︸

b

+

ε1

...εm

︸ ︷︷ ︸

ε

+‖p‖21m. (3.7)

Define matrix

M = Im −1

m1m1T

m, (3.8)

where

1m = [1 . . . 1]T ∈ Rm, (3.9)

17

Chapter 3. Instantaneous Positioning Algorithms

and note that M has 1m in its null space, i.e., M1m = 0. Now premultiplying both sides of3.7 by M we get rid of the term in ‖p‖2 and obtain the linear system:

MAp = Mb + Mε. (3.10)

A LS estimate can be obtained, if the matrix (MA)T (MA) is invertible, through:

p = Wb, (3.11)

where

W = ((MA)T (MA))−1(MA)T M. (3.12)

The resulting covariance of the estimate, in accord with assumption on ε, is

E{εεT} ≈ E{4DωωTD}

= 4DE{ωωT}D

= 4DRD (3.13)

where

D =

d1 0. . .

0 dm

. (3.14)

3.1.2 2D Trilateration

In some operative conditions, a good approximation is to consider that all the buoy hy-drophones have the same pz coordinate. This assumption introduces a symmetry in theplane where the buoys are deployed. Assume that pzi

= Z, i ∈ {1, . . . , m} where m is thenumber of available buoy measurements. Note that in the square range equation:

d2

i = (px − pBxi)2 + (py − pByi

)2 + (pz − Z)2 (3.15)

the third term is constant

K = (pz − Z). (3.16)

Define

p′Bi =

[pBxi

pByi

]

, p′[px

py

]

(3.17)

Now it is possible to rewrite (3.15) as follows

d2

i = ‖p′Bi‖2 + ‖p′‖2 − 2p′

TBip

′ + K2 (3.18)

18

Chapter 3. Instantaneous Positioning Algorithms

Stacking all the m squared range observations we obtain

2p′TB1...

2p′TBm

︸ ︷︷ ︸

A

p′ =

‖p′B1‖2 − r2

1

...‖p′Bm

‖2 − r2m

︸ ︷︷ ︸

b

+K2

1...1

︸︷︷︸

1m

+

ε1

...εm

︸ ︷︷ ︸

ε

+‖p′‖2

1...1

︸︷︷︸

1m

(3.19)

The resulting system solution can be determined in fashion with the solution of paragraph3.1.1, in this case the solution is a 2D fix calculated utilizing 3D measurements. The GIBsystem typically uses 4 buoys with hydrophones approximately at the same depth. In thiscase there is not enough information to uniquely determine a 3D position due to the planesymmetry. The planar approach needs only 3 buoys to determine an unique fix, introducingredundancy. Moreover the depth information is not lost because GIB system gives a depthinformation codified as explained in paragraph 2.4.

Weighted Trilateration

One of the common assumptions underlying most process modeling methods, LSE, is thateach data point provides equally precise information about the process variation. In otherwords, the standard deviation of the error term is constant over all values of the variables.This assumption clearly does not hold, because in real application each variable has itsinformative content. A solution to this problem can be given by Weighted Least SquareEstimator (WLSE). This approach minimizes residuals, like in LSE, considers also a differentweight for each point. The main disadvantage of WLSE is probably the fact that the weightshave to be known exactly. This is not the case in real applications, where estimated weightsmust be used instead. However, experience indicates that small variations in the the weightsdue to estimation do not often affect an estimation analysis.Premultiplying both sides of 3.19 by M matrix introduced in 3.8,the following linear systemis obtained:

MAp = Mb + Mε.

This method provides a solution that minimize the follwing cost functional

argminp

‖ε‖2 = argminp

‖MAp − Mb‖2 (3.20)

In the weighted case the linear system is multiply on the left by a symmetric positive definitematrix which take in account the covariance of the error.

Lets define

R , E{MεεT MT}

= ME{εεT}MT

= 4MDRDMT , (3.21)

19

Chapter 3. Instantaneous Positioning Algorithms

where R is defined in 3.2.

W , R− 12

then the resulting linear is

WMAp = WMb + WMε,

where the optimal solution in sense of Least Square is

p = (WMA)†WMb. (3.22)

Define (·)† as Pseoudoinversion operator

(WMA)† = ((WMA)T WMA)−1(WMA)T

= (AT MT R−1MA)−1AT MT R12 , (3.23)

define

K = MA, (3.24)

the solution can be express as

p = (KT R−1K)−1KT R−1b. (3.25)

The resulting covariance of the noise is

E{WMε(WMε)T} = E{WMεεT MT W T}

= WME{εεT}MT W

= R12 RR

12

= Im. (3.26)

Define the estimation error as

p = p − p, (3.27)

the estimate error covariance is

E{ppT} = E{(KT R−1K)−1KT R−1MεεT MT R−1K(KT R−1K)−T}

= (KT R−1K)−1KT R−1ME{εεT}MT R−1K(KT R−1K)−T

= (KT R−1K)−1KT R−1RR−1K(KT R−1K)−T

= (KT R−1K)−T . (3.28)

In this approach the functional expression is

argminp

‖WMε‖2 = argminp

εT MR−1Mε. (3.29)

LSE and WLSE have the same solution when R = σ2Im where Im is the identity matrixwith dimensions in accord with vector ε.

20

Chapter 3. Instantaneous Positioning Algorithms

3.2 Maximum Likelihood Estimation

A further solution to the Trilateration problem is based on Maximum Likelihood (ML). Thisis based on the maximization of the Likelihood function, which is a conditional probability.Remembering that r represent the measured ranges, and p the AUV position, the likelihoodfunction l(r|p) can be considered as a function of its second argument p, in other words,likelihood function works backward from probability, given a set of collect data r and adistribution of probability, it finds the optimal set of estimation parameters. Thus, thevalues of p that maximize l(r|p) is known as the Maximum Likelihood Estimator (MLE).A detailed description of Maximum Likelihood Estimation theory is given on [28]. TheLikelihood function, in accord with the Gaussianity assumption on the disturbance (3.2), isgiven as

l(r|p) =1

(2π)m2 |R|

12

exp{−1

2(r − d)T R−1(r − d)} (3.30)

where p represent the AUV position, r the measured range, and d the actual range.

A common practice in MLE is to work with the log-likelihood function

ln{l(r|p)} = ln

{

1

(2π)m2 |R|

12

}

−1

2(r − d)TR−1(r − d) (3.31)

Neglecting constant terms, the ML estimate can be found by solving

argminp

F (p) = argminp

1

2(r − d)T R−1(r − d) (3.32)

Note that the cost function F is not differentiable when some of the ranges di vanish, thatis, when the position of the vehicle coincides with the position of a landmark. It is realisticto assume that this situation never occurs in practice.

The MLE procedure is an analytic maximization for which a closed form solution doesnot exist. Often, and in practical application the maximization problem is solved with aniterative approach. Most common iterative algorithms are Gradient and Newton, which arebased on the likelihood gradient (∇F (p)) and hessian (∇2F (p)), respectively. A detaileddescription of those iterative algorithms is given in [15].

An iterative optimization scheme, at each execution requires a direction of minimization,and a step size determination. The direction of minimization, in the Gradient, is given by

δk = ∇F (pk),

where pk represents the estimate determined at previous iteration, instead in the case ofDescent Newton, the direction of minimization is given by

δk = −∇2F (pk)−1∇F (pk).

21

Chapter 3. Instantaneous Positioning Algorithms

The new estimate is calculated by moving along δk of an amount equal to the step size,which can be constant or variable. The step size problem is discussed in section 3.2.3.

An iterative scheme could require an infinite number of execution to determine the solu-tion to the MLE, avoiding this problem the scheme is arrested when a certain stop criterionis verified. The criterion adopted for the proposed iterative schemes, relies on:

• Maximum number of iteration (MAXIT );

• Norm of the Gradient (|∇F (pk)| < ε1);

• Cost function decreasing step (|pk − pk−1| < ε2).

ε1, ε2 are determined with respect to the error tolerances required.The Section 3.2.1 considers the case where likelihood function is based on squared ranges

and gives the expression for gradient and hessian, the section 3.2.2 considers the case of pureranges, gradient and hessian expression are given also.

3.2.1 MLE: Squared ranges

In fashion with LSE in this section are considered squared ranges insted pure range.Let define

di = ‖p − pBi‖2 ∈ R, i ∈ {1, ..., m}, (3.33)

ri = di + εi, (3.34)

be the squared ranges of actual distance between the AUV and buoys, and pB and p aredefined in section 3.1.1.

The MLE cost function F has the following form

F (p) =1

2(r − d)TR−1(r − d) (3.35)

where

d , [d1, d2, . . . , dm]T ∈ Rm,

r , [r1, r2, . . . , rm]T ∈ Rm,

w , [w1, w2, . . . , wm]T ∈ Rm,

where R is the error covariance matrix define as

R , E{wwT} ∈ Rm×m.

As mentioned in previous section the cost function in (3.35) has no closed form solution. Inorder to determine the ML estimate it is then necessary to resort to some kind of iterative

22

Chapter 3. Instantaneous Positioning Algorithms

optimization scheme. The gradient and Hessian of the cost function are derived in order toimplement a Gradient descent and Newton optimization algorithms.The gradient for the (3.35) is

∇F (p) = −2((r − d)TR−1C)T , (3.36)

and hessian is

∇2F (p) = 4CR−1CT − 2αT1mIn

= 4(p1m − pB)TR−1(p1m − pB) − 2(r − d)R−11mIn, (3.37)

where

C =

(p − pB1)T

...(p − pBm

)T

, (3.38)

and

α = R−1(r − d) = [α1, α2, . . . , αm]T . (3.39)

The reader can find the mathematical derivation of 3.36 and 3.37 on appendix A

3.2.2 MLE: Pure ranges

An alternative implementation of MLE can be developed considering pure range instead ofsquared ranges. According to (A.1), and defining

di = ‖p − pBi‖ ∈ R, i ∈ {1, ..., m}, (3.40)

ri = di + wi. (3.41)

The MLE cost function have the expression seen in (3.32)

F (p) =1

2(r − d)TR−1(r − d), (3.42)

where

d , [d1, d2, . . . , dm]T ∈ Rm,

r , [r1, r2, . . . , rm]T ∈ Rm,

w , [w1, w2, . . . , wm]T ∈ Rm,

and R is the error covariance matrix defined as

R , E{wwT} ∈ Rm×m (3.43)

23

Chapter 3. Instantaneous Positioning Algorithms

The gradient of the log likelihood function is given by

∇F (p) = −((r − d)T R−1D−1C)T , (3.44)

and its Hessian by

∇2F (p) = {[CTD−1R−1D−1C] −

m∑

i=1

αiHi}, (3.45)

where the matrix C is already defined in 3.38, and

D−1 =

1

d1. . . 0

.... . .

...0 . . . 1

dm

(3.46)

α = R−1(r − d) = [α1, α2, . . . , αm]T (3.47)

Hi =1

diIn −

1

d3i

(p − pBi)(p − pBi

)T (3.48)

3.2.3 Stepsize: Armijo rule

An implementation of MLE with an iterative scheme, determines at each execution an opti-mal step size. This operation, in practice, is very expensive computationally if good precisionin the computation is required. The issue of optimal step size is usually referred to line search.There are several methods of solving this problem but it is sufficient consider two classes,exact line search methods and inexact linear methods. The Armijo rule belongs to the secondclass. This approach guarantees a sufficient degree of accuracy to ensure the convergence ofiterative method. The Armijo rule consists in finding the first integer mi that satisfies

F (pk) − F (pk + βmisδk) ≥ −γβmis∇F (pk)T δk, (3.49)

where δk and pk represent respectively the search direction and the solution on kth iteration.Let s > 1 be the initial stepsize, γ ∈ (0, 1) the tolerance factor, and β ∈ (0, 1) the stepsizereduction factor and mi ∈ {1, 2, . . .}. The stepsize is calculated as

αk = βmks, (3.50)

where mk is the first integer that satisfies the inequality (3.49). A discussion and a proof onArmijo rule are given into [7].

3.2.4 Simulation Results

This paragraph describes the results of simulations to validate the proposed algorithms. Inthe simulations, four buoys were placed at the corners of a square with a 500m side. The

24

Chapter 3. Instantaneous Positioning Algorithms

accepted regions

α

γα∇F (pk)T δk

α∇F (pk)T δk

F (pk)

Figure 3.1: Step size determination by Armijo Rule.

measurement noise covariance matrix was set to R = σ2Im with σ = 9m, where Im is aidentity matrix with dimensions in accord with the number of available buoys. The initialposition estimate was set p0 = [20 120]T , and the depth is constant and known. In table3.2.4 the stop criterion and the Armijo’s rule parameters are shown. Figure 3.2 shows theevolution of the cost function (F (pk)) together with the norm of the gradient(|∇F (pk)|),and the estimate components for the Gradient and with pure ranges. Figure 3.3 shows theevolution of the cost function together with the norm of the gradient, and the estimatecomponents for the Newton and with pure ranges. The vertical axis for the cost functionand the norm gradient plots use the logarithmic scale. Figure 3.4 and Figure 3.5 show theGradient and Newton methods with squared ranges. As in the case of pure ranges solutionthe figures shows the evolution of the cost function together with the norm of the gradient,and the estimate components. Note the quadratic convergence of the Newton methods whenclose to the minimum.

Note that the cost functions have local minimum that depend highly on the number andposition of the buoys, as well as the intensity of the measurement noise. Figure 3.6 shows thisfact for the case of pure ranges MLE and Figure 3.7 shows the case of squared ranges MLE.This is an important issue, as well as on many optimization problems, that prevents themethods from being global. The the values on the contour lines represent the cost functionvalue inside the region

The solutions based on squared ranges likelihood function requires an accurate setupof the step size procedure and a stop criterion, because the cost function decreases fasterthan in the case of the pure ranges likelihood function. Both implementations show thatthe Newton method converges to the optimal solution with less number of iterations whencompared with the Gradient method. Hence, the MLE implementation based on pure rangeslikelihood function and the Newton method is suitable for real-time implementation.

25

Chapter 3. Instantaneous Positioning Algorithms

Table 3.1: MLE: Simulation parameters

ε1 1e − 9ε2 1e − 9α0 = β0s 2β 5e − 1γ 1e − 1

0 50 100 150 200 250 300 35010

−10

10−5

100

105

∇ F

(pk)

Iteration

0 50 100 150 200 250 300 35010

−2

100

102

F(p

k)

Iteration

0 50 100 150 200 250 300 350−10

−8

−6

−4

−2

0

Nor

th E

stim

ate

Err

or [m

]

Iteration

0 50 100 150 200 250 300 350−30

−25

−20

−15

−10

−5

0

5

10

Eas

t Est

imat

e E

rror

[m]

Iteration

Figure 3.2: Gradient and pure ranges.

26

Chapter 3. Instantaneous Positioning Algorithms

0 0.5 1 1.5 2 2.5 310

−20

10−10

100

1010

∇ F

(pk)

Iteration

0 0.5 1 1.5 2 2.5 310

−2

100

102

F(p

k)

Iteration

0 0.5 1 1.5 2 2.5 3−10

−5

0

5

Nor

th E

stim

ate

Err

or [m

]

Iteration

0 0.5 1 1.5 2 2.5 3−30

−20

−10

0

10

Eas

t Est

imat

e E

rror

[m]

Iteration

Figure 3.3: Newton and pure ranges.

27

Chapter 3. Instantaneous Positioning Algorithms

0 50 100 150 200 250 300 350 40010

−10

10−5

100

105

∇ F

(pk)

Iteration

0 50 100 150 200 250 300 350 40010

−2

100

102

F(p

k)

Iteration

0 50 100 150 200 250 300 350 400−10

−8

−6

−4

−2

0

Nor

th E

stim

ate

Err

or [m

]

Iteration

0 50 100 150 200 250 300 350 400−30

−20

−10

0

10

Eas

t Est

imat

e E

rror

[m]

Iteration

Figure 3.4: Gradient and squared ranges.

28

Chapter 3. Instantaneous Positioning Algorithms

0 0.5 1 1.5 2 2.5 3 3.5 410

−20

10−10

100

1010

∇ F

(pk)

Iteration

0 0.5 1 1.5 2 2.5 3 3.5 410

−2

100

102

F(p

k)

Iteration

0 0.5 1 1.5 2 2.5 3 3.5 4−10

−8

−6

−4

−2

0

Nor

th E

stim

ate

Err

or [m

]

Iteration

0 0.5 1 1.5 2 2.5 3 3.5 4−30

−20

−10

0

10

Eas

t Est

imat

e E

rror

[m]

Iteration

Figure 3.5: Newton and squared ranges.

29

Chapter 3. Instantaneous Positioning Algorithms

1

12

2

23 3

3

3

3

4

4

4

4

4

55

5

5

5

66

6

6

6

7

7

7

7

7

7

8

8

8

8

8

8

9

9

9

9

9

9

10

10

10

10

10

1011

11

11

11

11

11

12

12

12

12

12

1313

13

13

13

14

14

14

14

14

15

15

15

15

15

16

16

1616

16

16

17

17

17 17

17

17

18

18

18 18

18

18

19

19

1919

19

19

20

20

20 20

20

20

21

21

21 21

21

21

22

22

22

22

22

22

23

23

23

23

23

23

24

24

24

24

24

24

25

25

25

25

25

25

26

26

26

26

26

2627

27

27

27

27

27

28

28

2828

28

28

29

29

29

29

29

29

30

30

30

30

3031

31

31 31

31

32

32

32 32

32

33

3333

33

33

34

3434 34

34

35

35

35

35

3536

36

36

36

36

37

37

37

37

37

38

38

38 38

38

39

39

39

39

39

40

40

40

40

40

41

41

41 41

41

42

42

42

42

42

43

4343

43

43

44

44

44

44

44

4545

45

45

45

4646

46

46

46

4747

47

47

47

48

4848

48

48

49

49

49

49

49

49

5050

50

50

5151

51

51

52

52 52

52

5353

53

53

54

54

54

54

55

5555

55

56

56 56

5657

57 57

57

5858

58

58

59

59 59

59

60

6060

60

61

6161

61

62

6262

62

63

6363

63

64

6464

64

65

65

65

65

66

66

66

66

67

67

67

67

68

68

68

68

69

69

69

69

70

70

7071

71

7172

72

73

7374

74

75

75

76

76

77

7778

78

79

79

80

80

81

8182

82

8383

8484

85

85

86

86

87

87

88

88

89 8990

9091 91

9292

9393

94

9495

9596

9697 9798

9899 99100

100101

101102

102103103104 104

105105106

0 10 20 30 40 50

−10

0

10

20

30

40

Functional‘s contourMLE fixActual fixBuoys

Figure 3.6: Local and global minimums for pure ranges log-likelihood function.

1

1

2

2

2

2

2

3

3

3

3

3

44

4

4

4

45

5

55

5

56

6

6

6

66

7

77

7

7

7

78

88

8

8

8

89

9

9

9

99

9

10

10

10

10

10

10

10

11

11

11

11

11

1112

12

12

12

12

12

13

13

13

13

13

13

14

14

14

14

14

1415

15

15

15

15

15

16

16

16

16

16

1617

17

17

17

17

17

18

18

18

18

18

1819

19

19

19

19

1920

20

20

20

20

2021

21

21

21

21

2122

22

22

2222

2223

23

23

23

23

2324

24

24

2424

2425

25

25

2525

2526

26

26

2626

27

27

27

27

27

28

28

28

28 28

29

29

29

29

29

30

30

30

30 30

31

31

31

31 31

32

32

32

32

32

33

33

33

33 33

34

34

34

34 34

35

35

35

35 35

36

36

36

36 36

37

37

37

37 37

38

38

38

38 38

39

39

39

39 39

40

40

40

40 40

41

41

41

41 41

42

42

42

42 42

43

43

43

43

44

44

44

45

45

45

46

46

46

47

47

47

48

48

48

49

49

49

50

50

50

51

51

51

52

52

52

53

53

53

54

54

54

55

55

55

56

56

56

57

57

57

58

58

58

59

59

59

60

60

60

61

61

61

62

62

62

63

63

63

64

64

64

65

65

65

66

66

66

67

67

67

68

68

68

69

69

69

70

70

70

71

71

71

72

72

72

73

73

73

74

74

74

75

75

75

76

76

76

77

77

77

78

78

78

79

79

79

80

80

80

81

81

81

82

82

82

83

83

83

84

84

84

85

85

85

86

86

86

87

87

87

88

88

88

89

89

89

90

90

90

91

91

91

92

92

92

93

93

93

94

94

94

95

95

95

96

96

96

97

97

97

98

98

98

99

99

99

100

100

100

101

101

101

102

102

102

103

103

103

104

104

104

105

105

105

106

106

106

107

107

107

108

108

108

109

109

109

110

110

110

111

111

111

112

112

112

113

113

113

114

114

114

115

115

115

116

116

116

117

117

117

118

118

118

119

119

119

120

120

121

121

122

122

123

123

124

124

125

125

126

126

127

127

128

128

129

129

130

130

131

131

132

132

133

133

134

134

135

135

136

136

137

137

138

138

139

139

140

140

141

141

142

142

143

143

144

144

145

145

146

146

147

147

148

148

149

149

150

150

151

151

152

152

153

153

154

154

155

155

156

156

157

157

158

158

159

159160

161162

163

164165

166

167

168

169170

171

172

173

174175

176177

178179

180

181

182183

184

185

186187

188189

190191

192193

194 195196

197

198

199

200

201202 203

204 205206207

208209210

211212

213

214215

216217218219220

221222223224

225

−5 0 5 10 15 20 25 30 35 40−25

−20

−15

−10

−5

0

5

10

15

20

25Functional‘s contourMLE fixActual fixBuoys

Figure 3.7: Local and global minimums for squared ranges log-likelihood function.

30

Chapter 3. Instantaneous Positioning Algorithms

3.3 Cramer-Rao Lower Bound

The Cramer-Rao Lower Bound represent the minimum variance achievable from any unbiasedestimator that uses observations with a given probability density function. A completedissertation on Cramer-Rao Lower Bound can be found on [14], and [28]. Assuming that thebeacon positions are exactly known, and (3.2) is still valid, the likelihood function becomesa multivariable normal distribution

l(r|p) =1

(2π)m2 |R|

12

exp{−1

2(r − d)T R−1(r − d)} (3.51)

where d = d(p). The Cramer-Rao theorem states that

tr(Σ) , tr(E{ppT})

= tr(E{(p(r) − p)(p(r) − p)T}) ≥ tr(J−1) (3.52)

where tr(·) is the matrix trace operator, p(r) is the MLE, p = p − p, and J is the FisherInformation Matrix given by

J , E

{(∂

∂pln l(r|p)

)(∂

∂pln l(r|p)

)T}

= −E

{∂2

∂p2ln l(r|p)

}

(3.53)

The log-likelihood function can be written as (3.31) and taking derivatives respect to p weget

∂

∂pln l(r|p) =

(∂d

∂p

)T

R−1(r − d)

= CT R−1(r − d), (3.54)

where

C ,∂

∂p=

∂d1

∂px

∂d1

∂py

∂d1

∂pz

......

...∂dm

∂px

∂dm

∂py

∂dm

∂pz

=

px−pBx1

d1

py−pBy1

d1

pz−pBz1

d1...

......

px−pBxm

dm

py−pBym

dm

pz−pBzm

dm

. (3.55)

The Fisher Information Matrix can be computed as

J = E{[CT R−1(r − d)][CTR−1(r − d)]T

}

= E{CT R−1(r − d)(r − d)T R−1C

}

= CT R−1E{(r − d)(r − d)T

}R−1C

= CT R−1RR−1C

= CT R−1C (3.56)

31

Chapter 3. Instantaneous Positioning Algorithms

which finally yields

tr(Σ) ≥ tr([CT R−1C]−1) (3.57)

The reader can find an extensive discussion on CRLB into [9], and [28], where a proof of3.57 is also given.

3.3.1 Buoy Geometry

The CRLB is a useful tool to analyze the buoy geometry influence on minimum varianceachievable. As seen on 3.55, CRLB is directly related with buoy positions by using theCRLB is possible asses to which buoy configuration is optimal for a certain application. Inthe following figures (3.8,3.9,3.10) the lower bound on variance for several buoy geometry isshown. The CRLB is codified as a contour graph using a cold color scale, and the values onthe contour lines represent the minimum variance achievable inside the region. In order tohave a clear planar representation, the following assumptions were done:

• AUV depth exactly known;

• Pinger perfectly synchronized with buoys.

Under those assumption the C matrix define on 3.55 can be rewritten as

C =

px−pBx1

d1

py−pBy1

d1...

...px−pBxm

dm

py−pBym

dm

(3.58)

3.3.2 Algorithm Comparison

The CRLB sets the minimum variance of estimated fixes given a covariance measurementmatrix and a configuration of beacons. This limit could also be used to compare the per-formance of different solution. Figure 3.11 depicts the minimum variance achievable withsix different algorithms: 2D LSE, 2D WLSE, MLE based on Gradient method and pureranges, MLE based on Newton method and pure ranges, MLE based on Gradient methodand squared ranges, and MLE based on Newton method with squared ranges. In the x-axisare plotted the noise standard deviation value σ and in the y-axis are plotted the values ofthe variance. In this simulation four buoys are considered and σ ∈ [0.1, 19.1], where σ isexpressed in meters, and the noise covariance matrix is defined as R = σ2Im. On the eval-uation of the algorithm variances, for each values of σ, 2000 Montecarlo simulations weredone. For some values of σ the limit of CRLB is violated, this is due to the approximationof the algorithm variances done:

σ2 ≈1

N

N∑

i=1

(pi − µ)2,

32

Chapter 3. Instantaneous Positioning Algorithms

10.5

10.5

10.5

10.5

10.5

10.5

10.5

10.5

10.5

1010

10

10

10

1010

10

10

10

9.5

9.5

9.5

9.5

9.5

9.59.5

9.5

9.5

9.5

9.5

11

11

11

11

11

11

11

11

11.5

11.5

11.5

11.5

11.5

11.5

11.5

11.5

12

12

12

12

12

1212

12

12.5

12.5

12.5

12.5

12.5

12.5

12.512.5

13

13

13

13

13

13

13

13

13.5

13.5

13.5

13.5

14

14

14

14

14

14 1414

14.5

14.5

14.5

14.5

15

15

15

15

15.515.5

15.5

15.5

16

16

16

16

16.516.5

16.5

16.5

17

17

1717

9.5

9.5

9.5

9.5

17.5

17.5

17.5

17.5

18

18

1818

18.5 18.5

18.518.5

19

19

19

19

19.5

19.5

19.5

19.5

20

20

20

20

20.5

20.5

20.5

20.5

10

10

21

21

21

21

21.5

21.5

21.5

21.5

22

22

22

22

10.5

10.5 10.5

10.5

23 23

23 23

11 111111

−100 −50 0 50 100 150 200 250 300−100

−50

0

50

100

150

200

250

300

tr{[CTR−1C]−1}Buoys

Figure 3.8: Configuration of 4 buoys in square shape.

12

12

12

12

12 12

12

12

12

12

12

12

12

12

11

11

11

11

11

11

11

11

11

11

11

11

11

11

13

13

13

13

13

13

13

13

13

14

14

14

14

14

14

14

14

15

15

15

15

15

15

15

15

16 16

16

16

16

16

17

17

17

17

17

17

10

10 10

10

10

13

1313

13

18

18

18

18

19

19

19

19

20

20

20

20

21

21

21

22

22

22

23

23

14

14

14

14

24

24

25

25

26

26

27

27

28

28

29

29

30

30

31

31

32

32

33

33

34

34

35

35

36

36

37

37

38

38

39

39

40

40

40

40

41

41

42

42

42

43

43

43

44

44

45

45

45

46

46

15

1547

47

48

48

49

49

50

50

51

51

52

52

53

53

54

54

55

55

56

56

57

57

58

58

18

18

18

18

59

59

60

60

61

61

62

62

63

63

64

64

19

19

19

19

65

65

66

66

67

67

16

16

68

68

69

69

20

20

20

20

70

70

71

71

72

72

73

73

21

21

21

21

74

74

75

75

76

76

77

77

22

22

78

78

79

79

80

80

81

81

23

23

82

82

83

83

84

84

85

85

24

24

17

17

86

86

87

87

88

88

25

25

89

89

90

90

26

26

91

91

92

92

93

93

27

27

94

94

95

95

28

28

96

96

97

97

98

98

29

29

99

99

100

100

30

30

18

18

31

3132

32

33

33

34

34

35

35

36

36

37

37

38

38

39

39

40

40

41

41

−100 −50 0 50 100 150 200 250 300 350 400−100

−50

0

50

100

150

200

250

300

350

400

tr{[CTR−1C]−1}Buoys

Figure 3.9: Configuration of 4 buoys in rhombus shape.

33

Chapter 3. Instantaneous Positioning Algorithms

13

13

13

13

13

1313

13

13

13

13

13

13

13

14

14

14

1515

15

16

16

16

17

17

17

14

14

14 14

18

18

18

19

19

19

20

20

20

15

15

15

15

21

21

21

22

22

22

23

23

23

16

16

16

24

24

2425

25

25

17

17

17

14

14

14

14

26

26

26

27

27

27

15

15

15

15

18

18

18

28

28

28

16

16

16

16

29

29

29

19

19

19

17

17

17

17

30

30

30

18

18

18

18

31

31

20

20

20

32

32

33

33

19

19

34

34

35

35

21

21

20

20

36

36

37

37

38

38

22

22

39

39

21

21

13

13

40

40

41

41

42

42

23

23

22

22

43

43

44

44

45

45

46

46

24

24

23

23

47

47

48

48

49

49

50

50

25

25

24

24

51

51

52

52

53

53

26

26

54

25

25

55

56

27

27

5758

26

26

28

28 596061

29

29

27

27

62 6330

64

14

14

65

31

28

28

6632 67 68

29

29

3369

34 70

71

35

30

30

7236 7337

74

15

15

31

31

38 7539 7640

32

32

41 77

42

1616

4344

−100 −50 0 50 100 150 200 250 300 350 400−100

−50

0

50

100

150

200

250

300

tr{[CTR−1C]−1}Buoys

Figure 3.10: Configuration of 3 buoys in triangle shape.

this approximation becomes equality when N → ∞.The Figure 3.11 shows that the LSE provide estimates with less estimation error when

compared with MLE. The precision of an LSE can be improved by taking into account themeasurement noise variance providing a WLSE. The MLE is found to be efficient because itreaches the minimum achievable variance given by the Cramer-Rao Lower Bound (CRLB).However, the computational effort is higher then the LSE case. The solution based onsquared ranges likelihood function requires an accurate setup of the stepsize procedure anda stop criterion, because the cost function decreases faster than in the case of the pure rangeslikelihood function. Both mechanizations show that the Newton method converges to theoptimal solution with less number of iterations when compared with the Gradient method.Hence, the MLE mechanization based on pure ranges likelihood function and the Newtonmethod are suitable for real-time implementation.

34

Chap

ter3.

Instan

taneou

sPosition

ing

Algorith

ms

0 2 4 6 8 10 12 14 16 18 200

100

200

300

400

500

600

700

σ

tr{Σ}: Triangulationtr{Σ}: Weighted Triang.tr{Σ}: Gradienttr{Σ}: Newtontr{Σ}: Gradient2tr{Σ}: Newton2CRLB

Figu

re3.11:

Algorith

maccu

racycom

pared

with

CR

LB

.

35

Chapter 4

Filtering Methods

This chapter address the estimation of state vector for linear and nonlinear dynamic systems.This family of algorithms bases the estimation process not only on measurement informationbut also on dynamic/kinematic property of the system subject of estimation. The stateestimator of a linear dynamic system is introduced on paragraph 4.1.1, the extension tononlinear dynamic system is addressed into paragraph 4.2.1.

4.1 Linear Estimation Problem

Consider a discrete system governed by a stochastic linear difference state equations as

xk = Ak−1 + νk−1, (4.1)

and measurement equations

zk = Cxk + ωk. (4.2)

The random variables νk and ωk represent the process and measurement noises (respectively),that are assumed to be independent (of each other), white, and with normal probabilitydistributions

ν ∼ N(0, Q), (4.3)

ω ∼ N(0, R). (4.4)

where

R , E{ωωT} (4.5)

Q , E{ννT} (4.6)

are process noise covariance and measurement noise covariance matrices respectively. Thematrices A,C,Q, and R are assumed known, typically are time-varying but in practicalapplication are considered constant. The initial state x0 is unknown and described as arandom Gaussian variable with known mean (typically is the real initial state) and covariance(choose in accord with sensor and plant estimated noises).

36

Chapter 4. Filtering Methods

4.1.1 The Discrete Kalman Filter

The discrete Kalman Filter (KF) is a recursive algorithm that compute the estimate forthe current state only with the estimated state from the previous time step and the currentmeasurements. The equations for the Kalman filter are grouped into two classes: time updateequations and measurement update equations. The time update equations predict currentstate and error covariance matrix using the previous time step state. The measurementupdate equations correct the current state and error covariance matrix with the currentmeasurements.The specific equations for the time and measurement updates are presented below

xk(−) = Axk−1 (4.7)

Pk(−) = APk−1AT + Q (4.8)

A is from (4.1), while Q is from (4.3).

Kk = Pk(−)CT (CPk(−)CT + R)−1 (4.9)

xk = xk(−) + Kk(zk − Cxk(−)) (4.10)

Pk = (I − KkC)Pk(−) (4.11)

The first task during the measurement update is to compute the Kalman gain, Kk . Thenext step is to measure the process to obtain zk , and then to generate an a posteriori stateestimate by incorporating the measurement as in (4.10). The final step is to obtain an aposterior error covariance estimate via (4.11). The recursive nature is one useful features ofthe Kalman filter, it makes practical implementations really feasible.

In (4.1) and (4.2), the noise process ωk and νk can be assumed to be Gaussian. Sincethe state xk is a linear function of νk−1, it is also Gaussian. The same conclusion can bereached about the estimate xk since it is a linear combination of xk and ωk; so the error inthe estimate must be a Gaussian variable with zero mean and covariance Pk. A completejustification and theory of Kalman filters can be found in [5], [6], [11], [24], [35].

4.1.2 Uncertainty Ellipsoid

The uncertainty ellipsoid characterize the concentration of the estimate about the true valueof the state. Considering the probability density or a set of K Gaussian variables

l(p) =1

(|2π|K/2|P |1/2)exp(−

1

2(p − p)T P−1(p − p) (4.12)

Let K = 2 it is possible shown the probability density in a 3D graph as in Figure 4.1.Observing from (4.12) that the equal-probability contours are defined by the relation

(p − p)T P−1(p − p) = c2 (4.13)

which is the equation for an ellipse when K = 2.

37

Chapter 4. Filtering Methods

Figure 4.1: Gaussian probability density for K = 2.

Property. For K = 2, the probability that the error vector lies inside an ellipse whoseequation is

(p − p)T P−1(p − p) = c2 (4.14)

is

l = 1 − exp(−c2

2) (4.15)

The proof of this property and the general case for K > 2 are discussed in [28]. Con-cluding it is possible assert that the eigenvalue of P are scale factor for the ellipsoid axes.An application of uncertainty ellipsoid theory for position estimation is presented on [18].

4.1.3 Alternative formulation

In this paragraph an alternative formulation for the KF is introduced as described on [24].The equations that describe the gain matrix Kk and the error covariance matrix Pk can bewritten in a different form. First, lets prove an interinteresting inversion lemma.

Matrix Inversion Lemma. Suppose (n × n) matrices B and R are positive-definite. LetH be any, possibly rectangular, matrix. Let A be an n × n matrix related to B, R, and Haccording to

A = B − BHT [HBHT + R]−1HB (4.16)

38

Chapter 4. Filtering Methods

Then, A−1 is given by

A−1 = B−1 + HT R−1H (4.17)

Proof. The proof follows by direct multiplication.

AA−1 = [B − BHT (HBHT + R)−1HB][B−1 + HT R−1H ]

= I − BHT [(HBHT + R)−1 − R−1 + (HBHT + R)−1HBHTR−1]H

= I − BHT [(HBHT + R)−1 − (I + HBHT R−1) − R−1]H

= I

q.e.d

This result can be applied immediately to (4.11). Introduce the following correspondence torelate quantities in (4.11) and (4.16). Let

A ∼ Pk, B ∼ Pk(−), H ∼ Ck, R ∼ Rk

Assume that Rk and Pk(−) are positive-definite and apply (4.17), we obtain the new setequations for the EKF measurement update

Pk = Pk(−) − Pk(−)CTk [CkPk(−)CT

k + Rk]−1CkPk(−) (4.18)

Kk = PkCTk R−1

k (4.19)

xk = xk(−) + Kk(zk − Ckxk(−)). (4.20)

4.1.4 Example

In this paragraph a basical example is presented, a two states linear system model is con-sidered in order to illustrate the relation between P covariance matrix and the uncertaintyellipsoids.

Linear system

The system subject of this example is described by the following linear equations

xk+1 =

[1 00 1

]

xk + νk (4.21)

with a measurement z ∈ Rm that it

zk =

[1 00 1

]

xk + ωk. (4.22)

The filter paramter are shown in table4.1.4.

39

Chapter 4. Filtering Methods

Table 4.1: Kalman Filter Matrices

P0 diag{[(20)2 (20)2]}Q diag{[90 160]}R diag{[100 25]}

−5 0 5 10 15 20 25 30 35 40−10

−5

0

5

10

15

20

x, xhat

y, y

hat

Actual TrajectoryEstimated Trajectory

0 5 10 15 20 25 30−1

0

1

2

3

4

5

t

x err

0 5 10 15 20 25 30−2

0

2

4

6

8

t

y err

0 5 10 15 20 25 300

50

100

150

200

250

300

350

400

t

P+ e

igen

valu

es

Figure 4.2: Kalman Filter simulation results.

4.2 Nonlinear Estimation Problem

Considering the discrete-time process governed by the non linear stochastic equation

xk = f(xk−1, νk−1), (4.23)

with a measurement z ∈ Rm that it

zk = h(xk, ωk), (4.24)

where the random variables νk and ωk represent the process and measurement noise. Inthis case the non-linear function f(·) in the difference equation (4.23) relates the state at theprevious time step k−1 to the state at the current time step k. It includes as parameters thezero-mean process noise νk. The non-linear function h in the measurement equation (4.24)relates the state xk to the measurement zk. It is important underlining that the distributionsof the random variables are no longer normal after undergoing their respective nonlineartransformations, consequently, it is not necessary that ω and ν be Gaussian. However, theinitial value x0 may be assumed to be Gaussian random variate with known mean known

40

Chapter 4. Filtering Methods

−10 0 10 20 30 40−20

−15

−10

−5

0

5

10

15

20

25

30

Actual TrajectoryError EllipsoidEstimated Trajectory

Figure 4.3: Uncertainty ellipsoid graph.

n×n covariance matrix P0. The objective is to estimate xk to satisfy a specified performancecriterion

E{[x − x]T [x − x]}. (4.25)

4.2.1 Extended Kalman Filter

The Extended Kalman Filter (EKF) is the natural extension of the linear Kalman Filterto nonlinear state estimation problems. In this case the model equations are nonlineardifferential equations, the non-linear state function (4.23) is used to compute the predictedstate from the previous estimate and the non-linear measurement function (4.24) is usedto compute the predicted measurement from the predicted state. In order to use systemequations with the estimate error covariance, it is necessary to calculate the Jacobians ofthe matrices. At each time step the Jacobians are evaluated on current estimated state.Therefore to estimate a process with non-linear difference and measurement relationships, itis necessary linearize the system equation on (4.26) and (4.27),

xk ≈ xk + A(xk−1 − xk−1) + Wνk−1, (4.26)

zk ≈ zk + C(xk − xk) + V ωk, (4.27)

where

• xk and zk are the actual state and measurement vectors;

• xk and zk are the approximate state and measurement vectors from (4.26) and (4.27);

41

Chapter 4. Filtering Methods

• xk is an a posteriori estimate of the state at step k;

• the random variables νk and ωk represent the process and measurement noise, respec-tively;

• A is the Jacobian matrix of partial derivatives of f with respect to x, that is

Ak =∂f

∂x

∣∣∣x=x(k−1),ω(k−1)=0

; (4.28)

• W is the Jacobian matrix of partial derivatives of f with respect to ν, that is

Wk =∂f

∂ω

∣∣∣x=x(k−1),ω(k−1)=0

; (4.29)

• C is the Jacobian matrix of partial derivatives of h with respect to x, that is

Vk =∂h

∂x

∣∣∣x=xk,νk=0

; (4.30)

• V is the Jacobian matrix of partial derivatives of h with respect to ω, that is

Vk =∂h

∂ν

∣∣∣x=xk,νk=0

. (4.31)

Note that for simplicity in the notation the matrix indexes was neglected, it useful rememberthat each element represent partial derivative of the ith nonlinear function with respect thejth element of vector variable.The complete set of EKF equations is shown below, note that xk(−) for xk is substituted toindicate the a priori state estimate.

xk(−) = f(xk−1, 0) (4.32)

Pk(−) = AkPk−1ATk + WkQk−1W

Tk (4.33)

The previous equations represent the EKF time update, the ones below represent the EKFmeasurement update

Kk = Pk(−)CTk (CkPk(−)CT

k + VkRkVTk )−1 (4.34)

xk = xk(−) + Kk(zk − h(xk(−), 0)) (4.35)

Pk = (I − KkCk)Pk(−) (4.36)

The reader can find a extensive matematichal derivation of EKF equations on [5], [6], [11],[12], [24], [35]. The stability of EKF is sitll an open issue, some papers give a proof ofstability under certain assumption as [19], [20], [21].

42

Chapter 4. Filtering Methods

4.2.2 EKF Design

As the reader knows from previous paragraphs, the GIB system, subject of this thesis , iscomposed by four buoys equipped with GPS receivers, a free pinger deployed on underwatervehicle and a main surface control station. The features of the G.I.B. system are synchronizedcommunications between buoys and the pinger, depth information codified as a delay betweentwo consecutive pluses, and ranges are represent by times of arrival (TOA). Using the GIBfeatures and assuming that the AUV depth is available, the estimator can be based on afour states nonlinear system where two states describe the planar position and two statesdescribe the velocity components along the x-axis and y-axis. It is important to stress thatthe measured ranges are express with respect a 3D frameset, thus in order to simplify theconversion from 3D to 2D data, squared distances are considered. Next sections describe thenonlinear system and the Jacobians.

Measured range

Planar range

Depth

Buoy hydrophone

AUV

Figure 4.4: Ranges components.

Nonlinear System

This section describe the AUV state equations and the buoy measurements equations. Thenonlinear system equations are

pxk+1= pxk

+ pxk+ ν1k

pyk+1= pyk

+ pyk+ ν2k

pxk+1= pxk

+ ν3k

pyk+1= pyk

+ ν4k

(4.37)

and the nonlinear measurement equations are

r1k = (pxk− pBx1)

2 + (pyk− pBy1)

2 + ω1k

r2k = (pxk− pBx2)

2 + (pyk− pBy2)

2 + ω2k

r3k = (pxk− pBx3)

2 + (pyk− pBy3)

2 + ω3k

r4k = (pxk− pBx4)

2 + (pyk− pBy4)

2 + ω4k

(4.38)

where

43

Chapter 4. Filtering Methods

• pxk, pyk

, pxk, pyk

represent x and y coordinates and x-axis and y-axis velocity compo-nents;

• νnk, {n = 1, 2, 3, 4} represent the plant noise;

• (pBxi, pByi

), {i = 1, 2, 3, 4} buoy planar coordinates;

• ωmk, {m = 1, 2, 3, 4} represent the measurement noise.

The AUV orientation can be determine solving the following algebraic relation

tan θ =pyk

pxk

(4.39)

Jacobians

The “state” Jacobian matrices of the system are evaluate for each timestamp on x =xk−1, ν = 0, the parametric Jabocians are shown below

Ak[i, j] =∂f [i]

∂x[j]=

1 0 1 00 1 0 10 0 1 00 0 0 1

(4.40)

Wk[i, j] =∂f [i]

∂ν[j]=

1 0 0 00 1 0 00 0 1 00 0 0 1

(4.41)

The “measurement” Jacobian matrices of the system are evaluate for each timestamp onx = xk(−), ω = 0, the parametric Jabocians are shown below

Ck[i, j] =∂h[i]

∂x[j]=

2(pxk− pBx1) 2(pyk

− pBy1) 0 02(pxk

− pBx2) 2(pyk− pBy2) 0 0

2(pxk− pBx3) 2(pyk

− pBy3) 0 02(pxk

− pBx4) 2(pyk− pBy4) 0 0

(4.42)

Vk[i, j] =∂h[i]

∂ω[j]=

1 0 0 00 1 0 00 0 1 00 0 0 1

(4.43)

Simulation Results

This paragraph describes the results of simulations aimed at assessing the efficacy of thealgorithms derived. In the simulations, four buoys were placed at the corners of a squarewith a 500m side. The depth of the hydrophones was set to 10m for all the buoys. The targetwas assumed to move at 4 m/s speed along segments of straight lines and circumferences

44

Chapter 4. Filtering Methods

Table 4.2: EKF: Simulation parameters

x0 [180 320 − 1 − 1.73]T

x0 [160 300 − 0.5 − 2.23]T

P0 diag{[(25)2 (25)2 (0.5)2 (0.5)2]}Q diag{[(4e − 3)2 (4e − 3)2 (1e − 2)2 (1e − 2)2]}R diag{[25 25 25 25]}

with a 76m of diameter; see Figure 4.5. The range measurements were generated everyT = 1s and corrupted by a Gaussian measurement noise with a 0.1 m standard deviation,also the EKF was run at the same sampling period. The filter parameters are shown inTable 4.2.2. Figure 4.5 shows a simulation of actual and estimated target trajectories. Inthe figure, EKF stands for the data obtained with the filtering algorithm, Buoy stands forthe positions of the buoys given by GPS receivers installed on the buoys, and AUV standsfor the actual target trajectory. Figure 4.6 shows the error state evolutions, the actualand estimated state evolutions. In the figure, EKF stands for the data obtained with thefiltering algorithm, error stands for the estimation state error, and AUV stands for the actualtarget state evolutions. At this point, it is also important to recall that all estimates of thetarget motion are computed using acoustic range measurements only. In spite of this, theperformance of the filter is quite good.

0 100 200 300 400 500

0

100

200

300

400

500

North

Eas

t

Buoys

AUV

EKF

Figure 4.5: EKF simulation results.

45

Chapter 4. Filtering Methods

0 100 200 300 400 500 600−100

0

100

200

300

400

500

Time [s]

Nor

th E

stim

atio

n E

rror

[m]

error

EKF

AUV

0 100 200 300 400 500 600−100

0

100

200

300

400

500

Time [s]

Eas

t Est

imat

ion

Err

or [m

]

errorEKFAUV

0 100 200 300 400 500 600−2

−1

0

1

2

3

Time [s]

Nor

th V

eloc

ity E

stim

atio

n E

rror

[m/s

]

error

EKF

AUV

0 100 200 300 400 500 600−3

−2

−1

0

1

2

3

Time [s]

Eas

t Vel

ocity

Est

imat

ion

Err

or [m

/s]

errorEKFAUV

Figure 4.6: State error evolutions.

46

Chapter 5

Initialization and Outlier rejectionalgorithms

In this section two factor that afflict the EKF algorithm are discussed. The first concerns thechoice of the initial point, the second concerns an on-line measurement validation. EKF isnot a optimal estimator, thus with an inaccurate choice of initial point the filter may quicklydiverge. On-line validation is also a critical aspect, because the estimation process of thefilter depends on the past measurements, thus an outlier (completely wrong measurement)introduces significant errors in the estimation of current and future fixes. In order to run theEKF algorithm in real-world applications, the design precess has to consider also these twoaspects. A spatial-domain technique is used to solve the initial position issue, instead formeasurement validation a time-domain technique is presented. This approach is discussedon [2] and [30].

5.1 Initialization algorithm

Determaine a initial fix is not a trivial issue, because the presence of outliers could generatecompletely wrong estimates. For this reason, the initialization process cannot base theestimate only on current time; In practice, it is possible to wait for several GIB cycles beforeable to determine the initial fix. Moreover, the window of time considered on fix estimationhas to be enough informative, thus at least three or more measurements are needed. GIBarchitecture gives three measurements from each buoys for timestamp. On this discussionthe data received from each buoys may represent a possible combination of the followingthree cases:

τi = di + ωi

τi = di + k∆z + ωi

τi = ωi

Obviously, if the range and depth information are available from one buoy in an emissioncycle, the range TOA is received before the depth TOA. Assuming that at each timestamp,all the time of arrivals (GIB measurements) are less than 1000ms and at least three buoys are

47

Chapter 5. Initialization and Outlier rejection algorithms

working correctly, an algorithm based on least square estimation residual can be introduced.The algorithm works on windows of time, where the size is a parameter select from theuser before system deployment. First, the algorithm store measurement, until the windowsize is reached, than start to test all possible combination of three measurements for eachtimestamp in the window. In a timestamp one or four estimate could be available, if threeor four buoys are working respectively, other combination are wrong. Each instance oftrilateration produce a fix, the quality of estimates is determine by a test on residuals.Lets define

l(r) =1

(2π)m2 R

12

exp{−1

2(r − d)T R−1(r − d)} (5.1)

where r measured ranges, d estimated ranges, and

R = E{ωωT}. (5.2)

For each timestamp is selected the fix with highest probability that satisfy the followinginequality

(r − d)T R−1(r − d) < α1 (5.3)

where α1 is a value chose in accord with χ2 distribution (see appendix B).After this step are available N or less candidate fixes, where N is the size of the window. Theselected candidate could not represent only combination of real ranges but also combinationof real ranges and outliers, in order to reject wrong fixes a χ2 test is done again. In this caseis considered the trilateration error covariance and a term that consider the AUV motioncapabilities is added.Lets define

l(pi) =1

(2π)12 |R|

exp{−1

2(pi − pj)

T Σ−1(pi − pj)} (5.4)

where pi and pj represent the estimated fixed on timestamp i and j, h represent the sampletime, and

Σ = (AT MT S−1MA)−T +V 2

MAXh2(i − j)2

9I2 (5.5)

where the first term comes from (3.28) and S = 4MDRDMT . The second term is derivedfrom the assumption that the AUV probability of moving with a speed less than VMAX isequal to 3σ.

3σ = VMAX (5.6)

σ =VMAX

3(5.7)

σ2 =V 2

MAX

9(5.8)

48

Chapter 5. Initialization and Outlier rejection algorithms

The factor h2(i − j)2 in the second term of (5.5) takes in account the elapsed time betweenthe two timestamps considered. Two estimated fixes are consistent if the following χ2 testis verified

(pi − pj)T Σ−1(pi − pj) < α2 (5.9)

The value of α2, as α1, is chosen in accord with the tabled values on appendix B. The goalof this procedure is estimate the direction of motion of AUV and determine a reliable initialpoint for the tracking algorithm. The solution presented is a dedicated procedure for GIBsystem. Other dedicated solution for different architecture are discussed on [17] and [33].

5.2 Outlier Rejection

In the previous section the initialization issue is addressed, but another aspect have to beconsider when the EKF algorithm runs. The EKF estimate depends on previous timestamps,thus if a fix estimation is afflicted by an outlier (completely wrong measurement), an erroron current estimate and future estimates occurs. In order to solve this problem an on-linevalidation procedure is introduced. In view of the variety of variables that can be measured,a generic validation procedure for discrete-valued measurements can be introduced from theEKF theory, which is based on the measurement prediction covariance matrix.Lets define the measurement prediction covariance matrix ([5])

Sk+1 , E{rk+1rTk+1}

= Ck+1Pk+1(−)CTk+1 + 4DRDT (5.10)

where R is the measurement noise covariance matrix defined in (5.2), Pk+1(−) is the a prioriestimate error covariance matrix defined in (4.33), Ck+1 is the Jacobian defined in (4.42),and

rk+1 = rk+1 − rk+1 (5.11)

is the innovation.

Assuming that the true measurement at time k + 1 is normally distributed, one may definea region in the measurement space where the measurement will be found with some highprobability

Vk+1(α3) , {rTk+1S

−1

k+1rk+1 6 α3} (5.12)

where α3 is the threshold chose in accord with the χ2 distribution (see appendix B). Theregion defined by (5.12) is called the validation region. It is the ellipse of probability con-centration, measurements that lie inside the region are considered valid, those outside arediscarded.

49

Chapter 5. Initialization and Outlier rejection algorithms

Consistent

founded?initial fix

Spatial−Domaininitialization

predictionlimit of

Exceeded

Start

algorithmStarting up EKF

EKF prediction

Outlier rejection

EKF update

validatedmeasurements

>1

YES

YES

NO

Only prediction

NO

YES

Collect data intoN timestap window

Figure 5.1: Integration of EKF, initialization and outlier rejection procedures.

50

Chapter 6

Graphical User Interface

This chapter introduces a logical description of a Matlab graphics user interface (GUI)studied to setup and control the tracking process. This GUI is capable of proceding andconverting binary data packets, which represent the GIB packets, in a GIBdata structure.The binary packets contain the TOA collected at each of the hydrophones and the buoypositions given by the respective GPS receivers, and other information that describes thesystem status. A typical G.I.B. cycle has a rate of one second and this characteristic justifiesthe use of a Matlab-based software with a conventional PC.

In Figure 6.1 a logical description of the graphical user interface is given. The softwarecan operate with G.I.B. log file or directly connected on a serial port. First operative modeis known as Replay Mode, and the second one is called Real-Time Mode. The main blocksare Read Binary Data, and Tracking Algorithm. The tracking block is already described inFigure 5.1, the read block is described in Figure 6.2.

First block, in Figure 6.2, depicts a synchronization on packet header, when a packetheader is detected the reading procedure starts. The Read Bytes block is a conventionalbinary reading procedure. The main block is represented by the conversion block fromlongitude/latitude/altitude (LLA) coordinates to North East Down (NED) coordinates, adetailed description of the coordinate transformation procedure adopted in this thesis isgiven in paragraph C.

51

Chapter 6. Graphical User Interface

algorithmresults

Store

Serial PortBinary Data File

Selector

Read Binary Data

structuredata to GIBData

Conversion Binary

memoryStore in GUI data

Tracking Algorithm

Display Data

BuoyPosition Trajectory

AUVinformation

System

Figure 6.1: GUI logical scheme.

52

Chapter 6. Graphical User Interface

lon/lat nedBuoy Position:

Start

ready to storeGIBData

Waiting forGIB Header

Read Bytes

Checksummatch?

Yes

No

End

Figure 6.2: Read Binary Data block.

53

Chapter 7

Experimental Results

This chapter describes the analysis and results for the tracking algorithm using real-worlddata form a sea test.

7.1 Experimental scenario

Experimental data were acquired in Sines, Portugal, during a campaign of the MEDIRESproject at the Sines West breakwater. The system used to collect data is the commercialACSA/GIB described in paragraph 2.4. The four buoys are deployed in a square config-uration with approximately 500m on the side, each buoy has a submerged hydrophone atnominal depth of 10m. Since no information about hydrophones and GPS-receivers errorcharacteristics were available, manual tuning on Q, R and P0 matrices was made. TheG.I.B interrogation cycle was set to 1s as the sampling time for the extended Kalman filterintroduced in chapter 4.2.2. The N.E.D. reference system is considered.

Table 7.1 shows the filter parameters that were used in the mechanization of the posi-tioning algorithm described in chapter 4.2.2.

7.2 Results analysis

The estimated experimental trajectory of the underwater pinger is depicted in Figure 7.1.In the figure, EKF stands for the data obtained with the filtering algorithm, Buoy standsfor the positions of the buoys given by GPS receivers installed on the buoys, WLSE standsfor the data obtained with the weighted trilateration method, and MLE stands for the dataobtained with the MLE mechanization based on pure ranges likelihood function and the

Table 7.1: Experimental filter parameters

P0 diag{[(25)2 (25)2 (1)2 (1)2]}Q diag{[(2.5e − 10)2 (2.5e − 10)2 (0.4)2 (0.4)2]}R diag{[(9)2 (9)2 (9)2 (9)2]}

54

Chapter 7. Experimental Results

Newton method. The pinger trajectory was completed at a nominal depth of 2 meters withrespect to the seasurface level.

The Extended Kalman filter designed yields far better performance than the trilaterationsolutions proposed. The figure does not give total justice to this fact, because it does notreflect the fact that the trilateration fixes are often not available due to bad quality of thedata. In fact, only good trilateration data were considered.

In Figure 7.2 are shown the collected and validated measurement that are obtainedduring the experiment at sea. The zero value of the TOA indicates that no data werecollected. The buoys started transmitting data approximately after second 200, with theexception of buoy 2 that did not transmit data until the end of the experiment. This kind ofevents are frequent in real applications, thus if the positioning system is not enough robustdramatic failures can happen.

In Figure 7.2 the vertical scale is in milliseconds to emphasize that the buoys providetheir distance to the pinger codified as the time-delay between the emission time and thereception time of the first pulse. In the figure, the measurements are concentrated on twoparallel curves that represent the reception of the two consecutive pulses. However, thefigure shows other detections that cannot be attributed to the acoustic positioning system.A possible explanation of these detections is the reflection of the signal (multipath) or noisesdue to presence of other kind of underwater communications.

The result of the on-line measurement validation is represented in Figure 7.2 as cyancircles around the validated detects. The black circles represent the result of the initializationalgorithm (see paragraph 5.1).

In Figure 7.3 a detailed view of the TOA for all the buoys suggests that multipath aredetected, i.e. for buoy 4, approximately on second 750, two curves are available. Moreover,buoy 1 presents several outliers that show a high level of noise compared with the otherbuoys. An accurate analysis shows that also in buoy 1 and buoy 3, multipath are detected.

In Figures 7.5, 7.7, 7.8 are depicted the experimental results obtained with a further set ofdata. The experimental environment is the same descripted for the previous set of data. Theestimated experimental trajectory is represented in Figure 7.5, and it consists in a straightline. Notice that the estimate trajectory is not continue line because on the segment of timebetween the second 300 and 340 no detects were available. In this situation the algorithmcontinues to estimate the vehicle position (pure prediction) for certain period of time, overthis period the algorithm stops to run and starts again the initialization procedure.

In Figure 7.6 a zoomed view of the period of time between the second 300 and 340 isgiven. In the figure it is possible notice that the initialization procedure recovers the dataflow and provides a new starting point for the positioning algorithm. Notice that the vehiclevelocity components, in the initial point, are assumed equivalent and equal to 2m/s.

55

Chap

ter7.

Experim

ental

Resu

lts

2.06 2.07 2.08 2.09 2.1 2.11 2.12 2.13

x 104

−8.8

−8.79

−8.78

−8.77

−8.76

−8.75

−8.74

−8.73

−8.72x 10

4

North [m]

Eas

t [m

]

Buoy 1

Buoy 2

Buoy 3

Buoy 4

WLSE

MLE

EKF

Figu

re7.1:

Experim

ental

trajectory.

56

Chap

ter7.

Experim

ental

Resu

lts

0 200 400 600 800 1000 1200 14000

100

200

300

400

500

600

700

800Buoy 1

Time [s]

TO

As

[ms]

0 200 400 600 800 1000 1200 14000

100

200

300

400

500Buoy 2

Time [s]

TO

As

[ms]

0 200 400 600 800 1000 1200 14000

100

200

300

400

500

600

700

800Buoy 3

Time [s]

TO

As

[ms]

0 200 400 600 800 1000 1200 14000

200

400

600

800

1000Buoy 4

Time [s]

TO

As

[ms]

Figu

re7.2:

Tim

esof

Arrival

ofacou

sticpulses

ateach

buoy.

57

Chap

ter7.

Experim

ental

Resu

lts

600 700 800 900 1000 1100 1200200

250

300

350

400

Buoy 1

Time [s]

TO

As

[ms]

1150 1200 1250 1300 1350

0

20

40

60

80

100

120

140

160

Buoy 2

Time [s]

TO

As

[ms]

600 700 800 900 1000 1100 1200

250

300

350

400

450

Buoy 3

Time [s]

TO

As

[ms]

650 700 750 800 850 900 950 1000 1050 1100

100

150

200

250

300

350

400

450

500

550Buoy 4

Time [s]

TO

As

[ms]

Figu

re7.3:

Details

ofT

OA

atbuoy

s.

58

Chap

ter7.

Experim

ental

Resu

lts

0 200 400 600 800 1000 1200 14002.05

2.1

2.15x 10

4

Time [s]

Nor

th [m

]

0 200 400 600 800 1000 1200 1400−8.8

−8.75

−8.7x 10

4

Time [s]

Eas

t [m

]

0 200 400 600 800 1000 1200 1400−5

0

5

Time [s]

VN

orth

[m/s

]

0 200 400 600 800 1000 1200 1400−5

0

5

Time [s]

VE

ast [m

/s]

Figu

re7.4:

State

vectorevolu

tion.

59

Chap

ter7.

Experim

ental

Resu

lts

2.36 2.38 2.4 2.42 2.44 2.46 2.48

x 104

−9.05

−9.04

−9.03

−9.02

−9.01

−9

−8.99

−8.98

−8.97

−8.96x 10

4

North [m]

Eas

t [m

]

Buoy 1Buoy 2Buoy 3Buoy 4WLSEMLEEKF

Figu

re7.5:

Experim

ental

trajectory

ofsecon

dset

ofdata.

60

Chap

ter7.

Experim

ental

Resu

lts

200 250 300 350 400400

410

420

430

440

450

460

470

480

Buoy 1

Time [s]

TO

As

[ms]

200 250 300 350 400

300

320

340

360

380

400

420

Buoy 2

Time [s]

TO

As

[ms]

180 200 220 240 260 280 300 320 340 360 380

200

220

240

260

280

300

320

340

360

Buoy 3

Time [s]

TO

As

[ms]

150 200 250 300 350

210

220

230

240

250

260

270

280

290

Buoy 4

Time [s]

TO

As

[ms]

Figu

re7.6:

Detailed

ofT

OA

validation

inth

etim

eran

ge[300

−340].

61

Chap

ter7.

Experim

ental

Resu

lts

0 50 100 150 200 250 300 350 400 450 5000

100

200

300

400

500

600

700

800Buoy 1

Time [s]

TO

As

[ms]

0 50 100 150 200 250 300 350 400 450 5000

100

200

300

400

500

600

700

800Buoy 2

Time [s]

TO

As

[ms]

0 50 100 150 200 250 300 350 400 450 5000

100

200

300

400

500

600

700Buoy 3

Time [s]

TO

As

[ms]

0 50 100 150 200 250 300 350 400 450 5000

100

200

300

400

500

600Buoy 4

Time [s]

TO

As

[ms]

Figu

re7.7:

Tim

esof

Arrival

ofacou

sticpulses

ateach

buoy

(second

setof

data).

62

Chap

ter7.

Experim

ental

Resu

lts

0 50 100 150 200 250 300 350 400 450 5002.4

2.42

2.44x 10

4

Time [s]

Nor

th [m

]

0 50 100 150 200 250 300 350 400 450 500−9.015

−9.01

−9.005x 10

4

Time [s]

Eas

t [m

]

0 50 100 150 200 250 300 350 400 450 5000

0.5

1

Time [s]

VN

orth

[m/s

]

0 50 100 150 200 250 300 350 400 450 500−0.5

0

0.5

Time [s]

VE

ast [m

/s]

Figu

re7.8:

State

vectorevolu

tion(secon

dset

ofdata)

63

Chapter 8

Conclusions and future work

This thesis proposes several solutions to the problem of estimating the position of an un-derwater vehicle. The commercial system adopted for collecting data consists of four buoysthat compute the times of arriaval (TOA) of the acoustic signals emitted periodically by apinger installed on-board the target (so colled G.I.B. system).

8.1 Conclusions

Concerning the Trilateration problem, the accuracy analysis shows that the LSE provideestimates with less estimation error when compared with MLE (Figure 3.11). The precisionof an LSE can be improved by taking into account the measurement noise variance providinga WLSE. The MLE is found to be efficient because it reaches the minimum achievable vari-ance given by the Cramer-Rao Lower Bound (CRLB). However, the computational effort ishigher then the LSE case. The solutions based on squared ranges likelihood function requiresan accurate setup of the step size procedure and a stop criterion, because the cost functiondecreases faster than in the case of the pure ranges likelihood function. Both mechaniza-tions show that the Newton method converges to the optimal solution with less numberof iterations when compared with the Gradient method. Hence, the MLE mechanizationbased on pure ranges likelihood function and the Newton method are suitable for real-timeimplementation.

The buoy geometry analysis obtained with the CRLB shows that in a square configurationa good level of accuracy is yield. In the case of triangle shape in proximity of the buoys theaccuracy decreases quickly.

Concerning filtering algorithms, the experimental results show that the proposed Ex-tended Kalman Filter yields good results in presence of outliers or reduced number of vali-dated measurements. The main advantage of this mechanization is that it works with lessthan three valid measurements are available, thus provides a estimate when the Trilatera-tion methods can not operate. About the on-line validation measurements procedure, it isimportant notice that detects some multipath as ranges, this is due to lack of a multipathpropagation model that allow to differentiate among direct ranges and multipath.

64

Chapter 8. Conclusions and future work

8.2 Future work

Future work could address the inclusion of a multipath propagation model in the estimationprocedure improving the accuracy of the positioning system. Another interesting topic ofresearch is how to close the feedback to the AUV, providing the estimated fix by an acousticcommunication channel.

65

Bibliography

[1] Alex Alcocer, Paulo Oliveira, and Antonio Pascoal. Underwater acoustic positioningsystems based on buoys with gps. 8th ECUA, 2006.

[2] Alex Alcocer, Paulo Oliveira, and Antonio Pascoal. Study and implementation of anekf gib-based underwater positioning system. IFAC Journal of Control EngineeringPractice, 15(6):689–701, 2007.

[3] Alex Alcocer, Paulo Oliveira, Antonio Pascoal, and J. Xavier. Estimation of attitudeand position from range only measurements using geometric descent optimization onthe special euclidean group. 9 th International Conference on Information Fusion, July2006.

[4] Alex Alcocer, Paulo Oliveira, Antonio Pascoal, and J. Xavier. Maximum likelihood atti-tude and position estimation from pseudo-range measurements using geometric descentoptimization. 45 IEEE Conference on Decision and Control, December 2006.

[5] Yaakov Bar-Shalom and Thomas E. Fortmann. Tracking and Data Association.Acadamic Press, inc., 1998.

[6] Yaakov Bar-Shalom, X. Rong-Li, and Thiagalingam Kirubarajan. Estimation with Ap-plications to Tracking and Navigation: Theory Algorithms and Software. Jhon Wiley &Sons, Inc., 2001.

[7] Dimitri P. Bertsekas. Nonlinear Programming. Athena Scientific, 2nd edition, 1999.

[8] Andrea Caiti, Andrea Garulli, Flavio Livide, and Domenico Prattichizzo. Localization ofautonomous underwater vehicles by floating acoustic buoys: A set-membership approch.IEEE Journal of Oceanic Engineering, 30(1), January 2005.

[9] M. Deffenbaugh, J.G. Bellingham, and H. Schmidt. The relationship between spher-ical and hyperbolic positioning. In OCEANS ’96. MTS/IEEE. Prospects for the 21stCentury, 1996.

[10] Thor I. Fossen. Marine Control Systems: Guidance, Navigation, and Contol of Ships,Rigs and underwater Vehicles. Tapir Trykkeri, 2002.

[11] Arthur Gelb. Applied Optimal Estimation. The M.I.T. Press, 2001.

66

BIBLIOGRAPHY

[12] Mohinder S. Grewal and Angus P. Andrews. Kalman Filtering: Theory and PracticeUsing Matlab. Jhon Wiley & Sons, Inc., 2001.

[13] Mohinder S. Grewal, Lawrence R. Weill, and Angus P. Andrews. Global PositioningSystems, Inertial Navigation, and Integration. Jhon Wiley & Sons, Inc., 2007.

[14] S. M. Kay. Fundamentals of Statistical Signal Processing, Volume I: Estimation Theory.Prentice Hall PTR, 1998.

[15] David G. Luenberger. Introduction to Linear and Nonlinear Programming. Addison-Wesley Pub. Co., 1973.

[16] Marco Morgado, Paulo Oliveira, Carlo Silvestre, and J.F. Vasconcelos. Usbl/ins inte-gration technique for underwater vehicles, 2006.

[17] Edwin Olson, John Leonard, and Seth Teller. Robust range-only beacon localization.In Proceedings of Autonomous Underwater Vehicles, 2004.

[18] Leslaw R. Paradowski. Uncertainty ellipses and their application to interval estimationof emitter position. IEEE Transactions on Areospace And Electronic Systems, 33(1),January 1997.

[19] K. Reif, S. Gunther, E. Yaz, and R. Unbehauen. Stochastic stability of the discrete-timeextended kalman filter. IEEE Transactions on Automatic Control, 44(4), April 1999.

[20] K. Reif, F. Sonnemann, and R. Unbehauen. Modification of the extended kalman filterwith an additive term of instability. In Proceedings of the 35th Decision and Control,December 1996.

[21] K. Reif, F. Sonnemann, and R. Unbehauen. An ekf-based nonlinear observer with aprescribed degree of stability. Automatica, 34(9):1119–1123, September 1998.

[22] Robert M. Rogers. Applied Matehematics In Integrated navigation Systems. AIAAEducation Series. AIAA, 2000.

[23] Julius O. Smith and Jonathan S. Abel. The spherical interpolation method of sourcelocalization. IEEE Journal of Oceanic Engineering, 12(1), January 1987.

[24] Harold W. Sorenson. Kalman Filtering: Theory and Application. IEEE, 1985.

[25] Gilbert Strang and Kai Borre. Linear Algebra, Geodesy, and GPS. Wellesley-CambridgePress, 1997.

[26] H. G. Thomas. New advance underwater navigation techniques based on surface relaybuoys. IEEE, 1994.

[27] Hubert Thomas and Eric Petit. From autonomous underwater vehicles (auv’s) to su-pervised underwater vehicles (suv’s). IEEE Oceans’97 MTS, 1997.

67

BIBLIOGRAPHY

[28] Harry L. Van Trees. Detection, Estimation, and Modulation Theory Part I. Detection,Estimation, and Linear Modulation Theory. Jhon Wiley & Sons, Inc., 2001.

[29] Robert J. Urick. Principles Of Underwater Sound. Peninsula, 1996.

[30] J. Vaganay, J.J. Leonard, and J.G. Bellingham. Outlier rejection for autonomous acous-tic navigation. In Proceedings of International Conference on Robotics and Automation,April 1996.

[31] Keith Vickery. Acoustic positioning systems, a pratical overview of current systems.IEEE Autonomous Underwater Vehicles, 1998.

[32] Keith Vickery. Acoustic positioning systems, new concepts - the future. IEEE Au-tonomous Underwater Vehicles, 1998.

[33] S. Vike and J. Jouffroy. Diffusion-based outlier rejection for underwater navigation.IEEE Oceans’05 MTS, 2005.

[34] M.P. Wand. Vector differential calculus in statistics. The American Statistician, 56(1),February 2002.

[35] Greg Welch and Gary Bishop. An introduction to the kalman filter, July 2006.

68

Appendix A

MLE: Gradient and Hessianderivation

In accord with [34], if F : Rm → R and its first differential can be written as

δF (x) = gTdx (A.1)

where g ∈ Rm, then

∇F (x) = g (A.2)

and its second differential is

δ2F (x) = (δx)T Aδx (A.3)

where

∇2F (x) = A (A.4)

A.1 MLE: Squared ranges

Let define

di = ‖p − pBi‖2 ∈ R, i ∈ {1, ..., m} (A.5)

ri = di + wi (A.6)

be the squared ranges of actual distance between the AUV and buoys, and pB and p aredefined in section 3.1.1. The MLE cost function F has the following form

F (p) =1

2(r − d)T R−1(r − d) (A.7)

69

Appendix A. MLE: Gradient and Hessian derivation

where

d ,[d1, d2, . . . , dm]T ∈ Rm

r ,[r1, r2, . . . , rm]T ∈ Rm

w ,[w1, w2, . . . , wm]T ∈ Rm

where R is the error covariance matrix define as

R ,E{wwT} ∈ Rm×m

The first differential can be obtained as follows

δF = −1

2δdTR−1(r − d) −

1

2(r − d)T R−1δd (A.8)

= −(r − d)T R−1δd (A.9)

whereδd = [δd1, δd2, . . . , δdm]T ∈ R

m (A.10)

The first differential of d can be written

δdi = δ‖p − pBi‖2 (A.11)

= δ((p − pBi)T (p − pBi

)) (A.12)

= 2(p − pBi)T δp (A.13)

rewriting (A.13) in matricial form

δd =

δd1

...δdm

=

2(p − pB1)T

...2(p − pBm

)T

︸ ︷︷ ︸

C

δp (A.14)

concluding the gradient is given by

∇F (p) = −((r − d)TR−1C)T (A.15)

Lets introduce

α ,R−1(r − d) = [α1, α2, . . . , αm]T ∈ Rm

then the second differential can be calculate as follows

δF = −(r − d)T R−1δd = −αT δd

= −

m∑

i=1

αTi δdi (A.16)

δ2F = −m∑

i=1

δαiδdi −m∑

i=1

αiδ2di

= −δαT δd −

m∑

i=1

αiδ2di (A.17)

70

Appendix A. MLE: Gradient and Hessian derivation

remembering that

δdi = 2(p − pBi)T δp ∈ R

1×n

δ2d can be written as follows

δ2di = 2δpTδp (A.18)

Introducing (A.18) in (A.17) the finalexpression of second differential is obtained

δ2F = δdTR−1δd − 2αT 1mδpT δp

= δpT 4CCTδp − 2αT1mδpT δp (A.19)

concluding the hessian is given by

∇2F (p) = 4CR−1CT − 2αT1mIn

= 4(p1m − pB)T R−1(p1m − pB) − 2(r − d)R−11mIn (A.20)

A.2 MLE: Pure ranges

Lets define

di = ‖p − pBi‖ ∈ R, i ∈ {1, ..., m} (A.21)

ri = di + wi (A.22)

the MLE cost function have the expression seen in (3.32)

F (p) =1

2(r − d)T R−1(r − d) (A.23)

where

d ,[d1, d2, . . . , dm]T ∈ Rm

r ,[r1, r2, . . . , rm]T ∈ Rm

w ,[w1, w2, . . . , wm]T ∈ Rm

where R is the error covariance matrix define as

R ,E{wwT} ∈ Rm×m

The first differential of d can be written

δdi = δ‖p − pBi‖ (A.24)

=1

2di((p − pBi

)T (p − pBi))δp (A.25)

=1

di(p − pBi

)T δp (A.26)

71

Appendix A. MLE: Gradient and Hessian derivation

rewriting (A.26) in matricial form

δd =

δd1

...δdm

=

1

d1. . . 0

.... . .

...0 . . . 1

dm

︸ ︷︷ ︸

D−1

×

(p − pB1)T

...(p − pBm

)T

︸ ︷︷ ︸

C

δp (A.27)

concluding the gradient is given by

∇F (p) = −((r − d)TR−1D−1C)T (A.28)

In order to determine the second differential of F (x) lets define the follwing quantity

α ,R−1(r − d) = [α1, α2, . . . , αm]T ∈ Rm

δF = −αT δd (A.29)

= −

m∑

i=1

αiδdi

δ2F = −m∑

i=1

δαiδdi −m∑

i=1

αiδ2di

= (δd)TR−1δd −

m∑

i=1

αiδ2di (A.30)

Lets define

β , [β1, β2, . . . , βm]T ∈ Rm

where

βi = (p − pBi)T δp

then it is possible rewrite (A.26) as

δdi =1

diβi (A.31)

consequently

δ2di =1

diδβi −

1

d2i

βiδdi

=1

di

δβi −1

d3i

β2

i (A.32)

72

Appendix A. MLE: Gradient and Hessian derivation

determining δβi and β2i

δβi = δ(p − pBi)T δp

= (δp)T δp (A.33)

β2

i = ((p − pBi)T δp)2

= (δp)T (p − pBi)(p − pBi

)T δp (A.34)

then (A.32) can be rewritten

δ2di =1

di(δp)Tδp −

1

d3i

(δp)T (p − pBi)(p − pBi

)T δp

= (δp)T [1

di

In −1

d3i

(p − pBi)(p − pBi

)T ]δp

= (δp)T Hiδp (A.35)

where

Hi =1

di

In −1

d3i

(p − pBi)(p − pBi

)T (A.36)

now the second differential can be summarize as follows

δ2F = (δd)T R−1δd −m∑

i=1

αδ2d

= (δp)T{[CT D−1R−1D−1C] −

m∑

i=1

αiHi}δp (A.37)

then in accord (A.3) the hessian resulting is

∇2F (p) = {[CT D−1R−1D−1C] −

m∑

i=1

αiHi} (A.38)

73

Appendix B

Chi-Square distribution

In this appendix the χ2 distribution is introduced, for more details see [6]. Table B presentsthe points x on the chi-square distribution for a given upper tail probability

Q = P{y > x} (B.1)

where

y ∽ χ2 (B.2)

and n is the number of degrees of freedom (df). This tabulated function is also known asthe complementary distribution. An alternative way of writing (B.1) is

x(1 − Q) , χ2(1 − Q) (B.3)

which indicates that to the left of the point x the probability mass is 1 − Q.

Figure B.1: χ2 distribution.

74

Appendix B. Chi-Square distribution

n \ Q 0.99 0.975 0.95 0.90 0.75 0.5 0.25 0.1 0.05 0.025 0.01

1 2e-4 .001 .003 .016 .102 .455 1.32 2.71 3.84 5.02 6.632 .020 .051 .103 .211 .575 1.39 2.77 4.61 5.99 7.38 9.213 .115 .216 .352 .584 1.21 2.37 4.11 6.25 7.81 9.35 11.34 .297 .484 .711 1.06 1.92 3.36 5.39 7.78 9.49 11.1 13.35 .554 .831 1.15 1.61 2.67 4.35 6.63 9.24 11.1 12.8 15.16 .872 1.24 1.64 2.20 3.35 5.35 7.84 10.6 12.6 14.4 16.87 1.24 1.69 2.17 2.83 4.25 6.35 9.04 12.0 14.1 16.1 18.58 1.65 2.18 2.73 3.49 5.07 7.34 10.2 13.4 15.5 17.5 20.19 2.09 2.70 3.33 4.17 5.90 8.34 11.4 14.7 17.0 19.0 21.710 2.56 3.25 3.94 4.87 6.74 9.34 12.5 16.0 18.3 20.5 23.211 3.05 3.82 4.57 5.58 7.58 10.3 13.7 17.3 19.7 22.0 24.712 3.57 4.40 5.23 6.30 8.44 11.3 14.8 18.5 21.0 23.3 26.213 4.11 5.01 5.90 7.04 9.30 12.3 16.0 19.8 22.4 24.7 27.714 4.66 5.63 6.57 7.79 10.2 13.3 17.1 21.1 23.7 26.1 29.115 5.23 6.26 7.26 8.55 11.0 14.3 18.2 22.3 25.0 27.5 30.616 5.81 6.91 7.96 9.31 11.9 15.3 19.4 23.5 26.3 28.8 32.017 6.41 7.56 8.67 10.1 12.8 16.3 20.5 24.8 27.6 30.2 33.418 7.01 8.23 9.40 10.9 13.7 17.3 21.6 26.0 28.9 31.5 34.819 7.63 8.91 10.1 11.7 14.6 18.3 22.7 27.2 30.1 32.9 36.220 8.26 9.60 10.9 12.4 15.5 19.3 23.8 28.4 31.4 34.2 37.6

Table B.1: Table of critical values of χ2 distribution.

75

Appendix C

Coordinate Transformations

This section describes the process to derive a NED fix starting from a LLA fix. The procedureconsists in a first conversion from LLA to Earth Centered Earth Fixed (ECEF) and successiveconversion from ECEF to NED. The conversion are based on the World Geodetic System(WGS-84). The reader can find a exhaustive discussion about coordinate transformation on[10],[13] ,[22], and [25].

C.1 Earth Model

With reference to [22], there are many models for the Earth’s geometry, however most widelyused current Earth model is the WSG-84. Earth shape model parameters are illustrated inFigure C.1, which shows the elliptical cross section for the ellipsoid. The z axis is along therotation axis, and β is a radial distance in the x − y plane of the equator.

Figure C.1: Earth shape model.

Several geometrical parameters used in definitions for Earth shape models are illustrated

76

Appendix C. Coordinate Transformations

Defining parameters

Equatorial radius re 6378137 mAngular velocity, ωi/e 7.292115−5 rad/sEarth’s gravitational constant, µ 3.986005+14 m3/s2

Second gravitational constant, J2 1.08263−3

Derived constants

Flattering, f 298.2572235637Polar radius, rp 6356752.3142 mFirst eccentricity, ǫ 0.0818191908426Gravity at equator, gWGS0 9.7803267714 m/s2

Gravity formula constant, gWGS1 0.00193185138639Mean value (normal) gravity, g 9.7976446561 m/s2

Table C.1: WGS-84 ellipsoid constants.

in Figure C.1. The following relationship for eccentricity ǫ, ellipticity e, and flattering fdefine the ellipsoid’s shape and other variables:

ǫ =

(

1 −r2p

r2e

) 12

(C.1)

e = 1 −rp

re

(C.2)

f =1

e(C.3)

The WGS-84 earth model is defined by table C.1

C.2 ECEF coordinates from Longitude and Latitude

With reference to [10], the transformation from Ψ = [λ, φ]T for a given heights h to pECEF =[xe, ye, ze]

T is given by

xe

ye

ze

=

(N + h) cos φ cosλ(N + h) cos φ sin λ

(r2p

r2eN + h) sin φ

(C.4)

where

N =re

(1 − ǫ2 sin2 φ)12

(C.5)

For a ship h is the vertical distance from the sea level to the body frame coordinate origin.The body frame is usually chosen to coincide with the center of gravity.

77

Appendix C. Coordinate Transformations

C.3 NED coordinates from ECEF coordinates

Figure C.2: ECEF coordinate frame with z-axis along Earth’s rotation axis.

Referring to Figure C.2, the transformation matrix from ECEF to NED proceeds with thefollowing sequence of three rotations:Roatation 1: λ about ze

R1 =

cλ sλ 0−sλ cλ 0

0 0 1

(C.6)

Rotation 2: −φ about y′e

R2 =

cφ 0 sφ0 1 0

−sφ 0 cφ

(C.7)

Rotation 3: align axes from up-east-north to north-east-down

R3 =

0 0 10 1 0−1 0 0

(C.8)

78

Appendix C. Coordinate Transformations

or

Rne (z) =

−sφcλ −sφsλ cφ−sλ cλ o−cφcλ −cφsλ −sφ

(C.9)

Coordinate systems are established such that information can be exchanged betweeninterfacing systems in a consistent manner. These reference frames are orthogonal andright-handed.

79