An Underwater Acoustic Positioning System Based …bevuto, memorabile l’ultima festa delle...
Transcript of An Underwater Acoustic Positioning System Based …bevuto, memorabile l’ultima festa delle...
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