a range-based navigation system for autonomous underwater vehicles · a range-based navigation...

A Range-Based Navigation System for Autonomous

Underwater Vehicles

Margarida de Almeida Pedro

Thesis to obtain the Master of Science Degree in

Aerospace Engineering

Supervisor: Prof. António Manuel dos Santos Pascoal

Examination CommitteeChairperson: Prof. João Manuel Lage de Miranda Lemos

Member of the Committee: Prof. António Pedro Rodrigues de Aguiar

December 2014

Abstract

Single-beacon range-based navigation systems for underwater vehicles rely on a vehicle’s motion to

obtain range measurements at different locations in order to estimate its position. This motivates the

study of optimal trajectories for improving the accuracy of the position estimate while respecting mission

related criteria. The performance index used to compare different trajectories is the determinant of a

properly defined Fisher Information Matrix (FIM).

Assuming that heading measurements are available, the problem is studied for 2D and 3D, including

the case when depth measurements can be obtained. Analytical and numerical solutions are derived.

Examples of optimal or close-to-optimal trajectories obtained are (i) circumferences around the beacon

for 2D, (ii) cylindrical helices for 3D and (iii) a circumference around the beacon, in the same horizontal

plane, for 3D including depth measurements. Additionally for 2D, the case of two vehicles using the

same beacon and exchanging range information is studied, for which concentric trajectories that cir-

cumnavigate the beacon are shown to be optimal. An approach to deal with the case where the initial

position of the vehicle is known to lie in a region of uncertainty is presented. A navigation algorithm

consisting of an optimal trajectory planner and a minimum-energy estimator is proposed and tested in

the simulation of a practical scientific scenario.

Keywords: Underwater Range-Based Navigation; Single-Beacon Navigation; Trajectory Optimization;

Fisher Information Matrix

Resumo

Sistemas de navegação para veículos subaquáticos, baseados em medidas de distância e que usam

uma única fonte acústica, dependem do movimento do veículo para que este obtenha medidas de

distância em diferentes posições, de forma a estimar a sua posição. Este facto motiva o estudo de

trajectórias óptimas no sentido de maximizar a precisão da estimativa da posição e, simultaneamente,

respeitar critérios relacionados com a missão do veículo. Para comparar a performance de diferentes

trajectórias, utiliza-se o determinante da Matriz de Informação de Fisher.

Assumindo que se consegue medir a orientação do veículo, abordamos o problema para 2D e 3D,

incluindo o caso em que se mede a profundidade. Obtêm-se soluções analíticas e numéricas. Ex-

emplos de trajectórias óptimas ou ’quase-óptimas’ obtidas são (i) circunferências centradas na fonte

acústica para 2D, (ii) hélices circulares para 3D e (ii) circunferências no plano horizontal e centradas na

fonte acústica para 3D com medidas de profundidade. Adicionalmente, estuda-se o problema com dois

veículos a trocar informação acerca das distâncias, para o qual as trajectórias óptimas são paralelas

e circundam a fonte acústica. Apresenta-se ainda uma estratégia para lidar explicitamente com a in-

certeza na posição inicial. Finalmente, propõe-se um algoritmo de navegação, que inclui um planeador

de trajectórias óptimas e um estimador de energia mínima, que é testado em simulação para uma

aplicação científica.

Palavras-chave: Navegação subquática baseada em distâncias; Navegação com uma única fonte

acústica; Optimização de trajectórias; Matriz de Informação de Fisher

Contents

List of Tables I

List of Figures III

List of Acronyms V

Notation VII

1 Introduction 1

1.1 Historical Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.2 Underwater Navigation Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.3 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.4 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

1.5 Previous Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

1.6 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

1.7 Main Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2 Theoretical background 9

2.1 Estimation Theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.1.1 Maximum-Likelihood Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.1.2 Fisher Information Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.1.3 Minimum-Energy Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.2 Multicriteria Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.2.1 Pareto Optimality . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

3 2D single-beacon navigation 15

3.1 System model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.2 Fisher Information Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

3.2.1 Log-likelihood function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

3.2.2 FIM determinant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

3.3 Trajectory optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

3.3.1 Problem Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

3.3.2 Maximizing the FIM determinant . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

3.3.3 Minimizing energy consumption . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

3.3.4 Minimizing the deviation from a nominal trajectory . . . . . . . . . . . . . . . . . . 30

3.4 Two vehicles exchanging range information . . . . . . . . . . . . . . . . . . . . . . . . . . 32

3.4.1 System configuration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

3.4.2 Fisher Information Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

3.4.3 Trajectory Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

4 3D single-beacon navigation based on range measurements 41

4.1 System model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

4.2 Fisher Information Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

4.3 Trajectory optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

4.3.1 No constraints on the input functions . . . . . . . . . . . . . . . . . . . . . . . . . . 43

4.3.2 Cylindrical helices: close-to-optimal trajectories . . . . . . . . . . . . . . . . . . . . 47

4.4 A realistic assumption: known depth . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

4.4.1 Trajectory Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

4.4.2 Moving on a plane of constant depth . . . . . . . . . . . . . . . . . . . . . . . . . . 52

5 Single-beacon navigation under uncertainty in the initial position 55

5.1 Uncertainty in the initial position of the vehicle . . . . . . . . . . . . . . . . . . . . . . . . . 55

5.1.1 Maximizing the worst case scenario . . . . . . . . . . . . . . . . . . . . . . . . . . 56

5.2 An algorithm for single-beacon navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . 58

5.3 Minimum-energy state estimator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

5.4 Trajectory planner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

5.5 A practical scenario: simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

6 Conclusions and Future Work 69

6.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

6.2 Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70

Bibliography 71

Appendices 77

A Solution of the two vehicles problem 79

List of Tables

3.1 Setup parameters and optimization variables. . . . . . . . . . . . . . . . . . . . . . . . . . 21

3.2 Setup parameters used in the numerical optimization of the FIM determinant and the value

of the FIM determinant obtained for each solution. . . . . . . . . . . . . . . . . . . . . . . 27

3.3 Setup parameters used in the optimization of the FIM determinant and the energy con-

sumption by the vehicle, which results are reproduced in Figure 3.6. . . . . . . . . . . . . 29

3.4 Setup parameters used in the numerical procedure for maximizing the FIM determinant

while minimizing the deviation from a nominal trajectory. The results are presented in

Figures 3.8a and 3.8b. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

4.1 Setup parameters and results of the optimization of helices to maximize the FIM determi-

nant. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

4.2 Setup parameters and results of the optimization of helices to maximize the FIM determi-

nant, imposing limits on the input functions. . . . . . . . . . . . . . . . . . . . . . . . . . . 50

I

List of Figures

3.1 Range-based navigation schematic for 2D scenarios. . . . . . . . . . . . . . . . . . . . . . 15

3.2 Notation used for solving the optimization problem with no constraints on the input functions. 22

3.3 Optimal trajectories for maximizing the FIM determinant imposing no limitations on the

input functions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

3.4 Trajectories that maximize the FIM determinant resulting from the numerical optimization

with the parameters defined in Table 3.2 - with ψ0 = π2 . . . . . . . . . . . . . . . . . . . . . 27

3.5 Trajectories that maximize the FIM determinant resulting from the numerical optimization

with the parameters defined in Table 3.2 - with ψ0 = 0. . . . . . . . . . . . . . . . . . . . . 28

3.6 Pareto curve and the trajectories corresponding to each optimal solution (in the same

color) for the problem of minimizing the FIM determinant and the energy consumption by

the vehicle. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

3.7 Schematic of the problem of minimizing the deviation from a nominal trajectory while

maximizing the FIM determinant, simultaneously. . . . . . . . . . . . . . . . . . . . . . . . 30

3.8 Trajectories that maximize the FIM determinant while minimizing the deviation from a

nominal trajectory - linear motion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

3.9 Single-beacon navigation with two vehicles exchanging range information. . . . . . . . . . 32

3.10 Geometric insight on the two vehicles problem. . . . . . . . . . . . . . . . . . . . . . . . . 37

3.11 Schematic of the solution for the two vehicles problem. . . . . . . . . . . . . . . . . . . . . 38

3.12 Solution for the 2 vehicles problem - circular motion. . . . . . . . . . . . . . . . . . . . . . 38

4.1 Trajectories that maximize the FIM determinant taking in consideration the vehicle dynam-

ics - with p0 = [ 1 1 1]T [m], ψ0 = 7π4 [rad] and ∆t = 1 [s]. . . . . . . . . . . . . . . . . . . 48


ics - with p0 = [ 10 10 10]T [m], ψ0 = 0 [rad] and ∆t = 1 [s]. . . . . . . . . . . . . . . . . . 49


ics - with p0 = [ 1 1 1]T [m], ψ0 = 7π4 [rad] and ∆t = 1 [s]. . . . . . . . . . . . . . . . . . . 50

4.4 Trajectories that maximize the FIM determinant in 3D with known depth - with p0 =

[ 10 10 pd0]T [m], ψ0 = 0 [rad], ∆t = 1 [s], m = 50, v = 1.5 m/s and −rlb = rub = 20

o/s. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

III

5.1 Maximizing the minimum FIM determinant within an uncertainty region, with the uncer-

tainty region far from the beacon - c0 = (100, 100) [m], r0 = 20 [m], ψ0 = π [rad], ∆t = 1

[s], c = 1, m = 16, v = 1.5 m/s and −rlb = rub = 20 o/s. . . . . . . . . . . . . . . . . . . . 57

5.2 Maximizing the minimum FIM determinant within an uncertainty region, with the uncer-

tainty region enclosing the beacon - c0 = (5, 5) [m], r0 = 20 [m], ψ0 = π [rad], ∆t = 1 [s],

c = 1, m = 16, v = 1.5 m/s and −rlb = rub = 20 o/s. . . . . . . . . . . . . . . . . . . . . . . 57

5.3 Schematic of the navigation algorithm. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58

5.4 Flowchart of the algorithm for trajectory planning. . . . . . . . . . . . . . . . . . . . . . . . 64

5.5 Schematic of the scenario used for testing the navigation system. . . . . . . . . . . . . . . 65

5.6 Trajectory obtained in simulation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

5.7 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68

IV

List of Acronyms

AUV - Autonomous Underwater Vehicle

DR - Dead Reckoning

DVL - Doppler Velocity Logger

FIM - Fisher Information Matrix

GIB - GPS Intelligent Buoys

GPS - Global Positioning System

GNSS - Global Navigation Satellite System

IMU - Inertial Measurement Unit

INS - Inertial Navigation System

LBL - Long Baseline

LTV - Linear Time Varying

NED - North-East-Down

ROV - Remotely Operated Vehicle

SBL - Short Baseline

SLAM - Simultaneous Mapping And Navigation

TDOA - Time Difference Of Arrival

TOA - Time Of Arrival

USBL - Ultra Short Baseline

V

Notation

x Regular lower case symbol denotes a scalar

x Bold lower case symbol denotes a vector

X Regular upper case symbol denotes a matrix

XT Transpose of X

X−1 Inverse of X

|X| Determinant of X

In×n n-by-n identity matrix

x Time derivative of x

x Estimate of x

‖x‖ Euclidean norm of x

Jxf Jacobian of f with respect to x

E {·} Expected value

px(·) Probability density function of x

min f Minimum value of f

max f Maximum value of f

arg minxf Value of x that minimizes f

VII

Chapter 1

Introduction

1.1 Historical Background

The history of Marine Navigation starts when humans first started traveling in vessels through the

open sea. The need for establishing their position and orientation with respect to some known location

emerged immediately.

The people from around the Indian and Pacific Oceans are thought to be the first to navigate on

open sea, around 3000 BC. The Austronesians and their descendants, the Malays, the Micronesians

and the Polynesians were great navigators. However, their navigation system was mainly based in their

senses - there is no evidence that any instruments were used. The knowledge of what direction to take

was based on the weather, the winds, the migration of wild species, the motion of certain stars, different

in different days of the year; and this knowledge was passed by oral tradition, often in the form of a

song [1].

Around the 6th century BC, the people from the Mediterranean started taking advantage of the

systematic study of geometry and astronomy made by the Greeks to build navigation charts [2]. Only

much later, in the 15th century AC, when the Portuguese started exploring the African coast facing the

Atlantic Ocean, ‘the fear of Portuguese sailors to be lost in the high seas led them to trust in the heavenly

bodies; they had to determine their course by the stars.’ [3, p. 8]. It was the beginning of the systematic

study of Marine Navigation, leading to better instruments and techniques that allowed the sailors to

estimate their position by observing the night skies.

In the following centuries very precise maps became available and the evolution in Science favored

the improvement of techniques and instruments for navigation. We highlight the mechanical gyroscope,

first used as an instrument to measure attitude in 1743 - the beginning of Inertial Navigation. The next

major breakthrough for navigation was the discovery of electromagnetic waves and all the technology

that came with it, like satellite navigation systems GPS and Galileo.

Nowadays, systems like GPS have become widely available and it is common to find GPS receivers

in mass consumer products like mobile phones, cars and cameras. People all over the world are able to

estimate their position with less than 10 meters of uncertainty [4] and this may suggest that the problem

1

of navigation is almost solved, except for the search of more precise and cost-effective devices. In fact,

that is the case for the environments where GPS signals can reach.

A completely different scenario happens when environment conditions make the use of global navi-

gation satellite systems (GNSS) impossible, like in the underwater environment. Because GPS signals

are blocked by the water surface, GPS is not available as a navigation system underwater. There are

alternatives, but the present solutions are often ad-hoc and lack either flexibility or accuracy. Therefore,

underwater navigation is still an open field of research locking for a solution that is applicable to different

scenarios, covers a wide area and provides position and attitude information with accuracy and integrity,

in a cost-effective manner.

1.2 Underwater Navigation Systems

In the last decades, the use of AUVs (Autonomous Underwater Vechicles) and ROVs (Remotely

Operated Vehicles) has spread. AUVs are currently used in a wide range of scientific, commercial and

military applications, that include oceanographic surveying and mapping, production and exploration of

oil and gas and hull inspection [5]. Nevertheless new applications continue emerging.

In this context, good navigation systems are essential for the proper operation of AUVs, to assure (i)

the correct execution of planned trajectories, (ii) the ability to correctly georeference the data acquired

during surveys and (iii) that the AUVs can be recovered after missions [6].

Presently, there are three main classes of navigation system used in AUVs [7]: Inertial Navigation,

Acoustic Navigation and Geophysical and Feature Based Navigation. For a survey and introduction to

the topic see [5–9].

Dead Reckoning and Inertial Navigation

If no inertial position measurements are available, the most direct approach to the problem of naviga-

tion is Dead Reckoning (DR). It consists on the time integration of velocity or acceleration measurements

from a variety of sensors, providing an estimate of the position and orientation (pose) with respect to the

initial pose. Depending on the sensors that these systems include, the estimates have bigger or smaller

short-time accuracy, but the error of the position estimate always increases with time and distance trav-

eled. Examples of these sensors are [7,8]:

• Inertial Measurement Unit (IMU). Manly composed of a computer, accelerometers, rate gyros and

magnetic compasses, these systems combine the information of all these sensors via dead reck-

oning to continuously provide an estimate of the IMU position and attitude. IMUs provide estimates

with good short-time accuracy and high update rates, however their cost remains prohibitive and

they cannot take in account the current velocity.

• Doppler Velocity Logger (DVL). It measures the relative velocity with respect to the water or the

seabed (depending on the depth). It is often combined with inertial navigation systems (INS) like

IMUs to provide an estimate of the current velocity.

2

Acoustic Navigation

Acoustic systems are specially suitable for underwater applications due to the good propagation

characteristics of sound in water. The working principle of acoustic navigation systems is based on

the time of arrival (TOA) and/or on the time difference of arrival (TDOA) of acoustic signals between

acoustic transponders, which are used to compute range and/or bearing data between those acoustic

transponders. Combining this information, a relative position estimate can be obtained in different ways:

• Range-only data. Measuring the ranges of one vehicle to, at least, three non-colinear beacons of

known location, the relative position of the vehicle with respect to the beacons can be estimated.

• Bearing-only data. Similarly, measuring the bearings of one vehicle to, at least, three non-colinear

beacons of known location, the relative position of the vehicle with respect to the beacons can be

estimated.

• Range and bearing data. Measuring the range and bearing of one vehicle to one beacon of known

location is enough to provide an estimate of the vehicle position with respect to the beacon.

If the beacons positions are known in some absolute reference frame, then these system can provide

absolute position measurements for the vehicle. That is the major advantage of these systems.

However, these systems have some shortcomings as well: (i) in practice to obtain accurate estimates

the number of beacons is bigger than the theoretical minimum referred above; (ii) the correct estimation

of the vehicle position is highly dependent on the accuracy of the beacons calibration (position and, in

some cases, time synchronization); (iii) the area covered by these system is limited; (iv) high operational

costs related to the deployment, calibration and recover of the beacons.

Nevertheless, acoustic navigation system are the most widely employed for underwater navigation if

absolute positioning is required. Thus, there is a wide variety of these systems like Long Baseline (LBL)

[10], Short Baseline (SBL) [11], Ultrashort Baseline (USBL) [12] and GPS Intelligent Buoys (GIB) [13],

with different specifications is terms of accuracy, coverage, acquisition cost and operational costs. For a

detailed survey on these systems and an analysis of the advantages and disadvantages of each system

see [6,8,14–16].

Geophysical and Feature Based Navigation

Environmental features that show a rich spacial diversity can, in principle, be used to extract posi-

tion information. Examples of those features are bathymetric, geomagnetic and gravimetric data, light

intensity or the presence of certain chemical components. Until the present time, the most common

feature used in underwater navigation is bathymetric data, which is used in manned submarines for a

long time. The position estimate is obtained by measuring the local bathymetry over several points in

the vehicle trajectory and comparing those measurements with bathymetric maps using map-correlation

and filtering techniques [17]. Additionally, terrain features are often used in Simultaneous Mapping and

Navigation (SLAM) algorithms [18–20].

3

1.3 Motivation

In the field of marine robotics, a core problem that arises is the determination of the position and

orientation of a vehicle with respect to some inertial reference frame. Several navigation systems exist,

based on different working principles and with different advantages and shortcomings. One possible

way to find better solutions for underwater navigation is to combine the existent systems and technology

in order to overcome the individual drawbacks and enhance the advantages of the global system.

DR based navigation systems estimate the relative change in the vehicle position with errors that

are unbounded over time and distance, thus they require auxiliary navigation methods to provide error

correction and an absolute georeference [21]. Currently, the most common solution is to combine dead

reckoning or inertial navigation systems with acoustic navigation systems like LBL, SBL or USBL, that

can provide a position ’fix’.

However, as seen before, those acoustic navigation system have their own disadvantages, for in-

stance, (i) high operational costs associated with the deployment, calibration or recover of the acoustic

beacons, (ii) lack of flexibility in the sense that every time mission location changes the entire system

has to be relocated and recalibrated and (iii) the limited area of coverage.

To overcome some of these shortcomings, in recent years, several alternatives of acoustic navigation

systems using a single beacon and range measurements have been published [21–31]. These alter-

natives propose a navigation system consisting on a single-acoustic source, of known location, and a

moving vehicle that measuring its ranges to the source can estimate its relative position.

Single-beacon navigation systems using range measurements are a reduced-cost alternative with

respect to conventional acoustic systems (SBL, LBL), that combine the use of a single acoustic beacon

with dead reckoning or inertial navigation. The costs are reduced not only because the number of

beacons is reduced, but also because the calibration procedure is reduced to the determination of the

absolute position of the beacon. Other motivation for the development of single-beacon navigation

systems is the possibility of having smaller UAVs navigating with respect to larger UAVs using the same

principle.

On the other hand, single-beacon navigation introduces some difficulties due to the fact that a single

range measurement is not enough to define the vehicle position with respect to the beacon. Thus, the

vehicle has to move around and acquire range measurements at different positions, while measuring

the displacements within measurements, in order to estimate its own position. This implies that the

navigation system is dependent on the vehicle motion.

Since the navigation system described above is dependent on the vehicle motion, the main goal of

this thesis is to study optimal trajectories that maximize the information provided by the range measure-

ments about the vehicle position.

4

1.4 Problem Statement

Consider a system composed by a static acoustic beacon, at a known location, and a moving vehicle

that measures its range to the beacon and keeps track of its own motion on the body frame. Additionally

consider that the range measurements are corrupted by noise. We will tackle two problems:

i. Given that the vehicle starts at a certain initial position, we want to find which trajectories should

the vehicle follow in order to maximize the accuracy of the position estimate and, simultaneously,

minimize (i) the energy consumption by the vehicle or (ii) the deviation from a nominal trajectory;

ii. Given that the vehicle starts in a certain uncertainty region, we want to find which trajectories

should the vehicle follow in order to maximize the accuracy of the position estimate.

Additionally, consider that another vehicle navigates with respect to same beacon and the two ve-

hicles exchange range information. Given the initial positions of the two vehicles, we want to find what

kind of cooperative strategies, in terms of trajectories, should the vehicles follow in order to maximize

the accuracy of the position estimates of both vehicles.

1.5 Previous Work

One of the first references of single-beacon navigation is [32] where the authors describe the imple-

mentation of a navigation system using range data from one acoustic transponder (from a LBL commer-

cial solution) and yaw and relative velocity measurements from on-board sensors. To estimate the AUV

position and the ocean current velocity they implement a least squares based algorithm. In [22] a similar

solution is presented, but that relies on very accurate dead reckoning information and uses the range

data from a single transponder only as a periodic position ’fix’.

In [28], the authors propose an algorithm for 2D homing and navigation that relies on range data

from a single source and a ’low-cost’ solution for dead reckoning. The algorithm consist of two steps:

a initialization phase based on nonlinear least squares and a refinement phase based on an Extended

Kalman Filter. They showed convergence and robustness of the navigation system in simulation and

post-processing experiments. An important remark is that the algorithm includes maneuvers described

as optimal in the sense of the Fisher Information Matrix (FIM), although the analysis leading to those

results is not presented.

The solution presented in [31] considers a moving beacon and multiple AUVs that are able to mea-

sure their ranges to the beacon and use that information for self-localization. Moreover, a path planing

algorithm is proposed for the moving beacon that should minimize the position errors accumulated in the

other vehicles. The performance of the algorithm is evaluated in simulation. In [33] the authors analyze

the observablity of such a system and compare the performance of several estimators. In [21] the au-

thors describe the implementation of a navigation system using a moving beacon and multiple AUVs with

synchronized clocks. Thus, the AUVs can measure their range to the beacon from the one-way-travel-

time of the acoustic signal broadcasted by the moving beacon at periodic intervals. The paper focus

5

on the implementation of the acoustic communications and an Extended Kalman Filter for position es-

timation. Furthermore, results of post-processing data from deep-water experiments show comparable

performance with a conventional LBL system.

In [24–26] the authors assess the local observablity of the 2D single-beacon range-only localization

system in the presence of unknown constant ocean currents. Recurring to the linearization of the system

kinematics around potential vehicle trajectories they identify the nonobservable trajectories and prove

asymptotic convergence of the estimator for the remaining trajectories. In [34], the observability analysis

considers two kinds of maneuvers in 2D, consisting of straight-line and circular motion and using not

only range measurements to a single source but also heading measurements. The authors assess not

only local observability but also global observability, for the maneuvers considered.

In [27] the authors address the problem of 3D localization using range measurements to a single-

source and relative velocity measurements, in the presence of unknown constant currents. A state

transformation is applied such that, preserving the observability properties of the original non-linear time

varying system, the system can be regarded as a linear time varying system (LTV). Thus, necessary

and sufficient conditions are derived using LTV observability theory for linear systems. [35] contains a

similar observability analysis, but considering that the moving vehicle can also measure its attitude with

respect to an inertial reference frame, and employs other methodologies to assess local observability.

In [36], the authors assess the observability of 3D single beacon navigation for trimming trajectories.

Regarding trajectory optimization to improve the accuracy of the navigation system, [37] employs a

metric based on the FIM to optimize formation flights with position provided by a range-based navigation

system. In [38], the authors describe a metric based on the observability matrix for nonlinear systems,

that is used to assess the performance of the 3D single-beacon range-only navigation system, for a

static beacon and one vehicle undergoing different trajectories. Finally, [39, 40] use the determinant of

the FIM to find the optimal sensor placement that maximizes the accuracy of the localization of a static

target using range-only measurements to a set of beacons (sensors).

1.6 Thesis Outline

In Chapter 2 we introduce the most important theoretical concepts behind this thesis, namely (i)

estimation theory, including maximum likelihood estimation, the Fisher information matrix and minimum-

energy estimation, and (ii) multivariate optimization, in which we introduce the concept of Pareto opti-

mality.

In Chapter 3 we proceed to study optimal trajectories for single-beacon range-only navigation in

2D, given a certain initial position. The optimization consist of maximizing the determinant of the FIM,

used as a measure of the position estimate accuracy, and simultaneously (i) minimizing the energy

consumption by the vehicle and (ii) minimizing the deviation from a nominal trajectory. At the end of this

chapter we study optimal trajectories in the case of two vehicles navigating with respect to the same

beacon and exchanging range information.

In Chapter 4 we investigate the trajectories that maximize the FIM determinant given the initial posi-

6

tion of the vehicle. First we approach the problem considering that all the coordinates of the 3D position

need to be estimated and, later, we assume that the vehicle can measure the depth, thus discarding the

need to estimate depth with the range-based navigation system.

In Chapter 5 we introduce the problem of the uncertainty associated with the vehicle initial position.

We study optimal trajectories for this scenario in 2D and 3D with known depth. Additionally, we develop

a single-beacon range-based navigation algorithm consisting of (i) a recursive trajectory planner that

maximizes the degree of observability of the system and (ii) a minimum energy estimator for the vehicle

position.

Finally, in Chapter 6 we draw some conclusions from the work developed in this thesis and recom-

mend related future work.

1.7 Main Contributions

In Chapter 3 we provide a detailed geometric characterization of the trajectories that maximize the

FIM determinant for 2D single-beacon range-only navigation. Additionally, we provide an analysis of how

the performance of the system is affected by (i) the vehicle dynamics, (ii) the minimization of the energy

consumption by the vehicle and (iii) the constraint of following a nominal trajectory. When possible, we

provide a geometric characterization of the optimal trajectories taking this considerations in account.

In Chapter 4 we provide important properties of the trajectories that maximize the FIM determinant

for 3D single-beacon range-only navigation. Furthermore, we provide a geometric characterization of

optimal trajectories for the scenario where the vehicle moves in a horizontal plane and measures depth.

In Chapter 5 we use a maxmin optimization to find trajectories that ensure a minimum performance

of the navigation system when there is uncertainty associated with the initial position of the vehicle.

Moreover, we develop an algorithm that planes optimal trajectories to reduce the uncertainty on the

position estimate, in a recursive manner.

7

Chapter 2

Theoretical background

This chapter provides a brief overview of the main theoretical concepts behind this thesis. First we

introduce estimation theory for stochastic systems covering the maximum-likelihood framework and the

Fisher information matrix. Next, we approach the subject of minimum energy estimation for nonlinear

systems. Finally, we present a short introduction on multicriteria optimization, where we will introduce

the concept of Pareto optimality.

2.1 Estimation Theory

This Section is manly build upon [41] for which we refer the reader to for a broader and deeper

understanding on the subject.

In summary, estimation theory focus on the problem of parameter estimation, given a certain model

structure. This problem has four essential parts:

• Parameter space, where the variable of the estimation problem lies. Let the variable be denoted

by θ;

• Observation space, that in the classical problem is finite-dimensional, and consists on the domain

of the measurements from which we want to estimate the parameters. Let y denote a set of

measurements;

• Probabilistic mapping from parameter space to output space, that is the model structure re-

ferred above or probabilistic law that governs the effect of θ on y;

• Estimation rule, commonly referred to as an estimator, that from the observations y returns an

estimate of θ (denoted by θ(Y)).

There is a great number of different estimation rules, with different properties. In this thesis we will

focus on the maximum-likelihood and minimum-energy estimation. The Fisher Information Matrix is

introduced within the maximum-likelihood framework.

9

2.1.1 Maximum-Likelihood Estimation

Following on the four essential parts of the estimation problem enumerated before, consider that the

probabilistic mapping from the parameter space to the output space is given by a probability function

py(y|θ) that we will refer to as the likelihood function. We can also use the logarithm, ln py(y|θ), denoted

the log-likelihood function.

A maximum likelihood estimate θML(y) is a value of θ at which the likelihood function or the log-

likelihood function are maximum. If the maximum of the log-likelihood function lies on the parameter

space and the log-likelihood function has a continuous first derivative, then Equation 2.1 holds.

Jθ ln py(y|θ)|θ=θML= 0 (2.1)

Maximum-likelihood estimators have very desirable asymptotic properties (when the number of ob-

servations is very large, N →∞):

• Consistency: the estimate converges to the true value of the parameter;

• Efficiency: the variance of the estimation error reaches the Cramér-Rao Lower Bound (which will

be introduced next).

2.1.2 Fisher Information Matrix

If θML(y) is an unbiased estimate of θ then the Cramér-Rao Lower Inequality, given by

Cov{θML(y)− θ

}≥(E{

[Jθ ln py(y|θ)] [Jθ ln py(y|θ)]T})−1

(2.2)

holds. This result is of major importance because it imposes a limit on the performance of an unbiased

estimator. The right-hand side of the inequality given by Equation 2.2, called the Cramér-Rao Lower

bound, is the lowest bound of the variance of the estimation error of an unbiased estimator [42]. Its

inverse,

FIM(θ) = E{

[Jθ ln py(y|θ)] [Jθ ln py(y|θ)]T}

(2.3)

is the Fisher Information Matrix, denoted by FIM, and it can be understood as a measure of the amount

of useful information that the observations provide about the parameters to be estimated.

When the FIM is invertible, its inverse FIM−1 contains the information of the most and least error

sensitive directions of the parameter space. Moreover, in the case of unbiased estimates, the eigenvec-

tors of FIM−1 define the directions of the axes of the uncertainty ellipsoid associated with the estimation

error of θ, with the length of each axis being proportional to the corresponding eigenvalue. Therefore, the

determinant of FIM−1 is proportional to the volume of the uncertainty ellipsoid. Hence, the determinant

of the FIM increases when the volume of the uncertainty ellipsoid of the estimation error decreases.

Remarks:

• The property of the determinant of the FIM of being proportional to the inverse of the volume of

10

the uncertainty ellipsoid of the estimate error makes it a suitable performance index to compare

different estimates. Examples of the use of the determinant of the FIM as a metric for the accuracy

of estimates are widely available in the literature [40,43–45]. Other metrics related to the FIM are

its minimum singular value and the conditions number. For an extensive overview on these and

other metrics for the accuracy of parameter estimates see [46].

• In [47] the authors study the link between the invertibility of the FIM and the observability of pa-

rameters to be estimated in nonlinear systems affected by additive noise. They find that if the FIM

is non-singular for some parameter θ0 then the noise-free system is locally observable at θ0.

2.1.3 Minimum-Energy Estimation

In this section we introduce the subject of minimum-energy estimation for nonlinear systems following

the approach used in [48–50]. Minimum-energy estimators are formulated in a deterministic framework

by assuming that the unknown initial condition, process noise and observation noise are deterministic.

These disturbances are assumed to be bounded over time and the their norm is a measure of the

energy of the uncertainties. The minimum-energy estimate yields the system trajectory that produces

the observations with minimum energy of the uncertainties.

x = f (x,u) +G(x)w (2.4a)

y = h (x,u) +K(x)v (2.4b)

x(0) = x0 (2.4c)

Consider the problem of estimating the current state x(t) ∈ Rn of the nonlinear system given in Equa-

tions 2.4 from the past history of the system inputs u(τ) ∈ Rm for τ ∈ [ 0 , t] and some information about

the initial state x0. Assume that w(t) and v(t) are deterministic disturbances, unknown but bounded

over time. The minimum-energy estimate of the current state x(t) is given by

x(t) ∈ arg minx∈Rn

s (x, t) (2.5)

with

s (x, t) , minz,w,v

{(z(0)− x0)

TQ0 (z(0)− x0) +

1

2

t∫0

‖w(τ)‖2 + ‖v(τ)‖2dτ :

z(τ) = f (z(τ),u(τ)) +G (z(τ)) w(τ), y = h (z(τ),u(τ)) +K(z(τ))v(τ), z(t) = x

}(2.6)

where x0 and Q0 encode the a-priori information about the initial state.

For f , h,G andK Lipschitz continuous on Rn and piece-wise continuous input functions, the following

11

equations provide a local solution for the minimum-energy state estimate problem

Q = −QJxf (x,u)− (Jxf (x,u))TQ−QΓ(x)Q+ (Jxh (x,u))

TR−1(x) Jxh (x,u) (2.7)

˙x = f (x,u) +Q−1 (Jxh (x,u))TR−1(x) (y − h (x,u)) (2.8)

with

Γ(x) = G(x) (G(x))T (2.9)

R(x) = K(x) (K(x))T (2.10)

Remarks:

• Under the condition that this system is uniformly observable for every input, in [49] the author

proved asymptotic convergence of the estimate to the true state.

• In [50] the author showed that, for linear systems, the minimum-energy estimate is equivalent to

the minimum-variance estimate, being the later formulated in a stochastic framework.

• In [51] the authors highlight that, for linear systems, the minimum-energy estimate is identical to

that generated by the Kalman-Bucy filter for the analogous linear stochastic systems with initial

covariance Q−10 , because the structure of the two filters is identical. In that case, the authors

suggest that the matrix Q−1 plays the role of the covariance matrix of the estimation error.

2.2 Multicriteria Optimization

Multicriteria optimization, vector optimization or Pareto optimization concerns an optimization prob-

lem where multiple objective functions must be optimized simultaneously. In mathematical terms, the

problem can be formulated as

min ( f1(x) , f2(x) , ... , fm(x) ) subject to x ∈ X

where the integer m ≥ 2 is the number of objective functions and X is the set of feasible solution

vectors, typically defined by some constraint functions. Notice that we have formulated the problem as

a minimization problem, because a maximization problem can be easily transformed into a minimization

problem: max f(x) is equivalent to min −f(x).

If some vector x∗ ∈ X minimizes all the objective functions simultaneously, then x∗ is the optimal

solution. However, in multicriteria optimization it is usually not the case - typically there is not a feasible

solution x∗ that minimizes every objective function simultaneously. To deal with those cases we introduce

the concept of Pareto optimality.

12

2.2.1 Pareto Optimality

Pareto optimal solutions are feasible solutions that cannot be improved without degrading at least

one of the objective functions. In mathematical terms, x∗ and its outcome ( f1(x∗) , f2(x∗) , ... , fm(x∗) )

is a Pareto optimal solution if

i. fi(x∗) ≤ fi(x) ∀x∈X , for all i ∈ {1, ...,m}, and

ii. fi(x∗) < fi(x) ∀x∈X , for at least one i ∈ {1, ...,m}.

The set of all the Pareto optimal solutions for a problem is denominated the Pareto front or Pareto

boundary.

13

Chapter 3

2D single-beacon navigation

As a first step towards the solution of the general problem of single-beacon localization using range-

only measurements we study the problem for 2D scenarios. In Section 3.1, we derive the model of

the underlying system considering one vehicle moving in the 2D plane and measuring its distance to

the beacon. Next, in Section 3.2, we introduce the Fisher Information Matrix and its determinant for

the problem at hand, under the assumption that the range measurements are corrupted by additive

Gaussian noise. Then, in Section 3.3, we proceed to find the vehicle trajectories that maximize the

FIM determinant. Later, in the same section, we add other cost criteria to the optimization problem to

minimize the energy consumption by the vehicle and to minimize the deviation from a nominal trajectory.

3.1 System model

Figure 3.1: Range-based navigation schematic for 2D scenarios.

Consider the system depicted in Figure 3.1 which represents a vehicle moving while measuring its

distance d(t) : [0, tf ] → R+ with respect to a single beacon. The trajectory of the vehicle is controlled

through its speed v(t) : [0, tf ] → R+ and yaw rate r(t) : [0, tf ] → R . We assume that the velocity is

15

always aligned with the longitudinal axis of the body. The beacon location is known with respect to some

inertial reference frame {I}, with North-East-Down orientation (NED). Besides, the vehicle has access

to the heading angle ψ(t) : [ 0, tf ] → [ 0, 2π) at any given instant in time which gives it the orientation of

its body frame with respect to the inertial frame {I}.

Let the vehicle position in {I} be denoted by p(t) = [ pn(t) pe(t) ]T , with p(t) : [ 0, tf ] → R2 \ {0}.

Moreover, consider the beacon position in {I} to be fixed and denoted by b0 = [ bn0 be0 ]T . Note that the

distance between the vehicle and the beacon is given by

d(t) = ‖p(t)− b0‖ (3.1)

Then, the kinematic model of the system considered, with state x(t) = [ pn(t) pe(t) ψ(t) ]T , input u(t) =

[ v(t) r(t) ]T and output y(t) = [ d(t) ψ(t) ]

T for t ∈ [ 0, tf ] is given by

x(t) =

v(t) cos(ψ(t))

v(t) sin(ψ(t))

r(t)

(3.2)

y(t) =

‖p(t)− b0‖

ψ(t)

(3.3)

For the remainder of this thesis, and without loss of generality, we will assume the beacon to be

located at the origin of the reference frame {I}, b0 = 0 . Thus, the output Equation 3.3 can be simplified

into

y(t) =

‖p(t)‖

ψ(t)

(3.4)

Note that, if the beacon is not located at the origin of {I}, i.e. b0 6= 0, we can define a transformation

p , p− b0 which yields ˙p = p and d = ‖p‖. Then, considering the new variable p, the beacon is at the

origin. Because ψ defines the orientation of the body-frame with respect to the inertial frame and this

transformation does not involve any rotation, ψ is invariant to the transformation. Moreover, because r

and v are defined with respect to the vehicle body-frame, those are also invariant to this transformation.

3.2 Fisher Information Matrix

Consider the system introduced in Section 3.1. Let zi with i = 0 , ... , m − 1 denote a set of m

measurements of the distance between the vehicle and the beacon d(t) corrupted by additive noise.

Note that these measurements are obtained at different time instants, ti. Moreover, let di with i =

0 , ... , m− 1 denote the actual distances at the time instants of the measurements. The measurement

model is given by Equations 3.5.

16

zi = di + wi , i = 0 , ... , m− 1 (3.5a)

wi ∼ N (0, σ2) , i = 0 , ... , m− 1 (3.5b)

Assume that the model introduced in section 3.1 and the measurement model above describe the

system perfectly. Additionally, assume that the initial position of the vehicle p0 = ( pn0 pe0 )T is unknown,

but that from the set of measurements z we can obtain an unbiased estimate p0. Under these assump-

tions, the determinant of the FIM is proportional to the volume the uncertainty ellipsoid of the estimation

error. Thus, in the following steps, we will derive the FIM and the FIM determinant for this estimation

problem, and in Section 3.3 we will adopt the FIM determinant as a performance index for the estimates.

Note that if the FIM is nonsingular, the system is observable [47], meaning that trajectories that result

from the maximization of the FIM determinant (with |FIM | > 0) render the system observable.

3.2.1 Log-likelihood function

Let us introduce the vector notation where z = [ z0 z1 ... zm−1 ]T is a vector containing the range

measurements, d = [ d0 d1 ... dm−1 ]T contains the corresponding actual ranges and w = [w0 w1 ... wm−1 ]

T

is a vector containing the correspondent measurement noise samples with w ∼ N (0, R) and R = σ2 I.

Note that the distance between the vehicle and the beacon d(t) is a function of the position of the

vehicle p(t) and

p(t) = p0 +

t∫0

p(ψ(τ), v(τ)) dτ

= p0 +

t∫0

v(τ)

cos(ψ(τ))

sin(ψ(τ))

dτ (3.6)

with

ψ(τ) = ψ0 +

τ∫0

ψ(r(ζ)) dζ

= ψ0 +

τ∫0

r(ζ) dζ (3.7)

Because we can write the position of the vehicle at an arbitrary time t as a function of the initial

position p0, the initial heading ψ0 and the time history of the input functions v(τ) and r(τ) for τ ∈ [ 0 , t[,

the distance d(t) is also a function of p0, ψ0 and v(τ) and r(τ) for τ ∈ [ 0 , t]. With some abuse of

notation, we will denote the vector d as d(p0) to make the dependence explicit.

The likelihood function for the measurement vector with respect to the unknown initial position is

given by Equation 3.8, from which we derive the log-likelihood function given by Equation 3.9.

17

Lp0(z) = pz|p0(z|p0) =1

(2π)m2 |R|

exp

{−1

2(z− d(p0))

TR−1 (z− d(p0))

}(3.8)

lnLp0(z) = −m

2ln(2π)− ln(|R|)− 1

2(z− d(p0))

TR−1 (z− d(p0)) (3.9)

Let Jp0denote the Jacobian matrix, with derivatives with respect to the variable p0. Then, the

Jacobian of the log-likelihood function with respect to p0 is given by

Jp0lnLp0

(z) = (Jp0d)TR−1 (z− d) (3.10)

3.2.2 FIM determinant

The Fisher Information Matrix for the problem at hand is defined as

FIM(p0) , E{

( Jp0 lnLp0(z) ) ( Jp0 lnLp0(z) )T}

(3.11)

where E {x} denotes the expectation of x.

Making use of the results derived in Section 3.2.1, we proceed to compute the FIM:

FIM(p0) = E{[

(Jp0d)TR−1 (z− d)] [

(Jp0d)TR−1 (z− d)]T}

= E{

(Jp0d)TR−1 (z− d) (z− d)

T (R−1

)T(Jp0

d)}

= (Jp0d)TR−1E

{(z− d) (z− d)

T}(R−1

)T(Jp0

d)

= (Jp0d)TR−1R

(R−1

)T(Jp0

d)

=1

σ2(Jp0

d)T (Jp0d) (3.12)

Note that, by definition,

(Jp0d) ,

∂d0

∂pn0

∂d0

∂pe0...

...∂dm−1

∂pn0

∂dm−1

∂pe0

(3.13)

and [∂di∂pn0

∂di∂pe0

]=

[pnidi

peidi

], i = 0, ...,m− 1 (3.14)

where pi = [ pni pei ]T denotes the position of the vehicle at t = ti.

Thus, the FIM can be rewritten as follows:

18

FIM(p0) =1

σ2

m−1∑i=0

(pnidi

)2 m−1∑i=0

(pnidi

)(peidi

)m−1∑i=0

(pnidi

)(peidi

)m−1∑i=0

(peidi

)2

(3.15)

Remark 1 Interestingly, the FIM for the problem at hand has exactly the same structure as the FIM for

the dual problem tackled in [39], [40], [52] and [53], which consists on the problem of target localization

using a sensor-network and range-only measurements or a single mobile sensor. Some consequences

of this similarity between the three problems problems will be discussed in later sections.

Finally, in equation 3.16 we introduce the determinant of the Fisher Information Matrix for this prob-

lem.

|FIM(p0)| = 1

σ4

(m−1∑i=0

(pnidi

)2)(

m−1∑i=0

(peidi

)2)−

(m−1∑i=0

(pnidi

)(peidi

))2 (3.16)

Note that the FIM determinant depends not only on the actual ranges but also on the positions where

those ranges are measured. This confirms that different trajectories may yield different accuracy in the

initial position estimate.

3.3 Trajectory optimization

In the previous section we have computed the FIM determinant and stated that it can be used as a

measure of the accuracy of the initial position estimate. Besides, we have shown that it depends on the

vehicle trajectory. In this section we will find the trajectories that maximize the FIM determinant, which

we assume to be the trajectories that maximize the accuracy of the position estimate.

The accuracy of the navigation system is extremely important for it to be adequate for certain mis-

sions. However, we do not expect a vehicle to navigate with the single purpose of knowing its location.

Thus, we are also interested in finding optimal trajectories that meet some other mission-related criteria.

In addition to maximizing the determinant of FIM, in Section 3.3.3 we will study trajectories that minimize

the energy consumption by the vehicle and in Section 3.3.4 we will study trajectories that minimize the

deviation from a certain nominal trajectory.

In order to find the optimal trajectories we will first propose a parameterization for those trajectories

and then find the set of values for the parameters that optimize the criteria introduced before.

19

3.3.1 Problem Setup

Assumption 1. For practical reasons the vehicle speed is defined to be constant and the yaw rate is

piecewise constant. We have

v(t) = v (3.17)

r(t) = rk, for tk ≤ t < tk+1, and k = 0, ..., N − 1 (3.18)

with rlb ≤ rk ≤ rub, where rlb and rub are the lower and upper bounds on the vehicle yaw rate, respec-

tively; these bounds should be set in accordance with the characteristics of specific vehicles.

Assumption 2. Moreover, for simplicity we assume that the time instants at which the inputs may

change are equally distanced in time:

tk+1 = tk + T, k = 0, ..., N − 1 and t0 = 0 and tN = tf (3.19)

where T is the cycle interval. This is equivalent to

tk = k T, k = 0, ..., N − 1 (3.20)

Finally, we can write the vehicle position at any time as a function of the trajectory parameters p0,

ψ0, v, r = ( r0 , r1 , ... , rN−1 ) and T which is given by

p(t) = p0 + v

k−1∑j=0

1

rj

sin(ψ((j + 1)T ))− sin(ψ(j T ))

− cos(ψ((j + 1)T )) + cos(ψ(j T ))

+v

rk

sin(ψ(t))− sin(ψ(k T ))

− cos(ψ(t)) + cos(ψ(k T ))

(3.21a)

ψ(t) = ψ0 + T

k−1∑j=0

rj + (t− k T ) rk (3.21b)

k = floor

(t

T

)(3.21c)

where the function floor(x) returns the largest integer not greater than x.

If some rl is zero, with l ∈ {0, ..., N − 1}, the function p(t) has a singularity, which is removable.

In that case, if l < k, the term concerning rl in the summation appearing on the expression of p(t) is

replaced by

c∆t v

cos(ψ(l T ))

sin(ψ(l T ))

, if l < k (3.22)

or, if l = k, the third term of the expression of p(t) is replaced by

c (t− l∆t) v

cos(ψ(l T ))

sin(ψ(l T ))

, if l = k (3.23)

20

Assumption 3. Additionally, lets assume that while the vehicle is moving it gets the range measure-

ments at a constant sampling rate. Let the sampling interval be denoted by ∆t and the time instants at

which the range measurements are obtained be given by

ti = i∆t, i = 0, ...,m− 1 (3.24)

Assumption 4. Furthermore, for convenience, lets assume that the cycle interval of the yaw rate

function, T , is a multiple of ∆t. We introduce a new tuning parameter c, such that T = c∆t.

Then, the vehicle positions and the actual ranges, at the time the measurements are obtained, are

given by

k =i

c(integer division) (3.25a)

ψi = ψ0 + c∆t

k−1∑j=0

rj + mod

(i

c

)∆t rk (3.25b)

pi = p0 + v

k−1∑j=0

1

rj

sin(ψ(j+1) c

)− sin (ψjc)

− cos(ψ(j+1) c

)+ cos (ψjc)

+v

rk

sin (ψi)− sin (ψkc)

− cos (ψi) + cos (ψkc)

(3.25c)

di = ‖pi‖ (3.25d)

where mod(ic

)is the modulo operation, which returns the remainder of the integer division of i by c.

Again, if some rl is zero, with l ∈ {0, ..., k}, pi is undefined. In these cases, if l < k, the term

concerning rl in the summation appearing on the expression of pi is replaced by

c∆t v

cos(ψ(l c))

sin(ψ(l c))

, if l < k (3.26)

or, if l = k, the third term of the expression of pi is replaced by

(i− l c) ∆t v

cos(ψ(l T ))

sin(ψ(l T ))

, if l = k (3.27)

Remember the FIM determinant derived in Section 3.2.2,

|FIM(p0)| = 1

σ4

(m−1∑i=0

(pnidi

)2)(

m−1∑i=0

(peidi

)2)−

(m−1∑i=0

(pnidi

)(peidi

))2

Using the Equations 3.25 the FIM determinant is fully defined as a function of the problem parameters

and variables, which are summarized in Table 3.1.

setup parameters p0, ψ0, ∆t, m, σ, c, v, rlb, rub

problem variables r0, ..., rN−1, with N = m−1c

Table 3.1: Setup parameters and optimization variables.

21

Notice that the speed v is a parameter that should be set in accordance to the characteristics of

specific vehicles. Small AUVs usually keep the speed approximately constant during missions and a

typical value is 1.5 m/s. For the lower and upper bounds on the yaw rate we use a typical value from the

MEDUSA 1, about -20 and 20 degrees per second, respectively.

In the remainder of this Chapter, we will find trajectories that maximize the FIM determinant by the

proper choice of the yaw rate inputs. Later on, other criteria will be introduced in the optimization: the

energy consumption in the vehicle motion and the deviation from a nominal trajectory.

3.3.2 Maximizing the FIM determinant

3.3.2.1 No constraints on the vehicle trajectory

For the sake of getting a more general result, lets first discard the constraints imposed on the inputs

functions in the precious section. Assume the vehicle can execute any kind of maneuver, this is, the

input functions can be of any kind as long as the states evolution is continuous. In these conditions,

the problem can be broken in two parts: first we want to find the set of points where the vehicle should

get the range measurements so that it maximizes the determinant of FIM; and then we will try to find

trajectories that fit those points. This optimization problem was solved in collaboration with Navin Crasta.

Figure 3.2: Notation used for solving the optimization problem with no constraints on the input functions.

Remember the FIM determinant given by Equation 3.16 and check Figure 3.2. Note that the termspnidi

and peidi

on the determinant equation represent the cosine and sine, respectively, of the angle between

the axis pointing north and the position vector of the vehicle, αi.

Then, consider two new variables Γ1 , [ cos(α0) ... cos(αm−1) ]T and Γ2 , [ sin(α0) ... sin(αm−1) ]

T .

Note that

m−1∑i=0

(pnidi

)2

=

m−1∑i=0

cos2(αi) = ‖Γ1‖2 (3.28)

m−1∑i=0

(peidi

)2

=

m−1∑i=0

sin2(αi) = ‖Γ2‖2 (3.29)

1MEDUSA is a small AUV for scientific research, from Instituto Superior Técnico/Institute for System and Robotics

22

andm−1∑i=0

(pnidi

)(peidi

)=

m−1∑i=0

cos(αi) sin(αi) = ‖Γ1‖‖Γ2‖ cos(Θ) (3.30)

where Θ is the angle between Γ1 and Γ2.

Additionally note that

‖Γ1‖2 = m− ‖Γ2‖2 (3.31)

Then, the FIM determinant can be rewritten as

|FIM | = σ−4(‖Γ1‖2‖Γ2‖2 − ‖Γ1‖2‖Γ2‖2 cos2(Θ)

)= σ−4‖Γ1‖2‖Γ2‖2

(1− cos2(Θ)

)= σ−4‖Γ1‖2

(m− ‖Γ1‖2

)sin2(Θ) (3.32)

and, consequently,

ln(|FIM |) = −4 ln (σ) + ln(‖Γ1‖2

(m− ‖Γ1‖2

))+ ln

(sin2(Θ)

)(3.33)

Therefore maximizing the FIM determinant is equivalent to

max ln(|FIM |) (3.34)

with ln(|FIM |) given by Equation 3.33. The first order optimality conditions for this problem are given

by J ln(|FIM |) = 0 that yields

∂ ln(|FIM |)∂‖Γ1‖

=m− 2‖Γ1‖2

‖Γ1‖ (m− ‖Γ1‖)= 0 (3.35a)

∂ ln(|FIM |)∂Θ

=2 cos (Θ)

sin (Θ)= 0 (3.35b)

The solution for the systems of Equations 3.35 is given by

Θ∗ = (2n+ 1)π

2, n ∈ Z and ‖Γ∗1‖2 =

m

2

and due to the equality given by Equation 3.31 we have ‖Γ∗2‖2 = m2 .

Moreover, note that

∂2 ln(|FIM |)∂‖Γ1‖2

=m‖Γ1‖2 − 2‖Γ1‖4 −m2

‖Γ1‖2 (m− ‖Γ1‖2)2 (3.36a)

∂2 ln(|FIM |)∂Θ2

=−2

sin2 (Θ)(3.36b)

23

∂2 ln(|FIM |)∂‖Γ1‖∂Θ

= 0 (3.36c)

Therefore the Hessian matrix is given by

Hess (ln(|FIM |)) =

−2

sin2 (Θ)0

0m‖Γ1‖2 − 2‖Γ1‖4 −m2

‖Γ1‖2 (m− ‖Γ1‖2)2

(3.37)

and

Hess (ln(|FIM |))|(‖Γ1‖,Θ)=(‖Γ∗1‖,Θ∗) = −2

1 0

0 4m−1

< 0

This implies that ‖Γ∗1‖, ‖Γ∗2‖ and Θ∗ are optimal values which maximize the FIM determinant. And,

consequently, the maximum of the determinant of the FIM is given by

|FIM |∗ =( m

2σ2

)2

and the corresponding FIM

FIM∗ =( m

2σ2

)2

I2

Note that FIM∗p0is a diagonal matrix with equal entries, therefore all of its eigenvalues are equal.

Thus, the solution that maximizes the FIM determinant also maximizes the condition number (the ratio

between the minimum and maximum eigenvalues). Furthermore, recall that the determinant of a matrix

equals the product of its eigenvalues. If another solution existed with greater minimum eigenvalue, the

determinant would necessarily be bigger than the optimal, which is not possible. Thus, this solution also

maximizes the minimum eigenvalue.

In order to interpret what kind of trajectories this solution may yield, we borrow from Fourier analysis

a result known as the orthogonality relations for sines and cosines.

Orthogonality relations for sines and cosines Let p, q be nonzero integers and m be a positive

integer. Then

m−1∑i=0

cos

(2πp

mi

)= 0 (3.38a)

m−1∑i=0

sin

(2πp

mi

)= 0 (3.38b)

m−1∑i=0

sin

(2πp

mi

)cos

(2πq

mi

)= 0 (3.38c)

m−1∑i=0

cos

(2πp

mi

)cos

(2πq

mi

)=m

2δpq (3.38d)

24

m−1∑i=0

sin

(2πp

mi

)sin

(2πq

mi

)=m

2δpq (3.38e)

By setting p = q in the orthogonality relations for sines and cosines we have

m−1∑i=0

sin

(2πp

mi

)cos

(2πp

mi

)= 0 (3.39a)

m−1∑i=0

cos2

(2πp

mi

)=m

2(3.39b)

m−1∑i=0

sin2

(2πp

mi

)=m

2(3.39c)

Recall Equations 3.28, 3.29 and 3.30. Invoking Equations 3.39, the optimal solution yields

m−1∑i=0

cos(αi) sin(αi) = 0 (3.40a)

m−1∑i=0

cos2(αi) =

m−1∑i=0

cos2

(2πp

mi

)(3.40b)

m−1∑i=0

sin2(αi) =

m−1∑i=0

sin2

(2πp

mi

)(3.40c)

(a) Optimal solution - no constraints. (b) Optimal solution - constant turning rate.

Figure 3.3: Optimal trajectories for maximizing the FIM determinant imposing no limitations on the inputfunctions.

Figure 3.3a represents the general form of one family solutions for the problem at hand. From

Equations 3.40, it becomes clear that the optimality conditions are fulfilled if the points where the range

measurements are obtained are radially distributed around the beacon, with equal angles separating

25

each par of points. Additionally, the solution remains optimal if any of these points is replaced by its

reflection with respect to the beacon. A very important result is that the solution is not dependent

on the distance of the points to the beacon, neither on its sequence in time. Furthermore, note that the

optimality of a solution is not affected by a rotation centered in the beacon; this is, if we have α′i = α∗i +δα

for all i = 0, ...,m− 1, the solution in α′i remains optimal for any constant but arbitrary δα.

Note that combining all the properties of this family of solutions, rotation, reflection of each of the

points with respect to the beacon, no dependence on the distance to the beacon or on the order the

measurements are obtained, and different angular separation (controlled by the integer p), the possible

shapes of all the possible different trajectories that fulfill the conditions given by Equations 3.40 become

untraceable.

Figure 3.3b depicts an optimal solution with constant turning rate and maintaining a fixed distance to

the beacon. Note that the trajectory is such that the velocity of the vehicle is always perpendicular to the

line that connects the vehicle to the beacon. Interestingly, this is the maneuver described by [28] as the

optimal maneuver for single beacon navigation, where the authors state that it was obtained by studying

the FIM but do not elaborate on what kind of procedure were used.

Remark 2 As a consequence of Remark 1 in Section 3.2.2, the optimality conditions for 2D single-

beacon localization, with no constraints on the input functions, are the same as for the optimal 2D

sensor placement, in the dual problem. Also the optimal value found for the FIM determinant is same as

in [39], [52] and [53].

Finally, note that other kinds of solutions may exists that are not in the form of the orthogonality

conditions for sines and cosines, but that still fulfill the optimality conditions.

3.3.2.2 Constant speed and piece-wise constant yaw rate

While in the last Section we found the optimal trajectories discarding any limitations on the vehicle

maneuverability, in this Section we intend to find optimal trajectories considering the limitations on the

vehicles speed and turning rate. In this situation, the complexity of the optimization problem increases

considerably because we have to consider the vehicle kinematics explicitly. Thus, we will use the trajec-

tory parameterization that we introduced in Section 3.3.1, which allows for the number of optimization

variables to be finite, and numerical methods will be used to solve the optimization problem.

The problem of finding the trajectories that maximize the FIM determinant comes down to finding the

set of N values for the optimization variables that maximizes the FIM determinant. Mathematically we

have

maxr0,...,rN−1

|FIM | (3.41)

subject to

rlb < rk < rub, for k = 0, ..., N − 1 (3.42)

26

with |FIM | as a function of r defined by the Equations 3.16 and 3.25.

Figures 3.4 and 3.4 show the results of the numerical optimization using the Global Optimization

Toolbox of MATLAB, for the setup parameters presented in Table 3.2.

First, let us stress that from Section 3.3.2.1 we have learned that the optimal FIM determinant is

equal to m2

4σ4 , when imposing no constraints on the inputs functions. That sets an upper bound on the

performance we are able to achieve in this case, where we imposed certain constraints on the input

functions. In numbers, this means that, in best case, the FIM determinant for any of the trajectories

found in the numerical procedure is equal to m2

4σ4 .

Plot p0 [m] ψ0 [rad] ∆t [s] m c v [m/s] rlb [o/s] rub [o/s] σ4|FIM |

Figure 3.4a [ 10 0 ] π2 1 16 1 1.5 −20 20 64.00

Figure 3.4b [ 10 0 ] π2 1 16 1 1.5 −40 40 64.00

Figure 3.5a [ 10 0 ] 0 1 16 1 1.5 −20 20 38.00

Figure 3.5b [ 10 0 ] 0 1 16 1 1.5 −40 40 59.30

Table 3.2: Setup parameters used in the numerical optimization of the FIM determinant and the value ofthe FIM determinant obtained for each solution.

−10 0 10 20−10

−5

0

5

10

15

20

Northing[m

]

Easting [m]

BeaconOptimal trajectoryMeasurement points

(a) |r(t)| ≤ 20 deg/s.

−15 −10 −5 0 5

0

5

10

15

20

Northing[m

]

Easting [m]


(b) |r(t)| ≤ 40 deg/s.

Figure 3.4: Trajectories that maximize the FIM determinant resulting from the numerical optimizationwith the parameters defined in Table 3.2 - with ψ0 = π

2 .

From a combined analysis of Table 3.2 and Figures 3.4a and 3.4b, we can see that, in this two cases,

the constraints imposed on the input functions did not degrade the performance of the navigation system

- both trajectories yield σ4|FIM | = 64.

On the other hand, combining the information in Table 3.2 and Figures 3.5a and 3.5b, we see that, for

ψ0 = 0, the constraints imposed on the input functions, constrain the vehicle maneuverability in such a

way that it already degrades the navigation system performance (in comparison to the optimal situation).

Regarding the shape of the trajectories, neither of them is a perfect circumference around the bea-

con. They may result from rotations, and point reflections of the basis solution that yields the measure-

ment points radially distributed around the beacon or they may explore other families of solutions that

were not characterized in the previous section.

27

−5 0 5 10 15 200

5

10

15

20Northing[m

]

Easting [m]


(a) |r(t)| ≤ 20 deg/s.

−10 −5 0 5 10 15−5

0

5

10

15

20

Northing[m

]

Easting [m]


(b) |r(t)| ≤ 40 deg/s.

Figure 3.5: Trajectories that maximize the FIM determinant resulting from the numerical optimizationwith the parameters defined in Table 3.2 - with ψ0 = 0.

3.3.3 Minimizing energy consumption

In this section we will derive trajectories that simultaneously maximize the FIM determinant and

minimize the energy consumption along the trajectory. Thus, this Section should be understood as

continuation of the previous Section.

Since we intend to obtain general results, applicable to a wide variety of vehicles, the energy criterion

we will use is based on very general principles that are in accordance with the kinematic model used in

this chapter.

Assume that the energy consumption while the vehicle is moving in mainly used to overcome the

drag forces FD and drag torques τD. Furthermore assume that

|FD| ∝ |v|2 (3.43)

|τD| ∝ |r|2 (3.44)

where v and r denote the speed and yaw rate, respectively.

Additionally, from the principles of mechanics we borrow that the power of these drag forces and

torques, if v and r are constant, are given by

|PF | = |FD| |v| ∝ |v|3 (3.45)

|Pτ | = |τD| |r| ∝ |r|3 (3.46)

Neglecting the additional energy spent in changing the yaw rate, the basic model of energy con-

sumption that we derived is given by

P (t) = β |v|3 + γ |r(t)|3, β, γ > 0 (3.47)

Because the speed is a parameter and not an optimization variable the term involving v in the power

28

function can be discard in the optimization. Then, because optimizing some generic function α f(x)

yields the same result as optimizing f(x) the model parameter γ can be dropped. Therefore, the energy

criterion for the optimization is given by

E =

tf∫0

|r(t)|3dt (3.48)

which, for the piece-wise constant yaw rate function, is transformed into

E = c∆t

N−1∑j=0

|rj |3 − (Nc−m) ∆t |rN−1|3 (3.49)

Combining the energy criterion with the FIM determinant criterion in a single optimization problem

results on a multicriteria optimization problem, introduced in Section 2.2. In mathematical terms the

problem is formulated as

maxr0,...,rN−1

( |FIM | , −E ) (3.50)

subject to

rlb < rk < rub, for k = 0, ..., N − 1 (3.51)

with |FIM | defined as function of r by Equations 3.16 and 3.25, and E defined by Equation 3.49.

To solve this optimization problem we resorted to numerical methods, in specific we used the Multi-

criteria Optimization Toolbox from MATLAB.

45 50 55 60 650

0.05

0.1

0.15

0.2

0.25

σ4|FIM |

Energy

Pareto Curve

0 5 10 15

−10

−5

0

5

Easting [m]

Northing[m

]

Trajectories

Beacon

Figure 3.6: Pareto curve and the trajectories corresponding to each optimal solution (in the same color)for the problem of minimizing the FIM determinant and the energy consumption by the vehicle.

p0 [m] ψ0 [rad] ∆t [s] m c v [m/s] rlb [o/s] rub [o/s]

[ 5 0 ] π2 1 16 1 1.5 −20 20

Table 3.3: Setup parameters used in the optimization of the FIM determinant and the energy consump-tion by the vehicle, which results are reproduced in Figure 3.6.

Figure 3.6 shows the results of the optimization for the parameters in 3.3. The plot in the left side

shows the Pareto optimal solutions and a Pareto curve, obtained by connecting the solutions points

29

(blue line). The plot in the right side shows the trajectories associated to each of the Pareto solutions

showed in the left-side plot, using the same color. Notice that blue represents trajectories with less

energy consumption while red represents trajectories with bigger energy consumption by the vehicle.

The trade-off between the two objectives of the optimization is very clear:

• Minimizing the energy consumption leads directly to the reduction of the absolute values of the

yaw rate. This decreases the vehicle ability to circumnavigate the beacon, which is reflected in

lower values of the FIM determinant.

• Nevertheless, note (in the left-side plot) that the FIM determinant reaches its maximum value(mσ2

)2= 64

σ4 while the energy function continues increasing its value. This means that there is a

point where increasing the absolute value of the yaw rate does not improve the performance of the

navigation system.

3.3.4 Minimizing the deviation from a nominal trajectory

Figure 3.7: Schematic of the problem of minimizing the deviation from a nominal trajectory while maxi-mizing the FIM determinant, simultaneously.

In a real situation, we expect that the underwater vehicle is performing some mission while it tries to

estimate its position. Thus, we will study the case where the vehicle should follow a nominal trajectory

but it is given some freedom so that it can improve the position estimate accuracy. As depicted in Figure

3.7, we want to find other trajectories (in blue) with the same points of departure and arrival as the

nominal trajectory (in green), but that with minimal deviation from the later improve the accuracy of the

position estimate.

As in the later Section, we have a multicriteria optimization: we still want to maximize the FIM de-

terminant and simultaneously we want to minimize the deviation from the nominal trajectory. In order

to define the nominal and optimal trajectories the parameterization introduced in Section 3.3.1 is used.

Besides, all the assumptions enumerated in Section 3.3.2.2 hold.

30

Lets denote the nominal trajectory by pn(t) : [ 0, tf ]→ R2 and the actual trajectory by p(t) : [ 0, tf ]→

R2, both defined in the NED reference frame {I}. The deviation from the nominal trajectory is defined

as the integral of the squared position error along the trajectory, this is,

δ =

tf∫0

‖pn(t)− p(t)‖2 dt (3.52)

In this problem, because we want to be able to control how much freedom we give the vehicle to

improve the localization accuracy we will use a scalar optimization procedure, weighting the two objective

functions. Additionally, in order to give the vehicle enough maneuverability so that it can deviate from

the nominal trajectory but still reach the point of arrival of the nominal trajectory at the time of arrival, the

speed, constant during the motion, will also be a variable of the optimization. Formally, the optimization

problem is posed as

maxr0,...,rN−1,v

µ1 |FIM | − µ2 δ (3.53)

subject to

rlb < rk < rub, for k = 0, ..., N − 1 (3.54a)

0 ≤ v ≤ vub (3.54b)

with µ1 and µ2 weighting factors,|FIM | defined as function of the optimization variables by Equations

3.16 and 3.25, and δ defined as a function of the optimization variables by Equations 3.52 and 3.21.

To (numerically) solve the optimization problem stated above we used the Global Optimization Tool-

box from MATLAB. The integral in the δ function was discretized using a first order approximation and

an integration step of 0.01 seconds. Additionally, to force the final point of the solution trajectory to be

close-enough to the final point of the nominal trajectory we added an extra cost of 100 to the squared

position error concerning the last points of the two trajectories. This weighting factor was tuned in a

trial-and-error fashion.

Figures 3.8a and 3.8b show two solutions of this optimization procedure for different weighting factors

between the objective functions, µ1 and µ2. The setup parameters are presented in Table 3.4 and the

nominal trajectory is defined by a constant speed vnom, rnom = 0 and ψnom = ψ0.

p0 [m] ψ0 [rad] ∆t [s] m c rlb [o/s] rub [o/s] vub [m/s] vnom [m/s]

[ 10 10 ] π 1 16 1 −20 20 2 1.5

Table 3.4: Setup parameters used in the numerical procedure for maximizing the FIM determinant whileminimizing the deviation from a nominal trajectory. The results are presented in Figures 3.8a and 3.8b.

Obtaining the range measurements along the nominal trajectory, with sampling interval ∆t, yields

σ4|FIM |nom = 50.05, which is already close to the maximum achievable value σ4|FIM |∗ = 64 (for m =

16). Nevertheless, the soft oscillation that we seen in Figure 3.8a is enough to improve the performance

to σ4|FIM |µ1=10,µ2=1 = 52.44. Furthermore, the trajectory in Figure 3.8b allows for the performance

31

−10 0 10 20

−10

0

10

20

Northing[m

]

Easting [m]

BeaconNominal trajectoryOptimal trajectoryMeasurement points

(a) µ1 = 10 and µ2 = 1.

−20 0 20

−10

0

10

20

Northing[m

]

Easting [m]

BeaconNominal trajectoryOptimal trajectoryMeasurement points

(b) µ1 = 20 and µ2 = 1.

Figure 3.8: Trajectories that maximize the FIM determinant while minimizing the deviation from a nominaltrajectory - linear motion.

to reach σ4|FIM |µ1=20,µ2=1 = 63.09, although it deviates considerably from the nominal trajectory. In

practice, the trade-off between the two objective functions depends on the requirements of specific

missions.

3.4 Two vehicles exchanging range information

Figure 3.9: Single-beacon navigation with two vehicles exchanging range information.

In this Section we will tackle the problem of two underwater vehicles navigating with respect to the

same beacon, depicted in Figure 3.9. We will consider the situation where each of the vehicles has

access not only to its own range to the beacon, but also to the distance the between the other vehicle

and the beacon and to the distance between the two vehicles. Given this configuration we will derive

the optimal trajectories that each of the vehicles should perform in order to maximize the accuracy of

the position estimate of both of them. Additionally, we will show that this cooperation may improve the

accuracy of the position estimate with respect to case when each of the vehicles only has access to its

32

own range to the beacon.

3.4.1 System configuration

Consider two vehicles with the kinematic model introduced in Equation 3.2. Let the vehicles and the

beacon positions for t ∈ [0, tf ] to be denoted as follows:

• Vehicle 1 position 1p(t) =[

1pn(t) 1pe(t)]T ;

• Vehicle 2 position 2p(t) =[

2pn(t) 2pe(t)]T ;

• Beacon fixed position given by b = [ bn be ]T .

Without loss of generality, we hold the assumption of the beacon position at b = 0.

Let the between each vehicle and the beacon and between vehicles be denoted as follows:

• Vehicle 1 w.r.t the beacon: 1d(t) = ‖1p(t)‖;

• Vehicle 2 w.r.t the beacon: 2d(t) = ‖2p(t)‖;

• Vehicle 2 w.r.t vehicle 1: 3d(t) = ‖2p(t)− 1p(t)‖.

Consider that as the vehicles move, they get measurements of these distances and consider the follow-

ing measurement models

1zi = 1di + 1wi ,1wi ∼ N (0, 1σ2) (3.55a)

2zi = 2di + 2wi ,2wi ∼ N (0, 2σ2) (3.55b)

3zi = 3di + 3wi ,3wi ∼ N (0, 3σ2) (3.55c)

for i = 0, ...,m− 1, where

• 1zi, 2zi and 3zi denote the measurements samples;

• 1di, 2di and 3di denote samples of the actual distances 1d(t), 2d(t) and 3d(t), respectively;

• 1wi, 2wi and 3wi denote the measurement errors samples;

• and m denotes the number of samples.

3.4.2 Fisher Information Matrix

Let us introduce the vector notation. The following vectors contain information collected along the m

samples:

d =[

1d0 ... 1dm−12d0 ... 2dm−1

3d0 ... 3dm−1

]Tz =

[1z0 ... 1zm−1

2z0 ... 2zm−13z0 ... 3zm−1

]T33

In accordance with the measurement models defined in 3.55, the measurement vector z follows a

3m-dimensional normal distribution with average d and covariance Σ as defined below:

z ∼ N ( d , Σ ) (3.56)

with

Σ =

1σ2 Im×m 0m×m 0m×m

0m×m2σ2 Im×m 0m×m

0m×m 0m×m3σ2 Im×m

Thus, the likelihood function for the measurement vector z with respect to the unknown initial posi-

tions of the vehicles 1p0 and 2p0 is given by 3.57 from which we derive the log-likelihood function given

by Equation 3.58.

L(1p0,2p0) = (2π)−

3m2 |Σ|−

12 exp

{−1

2

(z− d(1p0,

2p0))T

Σ−1(z− d(1p0,

2p0))}

(3.57)

lnL(1p0,2p0) = −3m

2ln(2π)− 1

2ln |Σ| − 1

2

(z− d(1p0,

2p0))T

Σ−1(z− d(1p0,

2p0))

(3.58)

We proceed to compute the jacobian of the log-likelihood function with respect to 1p0 and 2p0 given

by

J(1p0,2p0) lnL(1p0,2p0) =

[J1p0

lnL(1p0,2p0) J2p0

lnL(1p0,2p0)

]∈ R1×4 (3.59)

with

Jjp0lnL =

(z− d

)TΣ−1(Jjp0

d), j = 1, 2

and

(Jjp0

d)

=

∂1d0

∂jpn0· · · ∂1dm−1

∂jpn0

∂2d0

∂jpn0· · · ∂2dm−1

∂jpn0

∂3d0

∂jpn0· · · ∂3dm−1

∂jpn0

∂1d0

∂jpe0· · · ∂1dm−1

∂jpe0

∂2d0

∂jpe0· · · ∂2dm−1

∂jpe0

∂3d0

∂jpe0· · · ∂3dm−1

∂jpe0

T

∈ R3m×2, j = 1, 2

where

∂1di∂1pn0

=1pni1di

∂1di∂1pe0

=1pei1di

∂2di∂2pn0

=2pni2di

∂2di∂2pe0

=2pei2di

34

∂3di∂1pn0

= −2pni − 1pni

3di

∂3di∂1pe0

= −2pei − 1pei

3di

∂3di∂2pn0

=2pni − 1pni

3di

∂3di∂2pe0

=2pei − 1pei

3di

∂2di∂1pn0

=∂2di∂1pe0

=∂1di∂2pn0

=∂1di∂2pe0

= 0

for i = 0, ...,m− 1.

The Fisher Information Matrix for the problem can now be derived:

FIM(1p0,2p0) = E

{(J(1p0,2p0) lnL(1p0,

2p0))T (

J(1p0,2p0) lnL(1p0,2p0)

)}= E

{((z− d

)TΣ−1

[J1p0

d J2p0d])T ((

z− d)T

Σ−1[J1p0

d J2p0d])}

=[J1p0

d J2p0d]T

Σ−1[J1p0

d J2p0d]∈ R4×4 (3.60)

which can be rewritten as

FIM(1p0,2p0) =

F11 F12 F13 F14

F12 F22 F23 F24

F13 F23 F33 F34

F14 F24 F34 F44

(3.61)

with

F11 =1

1σ2

m−1∑i=0

(1pni1di

)2

+1

3σ2

m−1∑i=0

(2pni − 1pni

3di

)2

(3.62a)

F12 =1

1σ2

m−1∑i=0

(1pni1di

)(1pei1di

)+

13σ2

m−1∑i=0

(2pni − 1pni

3di

)(2pei − 1pei

3di

)(3.62b)

F13 = − 13σ2

m−1∑i=0

(2pni − 1pni

3di

)2

(3.62c)

F14 = − 13σ2

m−1∑i=0

(2pni − 1pni

3di

)(2pei − 1pei

3di

)(3.62d)

F22 =1

1σ2

m−1∑i=0

(1pei1di

)2

+1

3σ2

m−1∑i=0

(2pei − 1pei

3di

)2

(3.62e)

35

F23 = F14 (3.62f)

F24 = − 13σ2

m−1∑i=0

(2pei − 1pei

3di

)2

(3.62g)

F33 =1

2σ2

m−1∑i=0

(2pni2di

)2

+1

3σ2

m−1∑i=0

(2pni − 1pni

3di

)2

(3.62h)

F34 =1

2σ2

m−1∑i=0

(2pni2di

)(2pei2di

)+

13σ2

m−1∑i=0

(2pni − 1pni

3di

)(2pei − 1pei

3di

)(3.62i)

F44 =1

2σ2

m−1∑i=0

(2pei2di

)2

+1

3σ2

m−1∑i=0

(2pei − 1pei

3di

)2

(3.62j)

The FIM determinant is given by Equation 3.63 where the terms are defined in Equations 3.62.

|FIM(1p0,2p0)| = F 4

14 − 2F12 F13 F24 F34 + F 212 F

234 − F11

(F 2

24 F33 + F22 F234

)+

+(F11 F22 − F 2

12

)F33 F44 − F 2

14 ( 2F13 F24 + F22 F33 + 2F12 F34 + F11 F44 ) +

+ 2F14 (F12 F24 F33 + F13F22 F34 + F11 F24 F34 + F12 F13 F44 ) + F 213

(F 2

24 − F22 F44

)(3.63)

3.4.3 Trajectory Optimization

Given the problem configuration introduced in the previous section, we want to find what kind of

trajectories should the two vehicles follow in order to maximize the FIM determinant.

Similarly to what we have done in Section 3.3.2.1, in this optimization we will impose no constraints

on the input functions. Thus, we will first derive the conditions that the locations where the range mea-

surements are obtained should respect so that the FIM is optimal. Then, we will try to find trajectories

that fit those points.

Mathematically, the optimization problem is formulated simply as

max |FIM(1p0,2p0)| (3.64)

with |FIM(1p0,2p0)| given by equation 3.63.

The optimality conditions, solution of the problem defined before, are given by Equations 3.65 ( for

the derivation see Appendix A).

m−1∑i=0

(1pni1di

)2

=

m−1∑i=0

(1pei1di

)2

=m

2(3.65a)

m−1∑i=0

(2pni2di

)2

=

m−1∑i=0

(2pei2di

)2

=m

2(3.65b)

36

m−1∑i=0

(2pni − 1pni

3di

)2

=

m−1∑i=0

(2pei − 1pei

3di

)2

=m

2(3.65c)

m−1∑i=0

(1pni1di

)(1pei1di

)= 0 (3.65d)

m−1∑i=0

(2pni2di

)(2pei2di

)= 0 (3.65e)

m−1∑i=0

(2pni − 1pni

3di

)(2pei − 1pei

3di

)= 0 (3.65f)

We can easily see that the conditions that concern each of the vehicles alone (Equations 3.65a,3.65b,

3.65d and 3.65e) are the same the conditions obtained for the problem concerning one single vehicle.

This may indicate the same type of trajectories that we have seen to be solution of the problem for one

single vehicle in Section 3.3.2.1, may also be solution of this problem. Lets investigate in what conditions

these trajectories fulfill the remaining conditions (Equations 3.65c and 3.65f).

Figure 3.10: Geometric insight on the two vehicles problem.

To help us understand the geometric structure of the problem we introduce Figure 3.10. Note that

1pni1di

= cos(1αi)1pei1di

= sin(1αi)

2pni2di

= cos(2αi)2pei2di

= sin(2αi)

2pni − 1pni3di

= cos(1αi + 3αi)2pei − 1pei

3di= sin(1αi + 3αi)

Then it becomes clear that if 3αi = kπ, k ∈ Z for all i ∈ {0, ...m− 1}, the conditions given by the

Equations 3.65c and 3.65f are fulfilled as long as the conditions that concern vehicle 1 are fulfilled

(Equations 3.65a and 3.65d). That corresponds to requiring that the two vehicles obtain the range

measurements at the same radials, as depicted in Figure 3.11.

This solution detains the same properties as the solution of the problem with one vehicle only, it

37

Figure 3.11: Schematic of the solution for the two vehicles problem.

is invariant to rotations around the beacon and the optimality is not dependent on the distance of the

vehicles to the beacon, and neither on the distance between the vehicles.

Figure 3.12: Solution for the 2 vehicles problem - circular motion.

In an analogous fashion to the problem considering one single vehicle, in Figure 3.12 we represent

the solution that renders the trajectories to be circular. If the vehicles have the same turning rate we

38

have the following relation for the speed and the radius of the two trajectories

1v1d

=2v2d

(3.66)

Additionally, a remark has to be made regarding the observability of this system. The optimal FIM de-

terminant gives us a measure of the degree of observability of this system composed by the two vehicles

sharing range information, that we can use to compare with optimal FIM determinant for 1 vehicle only.

For that, let the variance of the range measurements 1di, 2di and 3di, for all i ∈ {0, ...m− 1}, be the same

and equal to σ2, the variance of the range measurements in the problem of one vehicle. Then the optimal

value of the FIM determinant is |FIM(1p0,2p0)|∗ =

(34mσ2

)2. Given the optimal FIM determinant for the

problem of 1 vehicle alone, |FIM(p0)|∗ =(m

2σ2

)2, we can see that |FIM(1p0,2p0)|∗ > 2|FIM(p0)|∗,

which means that the cooperation between the two vehicles improves slightly the degree of observability

of the the system.

39

Chapter 4

3D single-beacon navigation based on

range measurements

In this chapter we extend the problem of single-beacon navigation using range information to the

3-dimensional space. We follow a very similar approach to the one used in the previous chapter for 2D,

with the required adaptations for 3D. To avoid repeating all the derivations, given that the logic behind

it and the procedures are the same, we use the same notation for 3D as used in 2D and refer to the

results obtained before. When necessary, the difference between 2D and 3D is highlighted.

First, we introduce the 3D model of the underlying system in Section 4.1. Then, we derive the FIM

and its determinant in Section 4.2. In Section 4.3, we proceed to find the 3D trajectories that maximize

the FIM determinant. Finally, in Section 4.4, we consider the case where depth measurements are

available and study optimal trajectories under this assumption.

4.1 System model

Consider the system introduced in Section 3.1. Let us introduce the 3D version of this system, with

the proper modifications.

Similarly to the 2D system, the 3D system consist of a vehicle moving while measuring its distance

d(t) with respect to a single beacon. In 3D the trajectory of the vehicle is controlled not only by v(t)

and the r(t), but also by the flight-path-angle γ(t), during the interval [0, tf ]. The flight-path angle is

measured with respect to the horizontal plane and is positive when the vehicle is diving. The beacon

remains located at the origin of the inertial reference frame {I} (NED oriented), thus b0 = 0. The vehicle

continues to be able to measure the heading angle.

Let the vehicle position in {I} be denoted by p =[pn pe pd

]T ∈ R3 \ {0}. Note that the distance

between the vehicle and the beacon is still given by the Euclidean norm of p, given in Equation 3.1.

The new kinematic model of the system with state x(t) =[pn(t) pe(t) pd(t) ψ(t)

]T , input u(t) =

41

[ v(t) r(t) γ(t) ]T and output y(t) = [ d(t) ψ(t) ]

T for t ∈ [ 0, tf ] is given by

x(t) =

v(t) cos(γ(t)) cos(ψ(t))

v(t) cos(γ(t)) sin(ψ(t))

v(t) sin(γ(t))

r(t)

(4.1)

y(t) =

‖p(t)‖

ψ(t)

(4.2)

4.2 Fisher Information Matrix

Similarly to the 2D case, let zi and di with i = 0 , ... , m− 1 denote a set of m measurements of d(t),

corrupted by additive noise, and the corresponding actual distances, respectively. These samples are

obtained along different time instants ti. The measurement model continues to be given by Equations 3.5

and 3.5b. Also the vector notation introduced in Section 3.2.1 remains valid, with the obvious distinction

that in this chapter d and di, with i = 0 , ... , m− 1 , refer to the 3D ranges.

Since the range measurements, contained in the vector z, follow an m-dimensional Gaussian dis-

tribution with average d and covariance R, the likelihood function of the measurements with respect to

the unknown initial position p0 is still given by Equation 3.8. Thus, the log-likelihood function is given by

Equation 3.9 and its jacobian with respect to p0 by Equation 3.10, with Jp0d now given by

(Jp0d) ,

∂d0

∂pn0

∂d0

∂pe0

∂d0

∂pd0...

......

∂dm−1

∂pn0

∂dm−1

∂pe0

∂dm−1

∂pd0

(4.3)

and [∂di∂pn0

∂di∂pe0

∂di∂pd0

]=

[pnidi

peidi

pdidi

], i = 0, ...,m− 1 (4.4)

Then, the FIM, defined by Equation 3.11, for the 3D case is given by

FIM(p0) =1

σ2

m−1∑i=0

(pnidi

)2 m−1∑i=0

(pnidi

)(peidi

)m−1∑i=0

(pnidi

)(pdidi

)m−1∑i=0

(pnidi

)(peidi

)m−1∑i=0

(peidi

)2 m−1∑i=0

(peidi

)(pdidi

)m−1∑i=0

(pnidi

)(pdidi

)m−1∑i=0

(peidi

)(pdidi

)m−1∑i=0

(pdidi

)2

(4.5)

42

Thus, the FIM determinant is given by

|FIM(p0)| =m−1∑i=0

(pnidi

)2m−1∑i=0

(peidi

)2 m−1∑i=0

(pdidi

)2

−

(m−1∑i=0

(peidi

)(pdidi

))2−

−m−1∑i=0

(pnidi

)(peidi

)[m−1∑i=0

(pnidi

)(peidi

)m−1∑i=0

(pdidi

)2

−m−1∑i=0

(pnidi

)(pdidi

)m−1∑i=0

(peidi

)(pdidi

)]+

+

m−1∑i=0

(pnidi

)(pdidi

)[m−1∑i=0

(pnidi

)(peidi

)m−1∑i=0

(peidi

)(pdidi

)−m−1∑i=0

(pnidi

)(pdidi

)m−1∑i=0

(peidi

)2]

(4.6)

4.3 Trajectory optimization

This section should be understood as an extension of the study of optimal trajectories for 2D, where

this is an attempt to get closer to reality, by studying the problem in 3D. As before, the performance index

that we use to compare different trajectories is the FIM determinant, but unlike the previous Chapter, no

other cost criteria will be added.

In a similar manner to Chapter 3, we start by approaching the optimization problem considering no

constraints in the input functions, to gain some intuition into the problem. However, we fail to identify a

family of trajectories that fulfill the optimality conditions. Nevertheless, the particular study of cylindri-

cal helices reveals that, for proper choices of the parameters, these helices can achieve achieve very

good performances in terms of the FIM determinant, even when the inputs are constrained to take in

consideration the limitations of the vehicle dynamics.

4.3.1 No constraints on the input functions

By imposing no constraints on the input functions, except that they result in a continuous evolution

of the system states, the optimization problem resumes to finding the locations at which the range

measurements should be obtained in order to maximize the FIM determinant. Then, we attempt to

investigate trajectories that fit the points obtained in the optimization.

Mathematically, the optimization problem can be formulated as

max |FIM(p0)| (4.7)

with FIM(p0) given by Equation 4.6.

Using a similar approach to Section 3.3.2.1, we can find analytical conditions for the measure-

ment points that guarantee optimality. Let us introduce new variables Γ1 ,[pn0d0

...pnm−1

dm−1

]T, Γ2 ,[

pe0d0

...pem−1

dm−1

]Tand Γ3 ,

[pd0d0

...pdm−1

dm−1

]T. Note that

‖Γ1‖2 =

m−1∑i=0

(pnidi

)2

43

‖Γ2‖2 =

m−1∑i=0

(peidi

)2

‖Γ3‖2 =

m−1∑i=0

(pdidi

)2

and

‖Γ1‖‖Γ2‖ cos(Θ12) =

m−1∑i=0

(pnidi

)(peidi

)

‖Γ1‖‖Γ3‖ cos(Θ13) =

m−1∑i=0

(pnidi

)(pdidi

)

‖Γ2‖‖Γ3‖ cos(Θ23) =

m−1∑i=0

(peidi

)(pdidi

)

where Θ12, Θ13 and Θ23 are the angles between Γ1 and Γ2, Γ1 and Γ3 and Γ2 and Γ3, respectively.

Furthermore note that di is the norm of pi =[pni pei p

di

]T , therefore

pnidi

+peidi

+pdidi

= 1 (4.8)

andm−1∑i=0

(pnidi

)2

+

m−1∑i=0

(peidi

)2

+

m−1∑i=0

(pdidi

)2

= ‖Γ1‖2 + ‖Γ2‖2 + ‖Γ3‖2 = m (4.9)

The FIM determinant can be rewritten as a function of the new variables as

|FIM | = ‖Γ1‖2 ‖Γ2‖2 ‖Γ3‖2︸︷︷︸f1

(1− cos2(Θ23)− cos2(Θ12) + 2 cos(Θ12) cos(Θ13) cos(Θ23)− cos2(Θ13)

)︸︷︷︸f2

(4.10)

Because the |FIM | ≥ 0 by construction, and f1 ≥ 0, we can infer that f2 ≥ 0. Therefore the

maximum of |FIM | is given by the product of the maximums of f1 and f2. Due to the constraint imposed

by Equation 4.9 on the variables ‖Γ1‖, ‖Γ2‖ and ‖Γ3‖, it comes clear that f1 is maximum for

‖Γ∗1‖2 = ‖Γ∗2‖2 = ‖Γ∗3‖2 =m

3

To find the maximum of f2 we solve the derivatives of f2 in order to Θ12, Θ13 and Θ23 equal to zero,

given by

∂f2

∂Θ12= −2 sin(Θ12) (cos(Θ12) + cos(Θ13) cos(Θ23)) = 0 (4.11a)

∂f2

∂Θ13= −2 sin(Θ13) (cos(Θ13) + cos(Θ12) cos(Θ23)) = 0 (4.11b)

∂f2

∂Θ23= −2 sin(Θ23) (cos(Θ23) + cos(Θ12) cos(Θ13)) = 0 (4.11c)

44

The only solution of the Equations 4.11 that yields f2 > 0 is

cos(Θ∗12) = cos(Θ∗13) = cos(Θ∗23) = 0

which is the maximum of f2 and yields f∗2 = 1.

Hence, the FIM determinant is maximum for

‖Γ∗1‖2 = ‖Γ∗2‖2 = ‖Γ∗3‖2 =m

3and cos(Θ∗12) = cos(Θ∗13) = cos(Θ∗23) = 0

and the optimal FIM and the optimal FIM determinant are given by

FIM∗ =1

σ2

m3 0 0

0 m3 0

0 0 m3

(4.12)

and

|FIM∗| =( m

3σ2

)3

(4.13)

Similarly to the solution obtained for 2D, the optimal FIM is a diagonal matrix with equal entries, thus,

all eigenvalues have the same value. This implies that the solution that maximizes the FIM determinant

also maximizes its minimum singular value.

In terms of the original variables, the optimality conditions are given by

m−1∑i=0

(pnidi

)2

=m

3(4.14a)

m−1∑i=0

(peidi

)2

=m

3(4.14b)

m−1∑i=0

(pdidi

)2

=m

3(4.14c)

m−1∑i=0

(pnidi

)(peidi

)= 0 (4.14d)

m−1∑i=0

(pnidi

)(pdidi

)= 0 (4.14e)

m−1∑i=0

(peidi

)(pdidi

)= 0 (4.14f)

In order to characterize this solution let βni , βei and βdi denote the angles between the position vector

pi and the axis pointing North, East and Down, respectively. Then, the terms pnidi

, pei

diand pdi

diare equal to

45

cos(βni ), cos(βei ) and cos(βdi ), respectively, and the optimality conditions become

m−1∑i=0

cos2(βni ) =m

3(4.15a)

m−1∑i=0

cos2(βei ) =m

3(4.15b)

m−1∑i=0

cos2(βdi ) =m

3(4.15c)

m−1∑i=0

cos(βni ) cos(βei ) = 0 (4.15d)

m−1∑i=0

cos(βni ) cos(βdi ) = 0 (4.15e)

m−1∑i=0

cos(βei ) cos(βdi ) = 0 (4.15f)

Finally, it becomes clear that, similarly to the solution for 2D, one family of solutions renders the

measurement points to be radially distributed around the beacon. This family of solutions has some

important properties:

i. The optimality of the solution does not depend on the distance to beacon but only on the direction

of the measurement points;

ii. The order at which the measurements are obtained is arbitrary;

iii. The solution remains optimal if all the points suffer an equal but arbitrary rotation centered in the

beacon;

iv. The solution remains optimal if any of the points is reflected with respect to the beacon.

The first two enumerated properties come naturally from the expressions above. To prove the last two

properties first note that we can write the optimality conditions as

m−1∑i=0

pidi

pTidi

=m

3I (4.16)

To prove point (iii) assume that the rotation is given by some rotation matrix R, that satisfies RRT = I.

Then, applying the rotation R to all the measurement points yields

m−1∑i=0

pi

di

pTidi

=

m−1∑i=0

Rpidi

pTi RT

di

= R

(m−1∑i=0

pidi

pTidi

)RT

46

= R(m

3I)RT

=m

3I

thus satisfying the optimality conditions.

To prove point (iv) remember that a reflection with respect to the origin is defined as a linear trans-

formation T (x) = −x. Applying this transformation to any of the measurements points yields

pi pTi = T (pi) (T (pi))

T= −pi (−pi)

T= pi p

Ti

therefore it does not affect the optimality conditions.

Regarding the possible shapes of the trajectories that fulfill these optimality conditions, none of the

usual candidates, lines, circumferences and cylindrical helices, are adequate. This is because to cover

all the optimal measurement points we need, in general, at least two of the three inputs v(t), r(t) and

γ(t) to be free. Identically to what we did in the previous chapter, we are assuming that the range

measurements are obtained at a constant sampling interval ∆t, and so, time is not a free variable.

Nevertheless, in the next section we will show that cylindrical helices, although not optimal, for certain

choices of parameters, v, r and γ, can achieve very good performances in terms of the FIM determinant,

close to optimal.

4.3.2 Cylindrical helices: close-to-optimal trajectories

Now we introduce the special case of cylindrical helices. Because these trajectories yield constant

inputs for all input variables, speed v, yaw rate r and flight-path angle γ, for most vehicles these do not

require any deflection of the control surfaces along the trajectory and, therefore, they are very convenient

for several applications. Besides, in [36] the authors show that the system described in Section 4.1

undergoing these trajectories is observable in a deterministic framework.

A practical scenario where these trajectories associated with a single-beacon navigation system

could be very useful is in the case of a underwater lab attached to the seabed (and used as a beacon

for navigation), associated with a glidder-like vehicle that covers the water column above the lab while it

gathers water samples for pollution monitoring for example [16].

For v(t) = v, r(t) = r and γ(t) = γ, nonzero constants, the measurements points and corresponding

actual ranges are given by

pi = p0 + v

cos(γ)

r(sin(ψ0 + r i∆t)− sin(ψ0))

cos(γ)

r(− cos(ψ0 + r i∆t) + cos(ψ0))

sin(γ) i∆t

(4.17a)

di = ‖pi‖ (4.17b)

for i = 0, ...,m− 1.

47

Finding the helix parameters that maximize the FIM determinant corresponds to solving the following

optimization problem

maxv,r,γ

|FIM(p0)| (4.18)

with |FIM(p0)| defined as a function of the setup parameters (∆t, p0 and ψ0) and optimization variables

(v, r and γ) by Equations 4.6 and 4.17.

No constraints on the input functions

Figures 4.1 and 4.2 show solutions obtained with a numerical optimization procedure from the Global

Optimization Toolbox from MATLAB.

−10

1

−1

0

1

0

0.5

1

Easting [m]Northing [m]

Dep

th[m

]


(a) m = 3.

−3−2

−10

1

−3−2

−10

1

0

1

2


Dep

th[m

]


(b) m = 10.

−2−1

01

−2−1

01

0

1


Dep

th[m

]


(c) m = 50.

Figure 4.1: Trajectories that maximize the FIM determinant taking in consideration the vehicle dynamics- with p0 = [ 1 1 1]T [m], ψ0 = 7π

4 [rad] and ∆t = 1 [s].

Table 4.1 shows the setup parameters that generate each of the trajectories as well as the trajectory

parameters v, r and γ and the ratio between the maximum achievable FIM determinant and the FIM

determinant for the trajectory. A first look at the table reveals that all the trajectories, even with different

setup parameters, achieve very good performances, above 83 percent of the optimal performance. A

more detailed analysis shows that when the number of samples increases, the module of the speed and

yaw rate required to achieve close-to-optimal performance decreases. A simple explanation for that fact

48

−10

0

10

05

1015

20

0

5

10


Dep

th[m

]BeaconOptimal trajectoryMeasurement points

(a) m = 3.

−10

0

10 010

20

0

5

10

Northing [m]Easting [m]

Dep

th[m

]


(b) m = 10.

−10

0

100

10

20

−5

0

5

10


Dep

th[m

]


(c) m = 50.

Figure 4.2: Trajectories that maximize the FIM determinant taking in consideration the vehicle dynamics- with p0 = [ 10 10 10]T [m], ψ0 = 0 [rad] and ∆t = 1 [s].

is that when the number of samples increases, the time that the vehicle has available to circumnavigate

the beacon also increases.

Figure p0 [m] ψ0 [o] ∆t [s] m v [m/s] r [o/s] γ [o] |FIM ||FIM |max

4.1a [ 1 1 1]T 315 1 3 2.94 −0.09 −119.75 0.98

4.1b [ 1 1 1]T 315 1 10 2.09 4.59 −43.14 0.99

4.1c [ 1 1 1]T 315 1 50 0.31 3.54 −7.98 0.99

4.2a [ 10 10 10]T 0 1 3 16.96 −12.69 −76.77 0.83

4.2b [ 10 10 10]T 0 1 10 6.6275 −13.36 −32.51 0.90

4.2c [ 10 10 10]T 0 1 50 2.06 −11.35 −10.76 0.99

Table 4.1: Setup parameters and results of the optimization of helices to maximize the FIM determinant.

Looking at the trajectories in Figures 4.1 and 4.2 and the measurement points there identified in

green, it is interesting to see how all the properties for the optimal measurement points (derived in

Section 4.3.1) can be applied to generate the spatial distribution of the measurement points seen in the

plots.

49

Considering the vehicle dynamics

To take into consideration the vehicle dynamics we use typical values from the MEDUSA AUV to

constrain the input functions. Hence, we solve the optimization problem given by Equation 4.18 subject

to

0 < v < vub (4.19a)

rlb ≤ r ≤ rub and r 6= 0 (4.19b)

− π

2< γ <

π

2and γ 6= 0 (4.19c)

with vub = 2 m/s and −rlb = rub = 20 o/s.

Solutions obtained using a numerical optimization procedure from the Global Optimization Toolbox

of MATLAB are showed in Figures 4.3. Table 4.2 shows the setup parameters and the results of the

optimization for each of the trajectories.

−10

1

−1

0

1

0

0.5

1


Dep

th[m

]


(a) m = 10.

−2−1

01

−2−1

01

0

0.5

1


Dep

th[m

]


(b) m = 20.

Figure 4.3: Trajectories that maximize the FIM determinant taking in consideration the vehicle dynamics- with p0 = [ 1 1 1]T [m], ψ0 = 7π

4 [rad] and ∆t = 1 [s].

Figure p0 [m] ψ0 [o] ∆t [s] m v [m/s] r [o/s] γ [o] |FIM ||FIM |max

4.3a [ 1 1 1]T 315 1 10 0.52 1.14 −20 0.28

4.3b [ 1 1 1]T 315 1 20 0.59 2.42 −19.1 0.99

Table 4.2: Setup parameters and results of the optimization of helices to maximize the FIM determinant,imposing limits on the input functions.

Following on the previous analysis, with no constraints on the input functions, it becomes clear that

the inclusion of these constraints may degrade the performance . Compare the performance obtained

with the trajectory in Figure 4.3b with the performance obtained for the trajectory in Figure 4.3a. Both

are obtained with the same setup parameters p0, ψ0, ∆t and m, but the FIM determinant for the first

is 0.99% of the maximum FIM determinant while for the latter is 0.28%. Naturally, the vehicle dynamics

may not allow for the vehicle to cover all the optimal measurement points within a limited time interval.

50

However, for increasing number of samples, the spatial density of the measurement points increases,

allowing the vehicle to cover a lot more optimal measurement points with the same displacement. Thus,

for a bigger number of samples, even considering the vehicle dynamics, it is possible to find cylindrical

helices that exhibit close-to-optimal performance in terms of the FIM determinant, as in Figure 4.3b.

4.4 A realistic assumption: known depth

The availability of depth measurements can significantly simplify the position estimation by discarding

the need for the vertical position to be estimated through the range information. Fortunately, depth

sensors are quite affordable, when compared to other navigation equipment and, therefore, an option in

many applications. Thus, we will introduce the required modifications on the problem formulation and

then study the optimal trajectories for this scenario.

The output of the system given by Equation 4.1 becomes

y(t) =

‖p(t)‖

ψ(t)

pd(t)

(4.20)

and the FIM becomes similar to the 2D case

FIM(pn0 , pe0) =

1

σ2

m−1∑i=0

(pnidi

)2 m−1∑i=0

(pnidi

)(peidi

)m−1∑i=0

(pnidi

)(peidi

)m−1∑i=0

(peidi

)2

(4.21)

but with di representing the 3D ranges. Therefore, the FIM determinant is given by Equation 3.16 that

we recall here:

|FIM(pn0 , pe0)| = 1

σ4

(m−1∑i=0

(pnidi

)2)(

m−1∑i=0

(peidi

)2)−

(m−1∑i=0

(pnidi

)(peidi

))2

Because, in 3D, the ranges contain information not only about the coordinates we want to estimate

pni and pei , but also about the coordinate pdi , when compared to the 2D problem, in the 3D problem with

known depth we lose ’resolution’. Except for the case when the pdi coordinate is zero for i = 0, ...,m− 1,

where the 3D ranges are equivalent to the 2D ranges. In this case we have the vehicle moving in

same horizontal plane where the beacon is located and the maximum achievable performance (FIM

determinant) and optimal trajectories are the same as for the 2D problem. Thus, the maximum FIM

determinant for the 3D problem with known depth is |FIM |opt =(m

2σ2

)2.

51

4.4.1 Trajectory Optimization

To find the optimal trajectories for the 3D problem with known depth, in any generic conditions, we

will use numerical methods to solve the following optimization problem:

maxr0,...,rN−1,γ0,...,γN−1

1

σ4

(m−1∑i=0

(pnidi

)2)(

m−1∑i=0

(peidi

)2)−

(m−1∑i=0

(pnidi

)(peidi

))2 (4.22)

subject to

rlb ≤ rk ≤ rub and − π

2≤ γk ≤

π

2, for k = 0, ..., N − 1 (4.23)

with pi and di defined as a function of the optimization variables by

k =i

c(integer division) (4.24a)

ψi = ψ0 + c∆t

k−1∑j=0

rj + mod

(i

c

)∆t rk (4.24b)

pi = p0 + v

k−1∑j=0

cos(γj)

rj

[sin(ψ(j+1) c

)− sin (ψjc)

]cos(γj)

rj

[− cos

(ψ(j+1) c

)+ cos (ψjc)

]c∆t sin(γj)

+ v

cos(γj)

rj[sin (ψi)− sin (ψkc)]

cos(γj)

rj[− cos (ψi) + cos (ψkc)]

mod(ic

)∆t sin(γk)

(4.24c)

di = ‖pi‖ (4.24d)

As in th 2D parameterization, this trajectory parameterization is singular for any ri = 0, but the singular-

ities are removable. The workaround on that is analogous to 2D, given in Section 3.3.1.

Examples of optimal trajectories

In Figures 4.4a and 4.4b we show numerical results of the problem defined above, with p0 =

[ 10 10 pd0]T [m], ψ0 = 0 [rad], ∆t = 1 [s], m = 50, v = 1.5 m/s and −rlb = rub = 20 o/s. Figure

4.4a shows the case with pd0 = 10 m, and the performance achieved is |FIM ||FIM |max

= 0.96. On the other

hand, Figure 4.4b shows the case with pd0 = 0 m, and the trajectory depicted achieves the maximum

performance |FIM ||FIM |max

= 1. In the first trajectory is clear that the vehicle should (i) go up to the horizontal

plane where the vehicle is located and (ii) circumnavigate the beacon.

4.4.2 Moving on a plane of constant depth

Considering the problem at hand with the vehicle motion constrained to a plane of constant depth,

it resembles a lot the 2D problem approach in the last chapter. Most important, it covers several prac-

tical scenarios that require the AUVs to move at approximately constant depth, like, for instance, in

oceanographic surveys the AUVs should move in a plane parallel to the seabed.

52

−30−20−10 010 −10 0 10 20

0

5

10


Dep

th[m

]BeaconOptimal trajectoryMeasurement points

(a) pd0 = 10.

−20−10

010 −10

010

20


Dep

th[m

]


(b) pd0 = 0.

Figure 4.4: Trajectories that maximize the FIM determinant in 3D with known depth - with p0 = [ 1010pd0]T

[m], ψ0 = 0 [rad], ∆t = 1 [s], m = 50, v = 1.5 m/s and −rlb = rub = 20 o/s.

Remember the FIM from the previous Section, given by Equation 4.21. Using spherical coordinates

we can rewrite it as

FIM(pn0 , pe0) =

1

σ2

m−1∑i=0

sin2(φi) cos2(αi)m−1∑i=0

sin2(φi) cos(αi) sin(αi)

m−1∑i=0

sin2(φi) cos(αi) sin(αi)m−1∑i=0

sin2(φi) sin2(αi)

(4.25)

If we set φi = φ for i = 0, ...,m− 1 and the motion of the vehicle is constrained to an horizontal plane

and the positions at which the range measurements

FIM(pn0 , pe0) =

sin2(φ)

σ2

m−1∑i=0

cos2(αi)m−1∑i=0

cos(αi) sin(αi)

m−1∑i=0

cos(αi) sin(αi)m−1∑i=0

sin2(αi)

(4.26)

and

|FIM(p0)| = sin4(φ)

σ4

(m−1∑i=0

cos2(αi)

)(m−1∑i=0

sin2(αi)

)−

(m−1∑i=0

cos(αi) sin(αi)

)2 (4.27)

From this we retrieve the solutions for the 2D problem, but constrained to a circumference of radius

pn sin(φ), with the FIM determinant scaled by sin4(φ). Thus, the performance of the navigation system

is maximum for sin4(φ) = 1, which renders the vehicle to move on the same horizontal plane where

the beacon is located. For any other horizontal plane, the optimal solution consists on a circumference

centered on the vertical projection of the beacon position on that plane and radius as large as possible.

53

Chapter 5

Single-beacon navigation under

uncertainty in the initial position

In this chapter we build and test in simulation an algorithm for single-beacon navigation using range

measurements. This algorithm is composed by a trajectory planner and a state estimator that work on a

loop.

In Section 5.1, we introduce the problem of having some uncertainty associated with the initial es-

timate of the position of the vehicle, that builds upon the previous chapters. Then, in Section 5.2 we

present the navigation algorithm, including the design of a minimum energy estimator for the vehicle

position in Section 5.3, and the trajectory planner in Section 5.4. Finally, in Section 5.5, we present sim-

ulation results of this algorithm for a practical scenario of the use of AUVs and single-beacon navigation

for scientific purposes.

5.1 Uncertainty in the initial position of the vehicle

In the previous chapters we have studied optimal trajectories for single-beacon navigation given a

certain initial position. It may seem contradictory for the reader that we plan optimal trajectories to

estimate the initial position of the vehicle, given a certain initial position for the vehicle. However, that was

just the first step for what we will do in this section. We wanted to start by understanding this problem:

’we don’t know where the vehicle starts, but if it starts at p0 what would be the optimal trajectory’. So that

now, we can go one step ahead and try to understand the real problem: ’if we know that the vehicle initial

position lies inside a region of uncertainty what is the optimal trajectory given that it can be anywhere

inside that region’. We approach this problem for 2D, because further on we assume that we have a

sensor that measures depth.

55

5.1.1 Maximizing the worst case scenario

We approach the problem of obtaining an optimal trajectory for the region of uncertainty by maxi-

mizing the worst case scenario. This uses the same concept as the minimax decision rule, that more

often appears in the context of Decision Theory [54]. An alternative would have been to maximize the

expected value of the FIM determinant inside the uncertainty region. However, we chose this approach

because it gives us a guarantee of a minimum performance, in terms of the FIM determinant.

Let us start by modeling the uncertainty region U . Often, in the stochastic framework, the a-priori

knowledge about the initial state is encoded in an estimate of the initial state x0 and the covariance of

that estimate P0, that translates the uncertainty associated with the initial ’guess’. Translating that into

a region of uncertainty could be achieved by analyzing the eigenvectors and corresponding eigenvalues

of the covariance matrix, that yield, respectively, the direction and a measure of the length of the axes

of the uncertainty ellipsoid, or ellipse in the 2D case. However, that process is rather complex and quite

often, in practice, the information available about the initial estimate is not that precise: we ’guess’ that

vehicle initial position is within a radius r0 of c0. Thus, we model the uncertainty region U as circle of

center c0 = [ cn0 ce0 ] and radius r0. Furthermore, we assume that the probability of the vehicle initial

position lies anywhere inside the circle is uniform.

Then, the problem can be formulated as

maxr

minp0∈U

|FIM(p0, r)| (5.1)

subject to

rlb < rk < rub, for k = 0, ..., N − 1 (5.2)

with |FIM | defined as a function of r and p0 by the Equations 3.16 and 3.25.

Due to the complexity of this problem, we (i) resort to numerical methods to obtain a solution and

(ii) discretize the uncertainty region U into a set of points inside and in the frontier of the circle, that we

denote by U .

Figures 5.1a and 5.2a show examples of trajectories that maximize the minimum FIM determinant

inside the uncertainty region, for different scenarios. The optimal trajectories are shown as if starting

in the center of the uncertainty region, but that is not necessarily the point inside the uncertainty re-

gion where the minimum FIM determinant for the trajectory is achieved. Because the performance of

these trajectories, for different reasons, is far from the maximum theoretical FIM determinant(m

2σ2

)2, we

generated random trajectories for performance comparison, that are shown in Figures 5.1b and 5.2b.

The trajectory shown in Figure 5.1a provides a minimum performance in terms of the FIM determinant

of 0.466σ−4, inside the uncertainty region, where the maximum achievable FIM determinant is 64σ−4.

On the other hand, the random trajectory shown in Figure 5.1b provides a minimum performance in

terms of the FIM determinant of 0.029σ−4. Therefore, (i) although very far from the maximum achievable

performance, the performance provided by the trajectory resulting from the optimization is rather superior

to the one achieved by a random trajectory; (ii) the fact that performance of both ,the trajectory resulting

56

0 50 1000

50

100

150Northing[m

]

Easting [m]

BeaconUOptimal Trajectory

(a) Best trajectory found.

0 50 1000

50

100

150

Northing[m

]

Easting [m]

BeaconURandom Trajectory

(b) Random trajectory.

Figure 5.1: Maximizing the minimum FIM determinant within an uncertainty region, with the uncertaintyregion far from the beacon - c0 = (100, 100) [m], r0 = 20 [m], ψ0 = π [rad], ∆t = 1 [s], c = 1, m = 16,v = 1.5 m/s and −rlb = rub = 20 o/s.

from the optimization procedure and the random trajectory, are so inferior to the maximum achievable

performance is easily explained by the limitations imposed by the vehicle dynamics, that do not allow

the vehicle to cover a wide angular area around the beacon in a few steps.

−20 0 20 40

−10

0

10

20

30

40

50

Northing[m

]

Easting [m]

BeaconUOptimal Trajectory

(a) Best trajectory found.

−20 0 20 40

−10

0

10

20

30

40

50

Northing[m

]

Easting [m]

BeaconURandom Trajectory

(b) Random Trajectory.

Figure 5.2: Maximizing the minimum FIM determinant within an uncertainty region, with the uncertaintyregion enclosing the beacon - c0 = (5, 5) [m], r0 = 20 [m], ψ0 = π [rad], ∆t = 1 [s], c = 1, m = 16,v = 1.5 m/s and −rlb = rub = 20 o/s.

The trajectory shown in Figure 5.2a gets a minimum FIM determinant of 5.668σ−4 inside the un-

certainty region, whereas the random trajectory, in Figure 5.2b, gets a minimum FIM determinant of

0.1127σ−1. Once again, the trajectory obtained from the optimization procedure outscores the random

trajectory, considerably. Considering that the points of the uncertainty region in this scenario ( represent-

ing the possible starting points) are much closer to the beacon than in the first scenario, it facilitates that

the vehicle covers a wider angular area around the beacon in the same number of steps m. That may

explain why the minimum FIM determinant achieved for this scenario is bigger than for the first scenario.

57

5.2 An algorithm for single-beacon navigation

Figure 5.3: Schematic of the navigation algorithm.

In this section we will describe a navigation algorithm for single-beacon navigation using range,

depth and heading measurements. Building upon the previous chapters, and the previous section, the

algorithm includes an optimal trajectory planner and a minimum energy estimator that work on a loop (the

two blue boxes depicted in Figure 5.3). Briefly, as depicted in Figure 5.3, given a certain initial estimate

of the state of the system and corresponding initial covariance matrix, (i) the trajectory planner feeds the

system speed, yaw rate and flight-path angle commands for an optimal trajectory and (ii) the position

estimator observes the outputs of the system (range, depth and heading) and provides an estimate of

the vehicle position and the associated uncertainty. Based on that information the trajectory planner

recomputes the optimal trajectory, restarting the loop. In the picture we make a distinction between the

input commands provided by the trajectory planner and the inputs that are fed to the observer because

the control loop may modify the input commands of the trajectory planner. Throughout this section we

will replace the green box (Motion Control + Vehicle dynamics) by the 3D model of the system. Thus,

the input commands and the inputs fed to the estimator are equivalent.

58

5.3 Minimum-energy state estimator

Consider the system with state x = [ pn pe pd ψ ]T , input u = [ v r γ ]T , output y = [ pd ψ d2 ]T and

the system dynamics given by

x(t) =

v(t) cos(γ(t)) cos(ψ(t))

v(t) cos(γ(t)) sin(ψ(t))

v(t) sin(γ(t))

r(t)

︸︷︷︸

f(x(t),u(t))

(5.3)

y(t) =

pd(t)

ψ(t)

(pn(t))2 + (pe(t))2 + (pd(t))2

︸︷︷︸

h(x(t))

(5.4)

Assume this system is affected by (i) an unknown process disturbance w, that models eventual errors

in the process model, and (ii) an unknown output disturbance v, that models the measurement noise in

the sensors. The later system is described by

x = f(x,u) + w (5.5)

y = h(x) + v (5.6)

Furthermore assume that the range measurements are obtained at constant sampling interval ∆t.

Thus,

d2(ti) = (pn(ti))2 + (pe(ti))

2 + (pd(ti))2, with ti = i∆t and i = 0, ...,m− 1 (5.7)

Let us divide the outputs of the system in continuous outputs, given by

yc(t) =

pd(t)ψ(t)

︸︷︷︸

hc(x(t),u(t))

+vc(t) (5.8)

and the discrete output, given by

yd(ti) = (pn(ti))2 + (pe(ti))

2 + (pd(ti))2︸︷︷︸

hd(x(ti),u(ti))

+vd(ti), i = 0, ...,m− 1 (5.9)

Then, the minimum-energy estimate of the current state x(t) is given by

x(t) ∈ arg minx∈R4

s(x, t) (5.10)

59

with

s(x, t) , minw,vc for τ∈[ 0,t ]vd(ti) for i=0,...,l

{(z(0)− x0)

TQ0 (z(0)− x0) +

1

2

t∫0

wT (τ) Γ w(τ) + vTc (τ)Rc vc(τ) dτ

+1

2

l∑i=0

Rd (vd(ti))2 : z(τ) = f (z(τ),u(τ)) + w(τ),

yc = hc (z(τ),u(τ)) + vc(τ), yd(ti) = hd (z(ti),u(ti)) + vd(ti),

z(t) = x

}(5.11)

where (i) tl corresponds to the time instant of the last discrete measurement before t, (ii) Γ, Rc, Rd > 0

are weighting parameters for the disturbances (an alternative formulation to the one given in Section

2.1.3) and (iii) x0 and Q0 encode the a-priori information about the initial state.

From a combined analysis of [49] and [51] we retrieve the solution of the minimum-energy estimate,

given by

• for ti ≤ t < ti+1, i = 0, ..., l

Q(t) = −Q(t)A(t)−AT (t)Q(t)−Q(t) ΓQ(t) + CTc (t)R−1c Cc(t) (5.12)

˙x(t) = f(x(t),u(t)) +Q−1(t)CTc (t)R−1c

(yc(t)− hc(x(t),u(t))

)(5.13)

• at t = ti+1, i = 0, ..., l − 1

Q(ti+1) = Q(t−i+1) + CTd (t−i+1)R−1d Cd(t

−i+1) (5.14)

x(ti+1) = x(t−i+1) +Q−1(ti+1)CTd (t−i+1)R−1d

(yd(ti+1)− hd(x(t−i+1),u(t−i+1))

)(5.15)

with

A(t) = Jxf(x(t),u(t)) =

0 0 0 −v(t) cos(γ(t)) sin(ψ(t))

0 0 0 v(t) cos(γ(t)) cos(ψ(t))

0 0 0 0

0 0 0 0

(5.16)

Cc(t) = Jxhc(x(t),u(t)) =

0 0 1 0

0 0 0 1

(5.17)

Cd(t) = Jxhd(x(t),u(t)) =[2pn(t) 2pe(t) 2pd(t) 0

](5.18)

Note that the Minimum-Energy Estimator equations are analogous to an Extended Kalman Filter.

However, the latter is formulated in a stochastic framework, assuming additive Gaussian noise, while the

former is derived in a deterministic setting under the assumption that disturbances are only bounded.

This estimator feeds the trajectory planner, in real time, the current state estimate x(t) and Q(t) that

60

we interpreter as the information matrix, from the comparison with the Extended Kalman Filter.

5.4 Trajectory planner

For the trajectory planner we use a similar approach to [28], in the sense that the algorithm consists

of two phases:

i. For a certain, predefined, time interval, the vehicle moves with the single purpose of improving the

initial position estimate. For that we employ the maximization procedure introduced in Section 5.1,

but with the required modifications to include the 3D model.

ii. Given a nominal trajectory that the vehicle should follow, the algorithm provides a trajectory that

do not deviate much from the nominal trajectory, but ensures enough ’excitation’ for the estimation

procedure. For that we employ the optimization procedure introduced in Section 3.3.4, with the

proper modifications to consider the system in 3D, with depth measurements available.

Both phases include replanning the trajectory while the vehicle is moving, which is dependent on the

availability of a new measure of the uncertainty region. Thus, let us first clarify how we obtain a measure

of the uncertainty region.

Uncertainty region

The state estimator provides the state estimate x = [ pn pe pd ψ d2 ]T and the covariance Q−1. From

the state estimate we take the estimates of the north and east position coordinates as the center of the

uncertainty region

c0 = [ pn pe ]T (5.19)

To obtain a measure of the radius of uncertainty we first truncate the information matrix to get the block

corresponding to the north and east position Qn,e

Q =

Qn,e Qcross terms

Qcross terms Qd,ψ

(5.20)

and, then, we take the maximum singular value of the inverse of Qn,e as the uncertainty radius

r0 =

√λmax(Q−1

n,e) (5.21)

Note the uncertainty concerns only the horizontal plane. Hence, the uncertainty region U is given by

U ={

(px, py, pz) : (px − pn)2 + (pe − pe)2 ≤ r0 ∧ pd = pd}

(5.22)

61

Condition to replan

Since between range measurements the uncertainty associated with the north and east coordinates

will, in the best case, remain constant, but most certainly increase, we only consider to replan the

trajectory when new range measurements are available. Even though, replannig the trajectory at every

step would be computationally very expensive and could lead to a very ’nervous’ trajectory. Thus, we

will only replan the trajectory when the uncertainty radius reduces to half or increases to the double.

Additionally, because the trajectories are only planned for m steps, if none of the previous conditions

is verified within m steps of the last trajectory that was planned, the algorithm has to provide another

trajectory as well.

Procedure 1

This procedure is very similar to the one presented in Section 5.1.1. We start by discretizing the

uncertainty region U into a set of points U . Then we use a maxmin optimization to find the trajectory the

maximizes the worst performance over the entire set U .

The procedure returns

arg maxr,γ

minp0∈U

|FIMp0(p0, r,γ)| (5.23)

subject to

rlb ≤ rk ≤ rub, for k = 0, ..., N − 1 (5.24a)

−π2≤ γk ≤

π

2, for k = 0, ..., N − 1 (5.24b)

γk −∆γ ≤ γk+1 ≤ γk + ∆γ, for k = 0, ..., N − 2 (5.24c)

with

|FIMp0(p0, r,γ)| = 1

σ4

(m−1∑i=0

(pnidi

)2)(

m−1∑i=0

(peidi

)2)−

(m−1∑i=0

(pnidi

)(peidi

))2 (5.25)

r = ( r0 , ... , rN−1 ) (5.26)

γ = ( γ0 , ... , γN−1 ) (5.27)

and pi and di defined as a function of the optimization variables by

k = floor

(t

c∆t

)(5.28a)

ψ(t) = ψ0 + c∆t

k−1∑j=0

rj + (t− k c∆t) rk (5.28b)

62

p(t) = p0 + v

k−1∑j=0

cos(γj)rj

(sin(ψ((j + 1) c∆t))− sin(ψ(j c∆t)))

cos(γj)rj

(− cos(ψ((j + 1) c∆t)) + cos(ψ(j c∆t)))

sin(γj) c∆t

+ v

cos(γk)rk

(sin(ψ(t))− sin(ψ(k c∆t)))

cos(γk)rk

(− cos(ψ(t)) + cos(ψ(k c∆t)))

sin(γj) (t− k c∆t)

(5.28c)

d(t) = ‖p(t)‖ (5.28d)

for t = i∆t.

If some rl is zero, with l ∈ {0, ..., k}, the function p(t) has a singularity, which is removable. In that

case, if l < k, the term concerning rl in the summation appearing on the expression of p(t) is replaced

by

c∆t v

cos(γl) cos(ψ(l c∆t))

cos(γl) sin(ψ(l c∆t))

sin(γl)

, if l < k (5.29)

or, if l = k, the third term of the expression of p(t) is replaced by

(t− k c∆t) v

cos(γk) cos(ψ(k c∆t))

cos(γk) sin(ψ(k c∆t))

sin(γk)

, if l = k (5.30)

Procedure 2

This procedure is analogous to the one described in Section 3.3.4 for 2D. Consider a nominal tra-

jectory, denoted pnom(t) : [0, tf ] → R3 (with tf ≥ m∆t) and a generic trajectory p(t) : [0,m∆t] → R3,

defined as a function of the inputs r and γ by the parameterization defined above in Equations 5.28; both

trajectories have the same initial conditions p0 and ψ0. The squared error between the two trajectories

for the interval [0,m∆t] is given by

δ =

m∆t∫0

‖pnom(t)− p(t)‖2 dt (5.31)

To simplify the numerical computations we use instead

δ =

L∑n=0

‖pnom(n∆int)− p(n∆int)‖2 (5.32)

with L =m∆t

∆intand ∆int small enough. Additionally, to penalize the deviation at the end of the two

trajectories, we put an extra cost in the last Lpenalty samples. Thus,

δ =

L−Lpenalty−1∑n=0

‖pnom(n∆int)− p(n∆int)‖2 +

L∑n=L−Lpenalty

η‖pnom(n∆int)− p(n∆int)‖2 (5.33)

63

Finally, the procedure returns the input commands provided by

arg minr,γ

µ1|FIMp0(p0, r,γ)|+ µ2 δ (5.34)

subject to Equations 5.24, with |FIMp0(p0, r,γ)| given by Equation 5.25, δ given by Equation 5.33, and

the generic trajectory being optimized given as function of the optimization variable by Equation 5.28.

Flowchart of the algorithm

Figure 5.4: Flowchart of the algorithm for trajectory planning.

To clarify the behavior of the trajectory planner we will go over the flowchart of the algorithm step-by-

step, which is presented in Figure 5.4:

i. When a new range measurement arrives, the observer updates the state estimate and the covari-

ance matrix and feeds that to the planning algorithm;

ii. Using the current state estimate and covariance matrix, the region of uncertainty is computed;

iii. If the radius of uncertainty is half or double of the radius of uncertainty when the last trajectory

was planned (or if the last trajectory was planned m steps before), a new trajectory is planned ;

otherwise it waits for a new range measurement;

iv. If a new trajectory needs to be planned, the algorithm checks in which phase it is: if it is still in the

period of time for phase 1 then it uses procedure 1, otherwise it uses procedure 2;

v. Using procedure 1, the algorithm provides a trajectory to reduce the uncertainty in the estimate

of the vehicle position, for the next m steps, despite the goals of the mission;

64

vi. Using procedure 2, the algorithm provides a trajectory that is a trade-off between following a

nominal trajectory (for mission purposes) and ensuring enough excitation for the position estimator.

5.5 A practical scenario: simulation results

One of the goals in underwater scientific research is to have, in a near future, several underwater

labs for monitoring the biodiversity, data collection in general or in-site experiments, to name a few

applications. An example of these labs is the already existing NOOA Aquarius Reef Base [55], in Florida

U.S.A.

Currently, we still know very little about deep-water environments and using these underwater labs in

those regions would be one important step in the direction of fulfilling that gap of knowledge. However,

one practical difficulty in that scenario is the access to those labs, since using human divers is impracti-

cal. Fortunately, the advances in AUV technology allow for AUVs to take care of the interaction between

the researchers on the surface and the underwater lab. This is the scenario where we propose to test,

in simulation, the navigation algorithm.

Figure 5.5: Schematic of the scenario used for testing the navigation system.

Figure 5.5 depicts the scenario that we used to test the navigation algorithm, in simulation. In detail,

the scenario consists of:

• A underwater lab, at 1000 meters below the sea surface with an acoustic beacon for navigation;

• An AUV that is launched at the see surface with an initial estimate of the state x0 and covariance

Q−10 . Note that the estimate of the position of the vehicle is given with respect to a NED reference

frame with origin at the lab;

65

• The AUV homes into the lab and docks there to collect the available data. For that it uses the

navigation algorithm developed in this chapter, by measuring its range to the lab.

• To complete the phase 1 of the algorithm, the vehicle has 5 minutes, in which it should improve the

position estimate;

• For the phase 2 the nominal trajectory consists of three straight lines as depicted in Figure 5.5:

– one vertical part, that covers half of the remaining vertical distance to the lab;

– one part with constant flight-path angle and constant heading angle;

– and, finally, one horizontal part with the same constant heading as the previous part, that

covers half of the horizontal distance to the lab when phase 2 begins.

• The vehicle moves at constant speed v = 1.5 m and the limits on the yaw rate and changing in the

flight path angle are −rlb = rub = 20 ◦/s and ∆γ = 5 ◦.

For the simulation it is assumed that the ranges are corrupted by zero-mean Gaussian noise of

variance 10 m2. The observer parameters are Γ = 0, Rc = diag([ 0.01 1 ]) and Rd = 0.01. The

estimates for north and east coordinates are initialized 100 m apart from the true coordinates and the

initial covariance is Q−10 = diag([ 100 100 10 1 ]).

Figures 5.6 and 5.7 show the trajectory and other results from the simulation.

Figure 5.6 shows the real trajectory of the vehicle in blue, the estimated trajectory in red and the

nominal trajectory in green, that starts only in the vertical part of the trajectories in the plot. The phase

1 of the algorithm corresponds to the first horizontal part of the trajectory. It is clear that the vehicle

circumnavigates the beacon while going downwards, as expected from the results obtained in Section

4.4. The phase 2 starts in the vertical part of the trajectory. The oscillations around the nominal trajectory

are more perceptible in the final horizontal part of the trajectory. Straight line trajectories directed to the

beacon, as the final part of the nominal trajectory, render the FIM determinant equal to zero. That is why,

in that part, the vehicle has to deviate more from the nominal trajectory to sufficiently excite the estimator.

Figures 5.7a and 5.7b show the estimation errors in the north and east coordinates, respectively, along

the simulation. Both plots show that the estimates converge to a neighborhood of the true position. A

closer look to the last seconds of the simulation reveals that when the position estimate indicates that the

vehicle reached the lab, the true position is 20 cm apart (the norm of the error vector). If more precision

is required for docking, short-range auxiliary systems can be used, such as cameras.

Figure 5.7c shows the determinant of the Cramér-Rao Lower Bound (CRLB) (the inverse of the

optimal FIM) and the determinant of the covariance of the estimation error concerning the north and

east coordinates. It is evident that the latter does not reach the former. However, the difference between

the two decreases in last seconds of simulation, when the vehicle reaches the horizontal plane of the

beacon and starts moving around it. Two possible explanations for the fact that the CRLB is not reached

are (i) the fact that there is no guarantee that the minimum is an efficient estimator for this system and

(ii) due to the uncertainty in the initial estimate and the limitations imposed by the vehicle dynamics, the

66

0200

400600

0200

400600

800

−1000

−800

−600

−400

−200

0


Depth

[m]

Nominal TrajectoryTrajectoryEstimated trajectoryBeacon

Figure 5.6: Trajectory obtained in simulation.

trajectory that the planner provides may not be able to reach the maximum performance in terms of FIM

or CRLB determinant.

67

0 500 1000 1500−120

−100

−80

−60

−40

−20

0

Northingerror[m

]

Time [s]

(a) Estimation error in the north coordinate.

0 500 1000 1500−100

−80

−60

−40

−20

0

20

Eastingerror[m

]

Time [s]

(b) Estimation error in the east coordinate.

0 500 1000 150010

−10

10−5

100

105

Time [s]

Covariance determinantCRLB determinant

(c) Performance Cramér-Rao Lower Bound.

Figure 5.7: Simulation results

68

Chapter 6

Conclusions and Future Work

6.1 Conclusions

This thesis approached the problem of single-beacon navigation using range measurements. Since

the working principle of the system requires that the vehicle, that is estimating its position, moves to

obtain range measurements at different locations, we focused on optimizing the vehicle trajectory to

improve the accuracy of the position estimate. To measure the accuracy of the position estimate that a

certain trajectory provides, the determinant of FIM was used.

First we tackled the problem for 2D, assuming that the initial position and heading are known. A

family of solutions was derived, analytically, consisting only of the spatial location of the optimal points at

which the range measurements should be obtained. The geometry of these solutions was fully charac-

terized and several properties were derived. One of these solutions consists of having the measurement

points radially distributed around the beacon. Thus, when the vehicle dynamics allows the vehicle to

go from one point to the next in one sampling interval, an optimal trajectory is a circumference around

the beacon. When the vehicle dynamics is considered and the maximum theoretical performance can-

not be achieved, examples of optimal trajectories, given the constraints, were obtained using numerical

procedures.

Still for the 2D problem, we studied the trade-off between maximizing the accuracy of the position

estimate and minimizing the energy consumption by the vehicle. The results show that the general trend

is that, reducing the energy consumption by the vehicle decreases the maneuverability, leading to a worst

performance in terms of the determinant of the FIM. Furthermore, we design a procedure for planning

optimal trajectories that are a trade-off between following as close as possible some nominal trajectory

and maximizing the FIM determinant. It was shown that, for most scenarios, the performance in terms

of the determinant of the FIM can be improved with minimal deviation from the nominal trajectory.

Moreover, the problem of two vehicles using the same beacon for navigation and exchanging range

information was studied in 2D. Analytical results show that the optimal trajectories for the two vehicles are

parallel and circumnavigate the beacon. Furthermore, the results, in terms of the maximum determinant

of the FIM, indicate that the cooperation between the two vehicles improves the performance slightly.

69

The 3D problem was analyzed next, and a family of optimal solutions for the locations of the mea-

surement points was obtained analytically. The general form of these solutions corresponds to having

the measurement points radially distributed around the beacon, but a few transformations are valid (as

in 2D). We failed to identify a family of trajectories that fits all the optimal measurement points, inde-

pendently from the number of points (samples) that are considered. Nevertheless, using numerical

procedures, we showed that cylindrical helices, for a proper choice of the parameters, achieve close-

to-optimal performances. Afterwards, the 3D problem considering depth measurements available was

tackled, which discards the need for the vertical coordinate of the vehicle position to be estimated from

the range measurements. An analysis resorting to analytical and numerical tools showed that the opti-

mal trajectory for that scenario consist of a circumference in the same horizontal plane where the beacon

is located. When considering the case of the vehicle motion constrained to an horizontal plane, that is

not the one where the beacon is located, the optimal trajectory consists of a circumference with the

largest possible radius.

Next, we approached the problem of having some uncertainty associated to the initial position esti-

mate. A maxmin optimization procedure was used to provide trajectories that maximize the minimum

performance (in terms of the determinant of the FIM), if the vehicle starts anywhere inside the uncer-

tainty region. The resulting trajectories were compared with randomly generated trajectories, proving to

be significantly better.

At last, a 3D navigation algorithm, that combines an optimal trajectory planner with a minimum-

energy estimator, was developed. The trajectory planner includes (i) a first phase with the single purpose

of reducing the uncertainty of the position estimate and (ii) a second phase that provides a trajectory

that is as close as possible to the nominal trajectory for the mission, but simultaneously provides enough

excitation for a good performance of the position estimator. The algorithm was tested in simulation for a

practical scenario and the results indicate that it is appropriated for this scenario and similar ones.

Finally, we want to highlight that, although this thesis focused underwater navigation, this work is

relevant for navigation in other environments where GPS signals are not available and using a similar

navigation system is an option.

6.2 Future work

Using the determinant of the FIM as a performance index, we obtained trajectories that maximize

the accuracy of a position estimate, for an unbiased estimator. There is no guarantee that the minimum-

energy estimator provides unbiased estimates for the system in consideration. Therefore, on a the-

oretical perspective, a possible next step is to design an estimator that can ensure that the estimate

converges to the true position. It would also be interesting to compare results obtained with different

estimators, using optimal trajectories as defined in this thesis.

On a practical perspective, an experimental validation of the navigation algorithm would be a natural

follow-up to this work, including an experimental analysis of the achievable performance in terms of the

accuracy of the position estimate.

70

References

[1] D. Lewis, We, the Navigators: The Ancient Art of Landfinding in the Pacific. University of Hawaii

Press, 1994. [Online]. Available: http://books.google.pt/books?id=SiCCMB6xQJoC

[2] N. Bowditch, N. Imagery, and M. Agency, The American practical navigator : an epitome of

navigation, ser. Pub. (United States. National Imagery and Mapping Agency). National Imagery

and Mapping Agency, 2002. [Online]. Available: http://books.google.pt/books?id=pXjHDnIE_ygC

[3] K. Mathew, History of the Portuguese Navigation in India, 1497-1600. Mittal Publications, 1988.

[Online]. Available: http://books.google.pt/books?id=Kl3IR3RJTIEC

[4] NSTB/WAAS T&E Team - William J. Hughes Technical Center, “Global positioning system stan-

dard positioning service performance analysis report,” Federal Aviation Administration GPS Product

Team, Tech. Rep., July 2014.

[5] J. W. Nicholson and A. J. Healey, “The present state of autonomous underwater vehicle (auv)

applications and technologies,” Marine Technology Society Journal, vol. 42, no. 1, pp. 44–51,

2008-03-01T00:00:00. [Online]. Available: http://www.ingentaconnect.com/content/mts/mtsj/2008/

00000042/00000001/art00008

[6] J. J. Leonard, A. A. Bennett, C. M. Smith, H. Jacob, and S. Feder, “Autonomous underwater vehicle

navigation,” in MIT Marine Robotics Laboratory Technical Memorandum, 1998.

[7] L. Stutters, H. Liu, C. Tiltman, and D. Brown, “Navigation technologies for autonomous underwater

vehicles,” Systems, Man, and Cybernetics, Part C: Applications and Reviews, IEEE Transactions

on, vol. 38, no. 4, pp. 581–589, July 2008.

[8] W. Kinsey, James C, Eustice, Ryan M, “A survey of underwater vehicle navigation: Recent ad-

vances and new challenges,” in IFAC Conference of Manoeuvering and Control of Marine Craft,

2006.

[9] E. Geyer, P. Creamer, J. D’Appolito, and R. Rains, “Characteristics and capabilities of navigation

systems for unmanned untethered submersibles,” in Unmanned Untethered Submersible Technol-

ogy, Proceedings of the 1987 5th International Symposium on, vol. 5. IEEE, 1987, pp. 320–347.

[10] J. Christensen, “Lbl nav - an acoustic transponder navigation system,” in OCEANS ’79, Sept 1979,

pp. 507–512.

71

http://books.google.pt/books?id=SiCCMB6xQJoC

http://books.google.pt/books?id=pXjHDnIE_ygC

http://books.google.pt/books?id=Kl3IR3RJTIEC

http://www.ingentaconnect.com/content/mts/mtsj/2008/00000042/00000001/art00008

http://www.ingentaconnect.com/content/mts/mtsj/2008/00000042/00000001/art00008

[11] S. Smith and D. Kronen, “Experimental results of an inexpensive short baseline acoustic positioning

system for auv navigation,” in OCEANS ’97. MTS/IEEE Conference Proceedings, vol. 1, Oct 1997,

pp. 714–720 vol.1.

[12] J. Opderbecke, “At-sea calibration of a usbl underwater vehicle positioning system,” in OCEANS

’97. MTS/IEEE Conference Proceedings, vol. 1, Oct 1997, pp. 721–726 vol.1.

[13] A. Alcocer, P. Oliveira, and A. Pascoal, “Study and implementation of an ekf gib-based underwater

positioning system,” Control engineering practice, vol. 15, no. 6, pp. 689–701, 2007.

[14] K. Vickery, “Acoustic positioning systems. a practical overview of current systems,” in Autonomous

Underwater Vehicles, 1998. AUV’98. Proceedings of the 1998 Workshop on. IEEE, 1998, pp.

5–17.

[15] P. H. Milne, Underwater acoustic positioning systems. Gulf Publishing Co., Houston, TX, 1983.

[16] I. F. Akyildiz, D. Pompili, and T. Melodia, “Underwater acoustic sensor networks: research

challenges,” Ad Hoc Networks, vol. 3, no. 3, pp. 257 – 279, 2005. [Online]. Available:

http://www.sciencedirect.com/science/article/pii/S1570870505000168

[17] R. Karlsson, F. Gusfafsson, and T. Karlsson, “Particle filtering and cramer-rao lower bound for

underwater navigation,” in Acoustics, Speech, and Signal Processing, 2003. Proceedings. (ICASSP

’03). 2003 IEEE International Conference on, vol. 6, April 2003, pp. VI–65–8 vol.6.

[18] S. Williams, G. Dissanayake, and H. Durrant-Whyte, “Towards terrain-aided navigation for under-

water robotics,” Advanced Robotics, vol. 15, no. 5, pp. 533–549, 2001.

[19] D. Ribas, P. Ridao, J. Neira, and J. Tardos, “Slam using an imaging sonar for partially structured

underwater environments,” in Intelligent Robots and Systems, 2006 IEEE/RSJ International Con-

ference on, Oct 2006, pp. 5040–5045.

[20] C. Roman and H. Singh, “Improved vehicle based multibeam bathymetry using sub-maps and slam,”

in Intelligent Robots and Systems, 2005. (IROS 2005). 2005 IEEE/RSJ International Conference

on, Aug 2005, pp. 3662–3669.

[21] S. Webster, R. Eustice, H. Singh, and L. Whitcomb, “Preliminary deep water results in single-

beacon one-way-travel-time acoustic navigation for underwater vehicles,” in Intelligent Robots and

Systems, 2009. IROS 2009. IEEE/RSJ International Conference on, Oct 2009, pp. 2053–2060.

[22] M. Larsen, “Synthetic long baseline navigation of underwater vehicles,” in OCEANS 2000

MTS/IEEE Conference and Exhibition, vol. 3, 2000, pp. 2043–2050 vol.3.

[23] R. M. Eustice, H. Singh, and L. L. Whitcomb, “Synchronous-clock, one-way-travel-time acoustic

navigation for underwater vehicles,” Journal of Field Robotics, vol. 28, no. 1, pp. 121–136, 2011.

[Online]. Available: http://dx.doi.org/10.1002/rob.20365

72


http://dx.doi.org/10.1002/rob.20365

[24] A. Gadre and D. Stilwell, “Toward underwater navigation based on range measurements from a

single location,” in Robotics and Automation, 2004. Proceedings. ICRA ’04. 2004 IEEE International

Conference on, vol. 5, April 2004, pp. 4472–4477 Vol.5.

[25] ——, “Underwater navigation in the presence of unknown currents based on range measurements

from a single location,” in American Control Conference, 2005. Proceedings of the 2005, June 2005,

pp. 656–661 vol. 1.

[26] ——, “A complete solution to underwater navigation in the presence of unknown currents based

on range measurements from a single location,” in Intelligent Robots and Systems, 2005. (IROS

2005). 2005 IEEE/RSJ International Conference on, Aug 2005, pp. 1420–1425.

[27] P. Batista, C. Silvestre, and P. Oliveira, “Single range aided navigation and source localization:

Observability and filter design,” Systems I& Control Letters, vol. 60, no. 8, pp. 665 – 673, 2011.

[Online]. Available: http://www.sciencedirect.com/science/article/pii/S0167691111001174

[28] P. Baccou and B. Jouvencel, “Homing and navigation using one transponder for auv, postprocessing

comparisons results with long base-line navigation,” in Robotics and Automation, 2002. Proceed-

ings. ICRA’02. IEEE International Conference on, vol. 4. IEEE, 2002, pp. 4004–4009.

[29] J. C. Hartsfield, “Single transponder range only navigation geometry (strong) applied to remus

autonomous under water vehicles,” 2005.

[30] C. E. LaPointe, “Virtual long baseline (vlbl) autonomous underwater vehicle navigation using a

single transponder,” Ph.D. dissertation, 2006.

[31] M. Chitre, “Path planning for cooperative underwater range-only navigation using a single beacon,”

in Autonomous and Intelligent Systems (AIS), 2010 International Conference on. IEEE, 2010, pp.

1–6.

[32] A. Scherbatyuk, “The auv positioning using ranges from one transponder lbl,” in OCEANS ’95.

MTS/IEEE. Challenges of Our Changing Global Environment. Conference Proceedings., vol. 3, Oct

1995, pp. 1620–1623 vol.3.

[33] M. F. Fallon, G. Papadopoulos, J. J. Leonard, and N. M. Patrikalakis, “Cooperative auv naviga-

tion using a single maneuvering surface craft,” The International Journal of Robotics Research, p.

0278364910380760, 2010.

[34] N. Crasta, M. Bayat, A. P. Aguiar, and A. M. Pascoal, “Observability analysis of 2d single beacon

navigation in the presence of constant currents for two classes of maneuvers,” in Control Applica-

tions in Marine Systems, vol. 9, no. 1, 2013, pp. 227–232.

[35] G. Indiveri and G. Parlangeli, “Further results on the observability analysis and observer design for

single range localization in 3d,” arXiv preprint arXiv:1308.0517, 2013.

[36] N. Crasta, M. Bayat, A. P. Aguiar, and A. M. Pascoal, “Observability analysis of 3d auv trimming

trajectories in the presence of ocean currents using single beacon navigation,” 2014.

73


[37] P. Huxel and T. U. of Texas at Austin, Navigation Algorithms and Observability Analysis

for Formation Flying Missions. University of Texas at Austin, 2006. [Online]. Available:

http://books.google.pt/books?id=r_fcnBYH0i4C

[38] F. Arrichiello, G. Antonelli, A. P. Aguiar, and A. Pascoal, “Observability metric for the relative local-

ization of auvs based on range and depth measurements: theory and experiments,” in Intelligent

Robots and Systems (IROS), 2011 IEEE/RSJ International Conference on. IEEE, 2011, pp. 3166–

3171.

[39] D. Salinas, “Adaptive sensor networks for mobile target localization and tracking,” Ph.D. dissertation,

Universidad Nacional de Educación a Distancia, Spain, 2013.

[40] S. MartíNez and F. Bullo, “Optimal sensor placement and motion coordination for target tracking,”

Automatica, vol. 42, no. 4, pp. 661–668, 2006.

[41] H. Trees, Detection, Estimation, and Modulation Theory (Part I). John Wiley and Sons, 2001.

[42] C. Radhakrishna Rao, “Information and accuracy attainable in the estimation of statistical parame-

ters,” Bulletin of the Calcutta Mathematical Society, vol. 37, no. 3, pp. 81–91, 1945.

[43] Z. H. QURESHI, T. S. NG, and G. C. GOODWIN, “Optimum experimental design for identification

of distributed parameter systems,” International Journal of Control, vol. 31, no. 1, pp. 21–29, 1980.

[Online]. Available: http://www.tandfonline.com/doi/abs/10.1080/00207178008961025

[44] D. C. Kammer, “Sensor placement for on-orbit modal identification and correlation of large space

structures,” Journal of Guidance, Control, and Dynamics, vol. 14, no. 2, pp. 251–259, 1991.

[45] M. Meo and G. Zumpano, “On the optimal sensor placement techniques for a bridge

structure,” Engineering Structures, vol. 27, no. 10, pp. 1488 – 1497, 2005. [Online]. Available:

http://www.sciencedirect.com/science/article/pii/S014102960500146X

[46] D. Ucinski, Optimal Measurement Methods for Distributed Parameter System Identification, ser.

Taylor and Francis series in systems and control. Taylor & Francis, 2004. [Online]. Available:

http://www.google.pt/books?id=OOVH4F8nVUUC

[47] C. Jauffret, “Observability and fisher information matrix in nonlinear regression,” Aerospace and

Electronic Systems, IEEE Transactions on, vol. 43, no. 2, pp. 756–759, April 2007.

[48] O. Hijab, Minimum Energy Estimation. University of California, Berkeley, 1980.

[49] A. J. Krener, “The convergence of the minimum energy estimator,” in New Trends in Nonlinear

Dynamics and Control and their Applications. Springer, 2003, pp. 187–208.

[50] ——, “Minimum covariance, minimax and minimum energy linear estimators,” in Stochastic Control

Theory and Stochastic Differential systems. Springer, 1979, pp. 490–495.

[51] A. P. Aguiar and J. P. Hespanha, “Minimum-energy state estimation for systems with perspective

outputs,” Automatic Control, IEEE Transactions on, vol. 51, no. 2, pp. 226–241, 2006.

74

http://books.google.pt/books?id=r_fcnBYH0i4C

http://www.tandfonline.com/doi/abs/10.1080/00207178008961025

http://www.sciencedirect.com/science/article/pii/S014102960500146X

http://www.google.pt/books?id=OOVH4F8nVUUC

[52] J. T. Isaacs, D. J. Klein, and J. P. Hespanha, “Optimal sensor placement for time difference of

arrival localization,” in Decision and Control, 2009 held jointly with the 2009 28th Chinese Control

Conference. CDC/CCC 2009. Proceedings of the 48th IEEE Conference on. IEEE, 2009, pp.

7878–7884.

[53] A. N. Bishop, B. Fidan, B. Anderson, K. Dogançay, and P. N. Pathirana, “Optimality analysis of

sensor-target localization geometries,” Automatica, vol. 46, no. 3, pp. 479–492, 2010.

[54] S. Russell and P. Norvig, Artificial Intelligence: A Modern Approach, ser. Prentice Hall series in

artificial intelligence. Prentice Hall, 2010. [Online]. Available: http://books.google.pt/books?id=

8jZBksh-bUMC

[55] A. Shepard, D. Dinsmore, S. Miller, C. Cooper, and R. Wicklund, “Aquarius undersea laboratory:

The next generation.” 1996.

75

http://books.google.pt/books?id=8jZBksh-bUMC

http://books.google.pt/books?id=8jZBksh-bUMC

Appendices

77

Appendix A

Solution of the two vehicles problem

Consider the optimization problem given by Equation 3.64, where the objective function, the FIM

determinant, is given by Equation 3.63. In order to find the maximum of the objective function we will

derive the 1st order optimality conditions. But first let us introduce a few new variables that will simplify

the resolution of the problem:

Γ1 ,

[1pn01d0

...1pnm−11dm−1

]T

Γ′1 ,

[1pe01d0

...1pem−11dm−1

]T

Γ2 ,

[2pn02d0

...2pnm−12dm−1

]T

Γ′2 ,

[2pe02d0

...2pem−12dm−1

]T

Γ3 ,

[2pn0 − 1pn0

3d0...

2pnm−1 − 1pnm−13dm−1

]T

Γ′3 ,

[2pe0 − 1pe0

3d0...

2pem−1 − 1pem−13dm−1

]TNote that

‖Γ1‖2 =

m−1∑i=0

(1pni1di

)2

‖Γ′1‖2 =

m−1∑i=0

(1pei1di

)2

= m− ‖Γ1‖2

‖Γ2‖2 =

m−1∑i=0

(2pni2di

)2

‖Γ′2‖2 =

m−1∑i=0

(2pei2di

)2

= m− ‖Γ2‖2

79

‖Γ3‖2 =

m−1∑i=0

(2pni − 1pni

3di

)2

‖Γ′3‖2 =

m−1∑i=0

(2pei − 1pei

3di

)2

= m− ‖Γ3‖2

‖Γ1‖(m− ‖Γ1‖2

) 12 cos(β1) =

m−1∑i=0

(1pni1di

)(1pei1di

)

‖Γ2‖(m− ‖Γ2‖2

) 12 cos(β2) =

m−1∑i=0

(2pni2di

)(2pei2di

)

‖Γ3‖(m− ‖Γ3‖2

) 12 cos(β3) =

m−1∑i=0

(2pni − 1pni

3di

)(2pei − 1pei

3di

)

where β1, β2 and β3 are the angles between Γ1 and Γ′1, Γ2 and Γ′2 and Γ3 and Γ′3, respectively.

Additionally, consider the new variables Γ1, Γ2 and Γ3 such that

‖Γ1‖2 =1

1σ2‖Γ1‖2

‖Γ2‖2 =1

2σ2‖Γ2‖2

‖Γ3‖2 =1

3σ2‖Γ3‖2

and new constants

c1 =m

1σ2

c2 =m

2σ2

c3 =m

3σ2

Now, we can rewrite the entries of the FIM, defined in Equations 3.62, as

F11 = ‖Γ1‖2 + ‖Γ3‖2 (A.1a)

F12 = ‖Γ1‖(c1 − ‖Γ1‖2

) 12 cos(β1) + ‖Γ3‖

(c3 − ‖Γ3‖2

) 12 cos(β3) (A.1b)

F13 = −‖Γ3‖2 (A.1c)

F14 = −‖Γ3‖(c3 − ‖Γ3‖2

) 12 cos(β3) (A.1d)

F22 = c1− ‖Γ1‖2 + c3 − ‖Γ3‖2 (A.1e)

F24 = −(c3 − ‖Γ3‖2

)(A.1f)

80

F33 = ‖Γ2‖2 + ‖Γ3‖2 (A.1g)

F34 = ‖Γ2‖(c2 − ‖Γ2‖2

) 12 cos(β2) + ‖Γ3‖

(c3 − ‖Γ3‖2

) 12 cos(β3) (A.1h)

F44 = c2− ‖Γ2‖2 + c3 − ‖Γ3‖2 (A.1i)

The FIM determinant as function of the new variables is given by

|FIM(1p0,2p0)| = ‖Γ1‖2 cos2(β1)

(c1 − ‖Γ1‖2

)(2‖Γ2‖‖Γ3‖ cos(β2) cos(β3)

√c2 − ‖Γ2‖2

√c3 − ‖Γ3‖2+

+ ‖Γ2‖2 cos2(β2)(c2 − ‖Γ2‖2

)+ ‖Γ3‖2 cos2(β3)

(c3 − ‖Γ3‖2

)−(‖Γ2‖2 + ‖Γ3‖2

)(c2 + c3−

−‖Γ2‖2 − ‖Γ3‖2))

+ 2‖Γ1‖‖Γ2‖‖Γ3‖ cos(β1)√c1 − ‖Γ1‖2

(‖Γ2‖ sin2(β2) cos(β3)

(‖Γ2‖2 − c2

)·

·√c3 − ‖Γ3‖2 + ‖Γ3‖ cos(β2) sin2(β3)

√c2 − ‖Γ2‖2

(‖Γ3‖2 − c3

))+ ‖Γ2‖2 cos2(β2)

(‖Γ2‖2 − c2

)·

·(‖Γ3‖2 cos2(β3)

(‖Γ3‖2 − c3

)+(‖Γ1‖2 + ‖Γ3‖2

) (c1 + c3 − ‖Γ1‖2 − ‖Γ3‖2

))+

+ 2‖Γ1‖2‖Γ2‖‖Γ3‖ cos(β2) cos(β3)(‖Γ1‖2 − c1

)√c2 − ‖Γ2‖2

√c3 − ‖Γ3‖2+

+ ‖Γ3‖2 cos2(β3)(‖Γ3‖2 − c3

) (‖Γ1‖2 + ‖Γ2‖2

) (c1 + c2 − ‖Γ1‖2 − ‖Γ2‖2

)+

+

(‖Γ3‖2

(‖Γ1‖2 + ‖Γ2‖2

)+ ‖Γ1‖2‖Γ2‖2

)(‖Γ3‖2

(−c1 − c2 + ‖Γ1‖2 + ‖Γ2‖2

)+ c1c2 + c1c3−

− c1‖Γ2‖2 + c2c3 − c2‖Γ1‖2 − c3‖Γ1‖2 − c3‖Γ2‖2 + ‖Γ1‖2‖Γ2‖2)

(A.2)

The 1st order optimality conditions are obtained by solving the gradient of the FIM determinant, with

respect to ‖Γ1‖, ‖Γ2‖, ‖Γ3‖, β1, β2 and β3, equal to zero.

To compute the derivative of |FIM(1p0,2p0)| with respect to the variables referred above we use the

following rule to simplify the algebraic computations:

∂|X|∂x

= |X|Tr

(X−1 ∂X

∂x

)

where X denotes an invertible square matrix, x denotes a 1-dimensional variable and Tr(·) denotes the

trace operation.

The partial derivatives of the FIM determinant are then given by

∂|FIM |∂‖Γ1‖

=1√

c1 − ‖Γ1‖2

[2‖Γ2‖‖Γ3‖ cos(β1)

(c1 − 2‖Γ1‖2

)(‖Γ2‖ sin2(β2) cos(β3)

(‖Γ2‖2 − c2

)·

81

·√c3 − ‖Γ3‖2 + ‖Γ3‖ cos(β2) sin2(β3)

√c2 − ‖Γ2‖2

(‖Γ3‖2 − c3

))+ ‖Γ1‖ cos2(β1)

(c1 − 2‖Γ1‖2

)·

·√c1 − ‖Γ1‖2

(‖Γ3‖

(4‖Γ2‖ cos(β2) cos(β3)

√c2 − ‖Γ2‖2

√c3 − ‖Γ3‖2 + ‖Γ3‖ cos(2β3)

(c3 − ‖Γ3‖2

) )+

+ ‖Γ2‖2 cos(2β2)(c2 − ‖Γ2‖2

)− ‖Γ3‖2

(2c2 + c3 − 4‖Γ2‖2

)− ‖Γ2‖2(c2 + 2c3) + ‖Γ2‖4 + ‖Γ3‖4

)+

+ 2‖Γ1‖√c1 − ‖Γ1‖2

(− 2‖Γ2‖‖Γ3‖ cos(β2) cos(β3)

(c1 − 2‖Γ1‖2

)√c2 − ‖Γ2‖2

√c3 − ‖Γ3‖2+

+ ‖Γ2‖2 cos2(β2)(‖Γ2‖2 − c2

) (c1 + c3 − 2

(‖Γ1‖2 + ‖Γ3‖2

))+ ‖Γ3‖2 cos2(β3)

(‖Γ3‖2 − c3

)·

·(c1 + c2 − 2

(‖Γ1‖2 + ‖Γ2‖2

))+ ‖Γ2‖2

(c1c2 − ‖Γ2‖2

(c1 + c3 − 2‖Γ1‖2

)+ c1c3 + c2c3−

− 2c2‖Γ1‖2 − 2c3‖Γ1‖2)

+ ‖Γ3‖2(−2‖Γ2‖2

(c1 + c2 + c3 − 2‖Γ1‖2

)+ c1c2 + c1c3 + c2c3 − 2c2‖Γ1‖2−

−2c3‖Γ1‖2 + 2‖Γ2‖4)

+ ‖Γ3‖4(−(c1 + c2 − 2

(‖Γ1‖2 + ‖Γ2‖2

))))](A.3)

∂|FIM |∂‖Γ2‖

=1√

c2 − ‖Γ2‖2

[‖Γ1‖2 cos2(β1)

(c1 − ‖Γ1‖2

)( (c2 − 2‖Γ2‖2

) (2‖Γ3‖ cos(β2) cos(β3)

√c3 − ‖Γ3‖2+

+ ‖Γ2‖ cos(2β2)√c2 − ‖Γ2‖2

)+ ‖Γ2‖

√c2 − ‖Γ2‖2

(−c2 − 2c3 + 2‖Γ2‖2 + 4‖Γ3‖2

))− 2‖Γ1‖‖Γ3‖·

· cos(β1)√c1 − ‖Γ1‖2

(c2 − 2‖Γ2‖2

)(2‖Γ2‖ sin2(β2) cos(β3)

√c2 − ‖Γ2‖2

√c3 − ‖Γ3‖2 + ‖Γ3‖ cos(β2)·

· sin2(β3)(c3 − ‖Γ3‖2

))+ 2‖Γ1‖2‖Γ3‖ cos(β2) cos(β3)

(‖Γ1‖2 − c1

) (c2 − 2‖Γ2‖2

)√c3 − ‖Γ3‖2+

+ ‖Γ2‖ cos2(β2)√c2 − ‖Γ2‖2

(2‖Γ2‖2 − c2

)(‖Γ3‖2 cos(2β3)

(‖Γ3‖2 − c3

)+ ‖Γ3‖2

(2c1 + c3 − 4‖Γ1‖2

)+

+ 2‖Γ1‖2(c1 + c3 − ‖Γ1‖2

)− ‖Γ3‖4

)+ 2‖Γ2‖

√c2 − ‖Γ2‖2

(‖Γ3‖2 cos2(β3)

(‖Γ3‖2 − c3

)(c1 + c2−

− 2(‖Γ1‖2 + ‖Γ2‖2

))+ ‖Γ1‖2

(c1c2 − 2‖Γ2‖2

(c1 + c3 − ‖Γ1‖2

)+ c1c3 + c2c3 − c2‖Γ1‖2 − c3‖Γ1‖2

)+

+ ‖Γ3‖2(c1c2 − 2‖Γ2‖2

(c1 + c3 − 2‖Γ1‖2

)+ c1c3 − 2c1‖Γ1‖2 + c2c3 − 2c2‖Γ1‖2 − 2c3‖Γ1‖2 + 2‖Γ1‖4

)+

+ ‖Γ3‖4(−(c1 + c2 − 2

(‖Γ1‖2 + ‖Γ2‖2

))))](A.4)

∂|FIM |∂‖Γ3‖

=1√

c3 − ‖Γ3‖2

[‖Γ1‖2 cos2(β1)

(‖Γ1‖2 − c1

)(‖Γ3‖

√c3 − ‖Γ3‖2

(− cos(2β3)

(c3 − 2‖Γ3‖2

)+ 2c2+

82

+c3 − 4‖Γ2‖2 − 2‖Γ3‖2)− 2‖Γ2‖ cos(β2) cos(β3)

√c2 − ‖Γ2‖2

(c3 − 2‖Γ3‖2

))− ‖Γ1‖‖Γ2‖ cos(β1)·

·√c1 − ‖Γ1‖2

(c3 − 2‖Γ3‖2

)(4‖Γ3‖ cos(β2) sin2(β3)

√c2 − ‖Γ2‖2

√c3 − ‖Γ3‖2 + 2‖Γ2‖ sin2(β2) cos(β3)·

·(c2 − ‖Γ2‖2

))+ ‖Γ2‖2‖Γ3‖ cos2(β2)

(‖Γ2‖2 − c2

)√c3 − ‖Γ3‖2

(− cos(2β3)

(c3 − 2‖Γ3‖2

)+ 2c1+

+ c3 − 4‖Γ1‖2 − 2‖Γ3‖2)

+ 2‖Γ1‖2‖Γ2‖ cos(β2) cos(β3)(‖Γ1‖2 − c1

)√c2 − ‖Γ2‖2

(c3 − 2‖Γ3‖2

)+

+ 2‖Γ3‖√c3 − ‖Γ3‖2

(cos2(β3)

(c3 − 2‖Γ3‖2

) (‖Γ1‖2 + ‖Γ2‖2

) (−c1 − c2 + ‖Γ1‖2 + ‖Γ2‖2

)+

+ c1(c2(‖Γ1‖2 + ‖Γ2‖2

)+ c3

(‖Γ1‖2 + ‖Γ2‖2

)− 2‖Γ3‖2

(‖Γ1‖2 + ‖Γ2‖2

)+ ‖Γ2‖2

(−(2‖Γ1‖2 + ‖Γ2‖2

)))+

+ c2(c3(‖Γ1‖2 + ‖Γ2‖2

)− ‖Γ1‖4 − 2‖Γ1‖2

(‖Γ2‖2 + ‖Γ3‖2

)− 2‖Γ2‖2‖Γ3‖2

)+(‖Γ1‖2 + ‖Γ2‖2

)·

·(−c3

(‖Γ1‖2 + ‖Γ2‖2

)+ 2‖Γ1‖2

(‖Γ2‖2 + ‖Γ3‖2

)+ 2‖Γ2‖2‖Γ3‖2

))](A.5)

∂|FIM |∂β1

= −2‖Γ1‖ sin(β1)√c1 − ‖Γ1‖2

(1

2‖Γ1‖ cos(β1)

√c1 − ‖Γ1‖2

(‖Γ3‖

(4‖Γ2‖ cos(β2) cos(β3)·

·√c2 − ‖Γ2‖2

√c3 − ‖Γ3‖2 + ‖Γ3‖ cos(2β3)

(c3 − ‖Γ3‖2

) )+ ‖Γ2‖2 cos(2β2)

(c2 − ‖Γ2‖2

)−

− ‖Γ3‖2(2c2 + c3 − 4‖Γ2‖2

)− ‖Γ2‖2(c2 + 2c3) + ‖Γ2‖4 + ‖Γ3‖4

)+ ‖Γ2‖‖Γ3‖

(‖Γ2‖ sin2(β2) cos(β3)·

·(‖Γ2‖2 − c2

)√c3 − ‖Γ3‖2 + ‖Γ3‖ cos(β2) sin2(β3)

√c2 − ‖Γ2‖2

(‖Γ3‖2 − c3

) ))(A.6)

∂|FIM |∂β2

= −2‖Γ2‖ sin(β2)√c2 − ‖Γ2‖2

(1

2‖Γ2‖ cos(β2)

√c2 − ‖Γ2‖2

(4‖Γ1‖‖Γ3‖ cos(β1) cos(β3)·

·√c1 − ‖Γ1‖2

√c3 − ‖Γ3‖2 + 2‖Γ1‖2 cos2(β1)

(c1 − ‖Γ1‖2

)+ ‖Γ3‖2 cos(2β3)

(c3 − ‖Γ3‖2

)− ‖Γ3‖2·

·(2c1 + c3 − 4‖Γ1‖2

)− 2‖Γ1‖2

(c1 + c3 − ‖Γ1‖2

)+ ‖Γ3‖4

)+ ‖Γ1‖‖Γ3‖

(‖Γ1‖ sin2(β1) cos(β3)

(‖Γ1‖2−

−c1)√c3 − ‖Γ3‖2 + ‖Γ3‖ cos(β1) sin2(β3)

√c1 − ‖Γ1‖2

(‖Γ3‖2 − c3

) ))(A.7)

∂|FIM |∂β3

= −2‖Γ3‖ sin(β3)√c3 − ‖Γ3‖2

(‖Γ1‖2 cos2(β1)

(c1 − ‖Γ1‖2

) (‖Γ2‖ cos(β2)

√c2 − ‖Γ2‖2+

+ ‖Γ3‖ cos(β3)√c3 − ‖Γ3‖2

)+

1

2‖Γ1‖‖Γ2‖ cos(β1)

√c1 − ‖Γ1‖2

(4‖Γ3‖ cos(β2) cos(β3)

√c2 − ‖Γ2‖2·

83

·√c3 − ‖Γ3‖2 + ‖Γ2‖ cos(2β2)

(c2 − ‖Γ2‖2

)− c2‖Γ2‖+ ‖Γ2‖3

)+ ‖Γ2‖2‖Γ3‖ cos2(β2) cos(β3)·

·(c2 − ‖Γ2‖2

)√c3 − ‖Γ3‖2 + ‖Γ1‖2‖Γ2‖ cos(β2)

(‖Γ1‖2 − c1

)√c2 − ‖Γ2‖2 − ‖Γ3‖ cos(β3)·

·√c3 − ‖Γ3‖2

(‖Γ1‖2 + ‖Γ2‖2

) (c1 + c2 − ‖Γ1‖2 − ‖Γ2‖2

))(A.8)

The 1st order optimality conditions are then given by

(∂|FIM |∂‖Γ1‖

,∂|FIM |∂‖Γ2‖

,∂|FIM |∂‖Γ3‖

,∂|FIM |∂β1

,∂|FIM |∂β2

,∂|FIM |∂β3

)= 0 (A.9)

The solutions for the system of equations defined by Equation A.9 are:

i. sin(β∗1), sin(β∗2), sin(β∗3) = 0

This solution yields |FIM |∗ = 0, so it is minimum.

ii. ‖Γ1‖∗, ‖Γ2‖∗ = 0 ∨ ‖Γ2‖∗, ‖Γ3‖∗ = 0 ∨ ‖Γ1‖∗, ‖Γ3‖∗ = 0

These solutions yield |FIM |∗ = 0, so they are minima.

iii. ‖Γ1‖∗ =c12, ‖Γ2‖∗ =

c22, ‖Γ3‖∗ =

c32∧ cos(β∗1), cos(β∗2), cos(β∗3) = 0

This solution yields |FIM |∗ =

(c1 c2 + c2 c3 + c1 c3

4

)2

and it is the only maximum, thus the global

maximum.

84

a range-based navigation system for autonomous underwater vehicles · a range-based navigation...

Documents