terrain aided navigation for autonomous underwater vehicles … · 2020. 1. 20. · abhilash...

Terrain Aided Navigation for Autonomous Underwater Vehicles

with Local Gaussian Processes

Abhilash Chowdhary

Thesis submitted to the Faculty of the

Virginia Polytechnic Institute and State University

in partial fulfillment of the requirements for the degree of

Masters of Science

in

Computer Engineering

Daniel J. Stilwell, Chair

Ryan K. Williams

Pratap Tokekar

May 10, 2017

Blacksburg, Virginia

Keywords: Autonomous Underwater Vehicles, Navigation, Gaussian Processes

Copyright 2017, Abhilash Chowdhary

Terrain Aided Navigation for Autonomous Underwater Vehicles with Local

Gaussian Processes

Abhilash Chowdhary

(ABSTRACT)

Navigation of autonomous underwater vehicles (AUVs) in the subsea environment is partic-

ularly challenging due to the unavailability of GPS because of rapid attenuation of electro-

magnetic waves in water. As a result, the AUV requires alternative methods for position

estimation. This thesis describes a terrain-aided navigation approach for an AUV where,

with the help of a prior depth map, the AUV localizes itself using altitude measurements

from a multibeam DVL. The AUV simultaneously builds a probabilistic depth map of the

seafloor as it moves to unmapped locations.

The main contribution of this thesis is a new, scalable, and on-line terrain-aided navigation

solution for AUVs which does not require the assistance of a support surface vessel. Simu-

lation results on synthetic data and experimental results from AUV field trials in Panama

City, Florida are also presented.

Terrain Aided Navigation for Autonomous Underwater Vehicles with Local

Gaussian Processes

Abhilash Chowdhary

(GENERAL AUDIENCE ABSTRACT)

Navigation of autonomous underwater vehicles (AUVs) in subsea environment is particularly

challenging due to the unavailability of GPS because of rapid attenuation of electromagnetic

waves in water. As a result, the AUV requires alternative methods for position estimation.

This thesis describes a terrain-aided navigation approach for an AUV where, with the help of

a prior depth map, the AUV localizes itself using altitude measurements from a multibeam

DVL. The AUV simultaneously builds a probabilistic depth map of the seafloor as it moves

to unmapped locations.

The main contribution of this thesis is a new, scalable, and on-line terrain-aided navigation

solution for AUVs which does not require assistance of a support surface vessel. Simulation

results on synthetic data and experimental results from AUV field trials in Panama City,

Florida are also presented.

To my Mother, Father and Brother

iv

Acknowledgments

I would like to thank my advisor, Dr. Daniel Stilwell, for advice, feedback and support, and

for being an example throughout my Masters. I want to thank you for letting me pursue

the thesis work on the topic of my liking and always being there to guide me in the right

direction. I’m also grateful to other members in my thesis committee, Dr. Ryan Williams

and Dr. Pratap Tokekar, for their guidance and help from the initial days of my research

work.

Two years of my Masters would not have been an easy ride without my fellow labmates

Autonomous Systems and Controls Lab (ASCL) at Virginia Tech. I would like to thank

Scott, Rand, Rami, Jack, Harun, Stephen, Nick, Larkin and Kepler for many hours of

stimulating discussions, sharing some unforgettable laughs and proofreading this work.

I would also like to thank my friends Tommy, Lily, Josh, Oscar, Jennifer, Siddharth, Sarthak,

Tapas, Chris, Ramakrishna and Dixika for their support and memorable time in Blacksburg.

I would also like to thank my friends Jasmeet and Ashish for being great hosts, every time

I visited Bay Area during this time.

v

Finally, I would like to thank my parents and my brother Abhishek for their caring support

and love.

vi

Contents

Contents x

List of Figures xi

List of Figures xiv

List of Tables xv

List of Tables xv

1 Introduction 1

1.1 Related Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.1.1 Terrain Based Navigation . . . . . . . . . . . . . . . . . . . . . . . . 3

1.1.2 SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.2 Localization using GP based observation model . . . . . . . . . . . . . . . . 4

vii

1.3 Outline and contributions of thesis . . . . . . . . . . . . . . . . . . . . . . . 5

2 Gaussian Processes 8

2.1 Sampling from GP . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.2 GP Posterior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.3 Nonparametric Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.4 Covariance Function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.5 Inference with Noisy Observation . . . . . . . . . . . . . . . . . . . . . . . . 12

2.5.1 Bayesian Perspective . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.5.2 Hyperparameter Learning . . . . . . . . . . . . . . . . . . . . . . . . 14

2.6 Multi-Dimension Input . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

2.7 GP with uncertain inputs . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

2.8 GP Limitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

2.9 Local Gaussian Processes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

2.9.1 Partitioning Data . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

2.9.2 Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

2.9.3 Updating Local Models . . . . . . . . . . . . . . . . . . . . . . . . . . 23

3 State Space Inference Using Gaussian Processes 25

viii

3.1 Inference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

3.2 Particle Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

3.2.1 Basic Description for AUVs . . . . . . . . . . . . . . . . . . . . . . . 29

3.2.2 Inference using Particle Filters . . . . . . . . . . . . . . . . . . . . . . 30

3.3 GP based Particle Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

3.3.1 GP as Observation Model . . . . . . . . . . . . . . . . . . . . . . . . 32

3.3.2 On-line updating of observation model . . . . . . . . . . . . . . . . . 33

3.4 Local GP based Particle Filters . . . . . . . . . . . . . . . . . . . . . . . . . 36

4 Terrain-Aided Navigation 40

4.1 The Terrain-Aided Navigation Problem . . . . . . . . . . . . . . . . . . . . . 40

4.1.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

4.2 State Space Model for TAN . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

4.2.1 Prediction Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

4.2.2 Observation Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

4.2.3 Multibeam DVL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

4.3 Particle Filtering strategy for multibeam DVL . . . . . . . . . . . . . . . . . 45

4.3.1 Likelihood for re-sampling . . . . . . . . . . . . . . . . . . . . . . . . 46

ix

4.3.2 Inference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

4.4 Cost of running into the shore . . . . . . . . . . . . . . . . . . . . . . . . . . 47

5 Experimental Evaluation 50

5.1 Simulation based Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . 50

5.1.1 Comparison of Local and Sparse Approximation based methods . . . 51

5.1.2 Simulating AUV’s Trajectory . . . . . . . . . . . . . . . . . . . . . . 53

5.1.3 Cost of Running into the Shore . . . . . . . . . . . . . . . . . . . . . 54

5.2 Real world Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

5.2.1 The Virginia Tech 690s AUV . . . . . . . . . . . . . . . . . . . . . . 55

5.2.2 Field Trials . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56

6 Conclusions 69

6.1 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70

Bibliography 71

x

List of Figures

2.1 Graphical model for Gaussian Processes. zi denote the observations, xi denote

the inputs(or location) and fi denote the latent realizations from the GP model. 9

2.2 This figure shows inference and training in local GP model. Data set is clus-

tered into various subset and GP models are learned separately on each subset.

Inference at the yellow dot is a weighted mean of the GP inferences of the

nearest (M = 4) clusters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

3.1 Graphical model for a sequential Markov process with latent states and ob-

servation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

3.2 Graphical model for a state-space model with observation model modeled as

a GP . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

xi

3.3 Graphical model for a state-space model with LGP as the observation model.Here

xt denote the latent states of the system and zt denote the observations made

by the system. The nodes LGPi model the local GP models using a subset

of the observations and corresponding states. h() is the observation model

which consists of all the local GP models. g() is the prediction model of the

system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

4.1 The Virginia Tech 690s during Panama City trials . . . . . . . . . . . . . . . 41

5.1 3D plot of 600×300 grid depicting the terrain of the seafloor. 0 on the Z-axis

represents the bottom most point of the floor from the surface. . . . . . . . . 51

5.2 Comparison of depth prediction over the synthetic data of Figure 5.1 using

1900 random points with known depth values. Figures on the left show the

mean depth prediction and bias associated with it, for an LGP model with 30

clusters and Gaussian(RBF) kernel. Figures on the right show the mean depth

prediction and bias associated with it, for an sparse approximation for GP over

all the 1900 points. The root mean squared errors in depth prediction for LGP

and sparse GP based model were 7.27 meters and 4.17 meters respectively. . 52

5.3 Shows the comparison of ten estimated trajectories from LGP-PF with prior

LGP model and re-learned LGP model. . . . . . . . . . . . . . . . . . . . . . 54

xii

5.4 Plot of cost of an AUV running into the shore, moving at a depth of 30 meters

below the surface of water. . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

5.5 Prior depth map with data points represented as black dots (4834 in number).

The contour plot shows predictive mean depth from sparse GP model fit on

the training data points from first and second mission. . . . . . . . . . . . . 57

5.6 Prior depth map with data points represented as black dots (4834 in number).

The contour plot shows predictive mean depth from LGP model fit on the

training data points from first and second mission. . . . . . . . . . . . . . . 58

5.7 The Virginia Tech 690s AUV trajectory for Panama City trials as estimated

by LGP-PF. It is also compared to range-based solution in [12]. GPS updates

are shown as red dots and the numbers next to them denote the sequence

of these updates. Blue dots represent range based estimate. Yellow dots

represent LGP-PF based estimate. . . . . . . . . . . . . . . . . . . . . . . . 59

5.8 This figure shows three different areas on the predicted depth map from LGP.

White region denotes where the prediction was zero and is neglected. Black

dots represent the down-sampled prior depth data points used for learning the

LGP model. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

5.9 This figure shows the AUV starting in the south (initial start point) and it

resurfaces in north with GPS update. The error in estimation is about 10

meters. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

xiii

5.10 This figure shows the AUV starting from south and going to east via west and

north. The error in estimation is about 25 meters with respect to GPS update. 63

5.11 This figure shows the AUV starting from west and going to north via east.

The error in estimation increases to about 30 meters with respect to GPS

update. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

5.12 This figure shows the AUV starting from the south and going to east via west.

The error in estimation drops to about 8 meters with respect to GPS update. 65

5.13 This figure shows the errors in location estimate using LGP-PF relative to

GPS updates at each of the AUV resurface. Negative error means AUV is

underwater and does not have GPS fix. . . . . . . . . . . . . . . . . . . . . 67

xiv

List of Tables

5.1 Sensors in the Virginia Tech 690s . . . . . . . . . . . . . . . . . . . . . . . . 56

xv

Chapter 1

Introduction

Autonomous underwater vehicles (AUVs) have to localize themselves under water for car-

rying out various missions. Since electromagnetic waves suffer from very high attenuation

in water, an AUV has to come to the surface to update its location using global position-

ing system (GPS). To complement the use of GPS for AUVs, other navigation systems are

used, like inertial navigation system (INS), Doppler velocity logs (DVL), ultrashort-baseline

(USBL), and long-baseline (LBL) acoustic positioning systems.

For navigation, systems like INS coupled with a DVL give an un-bounded error for localizing

the AUV [2]. Systems that use acoustic ranging for navigation, such as LBL and USBL,

require that one or more acoustic reference nodes be deployed along with the AUV, and

moreover, the AUV must remain within the acoustic range of the reference nodes. Generally,

a support vessel is used for installing acoustic reference nodes, which adds to the operational

cost the AUV. Alternative to range based navigation, an AUV can localize itself by just

1

Abhilash Chowdhary Introduction 2

using information extracted from the terrain with the help of different sensors, and thereby

reducing the cost of operation. As a result of reduced cost of operation, terrain based

navigation solutions may be more helpful in certain applications.

Algorithms that utilize terrain for navigation can be divided into two categories: the first

is localizing the AUV in an a priori generated map of the ocean floor, and the second is

localizing the AUV while creating the map for the ocean floor without the need of a prior

map. The first kind of algorithms form a category called terrain-aided navigation (TAN)

problem. Terrain-aided navigation methods provide localization for an autonomous vehicle,

with respect to a priori known map and a sequence of real-time measurements of the local

height of the terrain. Choice of the method used for measuring the local height of the terrain

depends on the approach being used.

An alternative approach is to generate a map of the environment (in this case a bathymetric

map) while simultaneously localizing itself on the map generated. Because the accuracy of

navigation is dependent on the accuracy of the map generated and vice-versa, this approach

enforces a consistency in maps generated [27]. The technique of simultaneous localization

and mapping is known as simultaneous localization and mapping (SLAM) [27] and has been

extensively used, and researched in context of aerial and ground robots. However, there is

comparatively less research done on SLAM in subsea environment.


1.1 Related Work

1.1.1 Terrain Based Navigation

A linear Kalman filter based approach with sonar for depth measurements is used in [21].

Particle filter based methods for localization have also been used for localization using similar

observation models in [6], [5]. [21], [6] and [5] require prior digital elevation maps (DEM) of

the seafloor for them to work, and they do not model the correlation between the depth at

any two locations in and outside the map.

1.1.2 SLAM

Barkby et al. in [2], addressed the problem of simultaneously localizing and mapping via

AUV for a flat seafloor. The SLAM technique used in [2] follows from the distributed

particle SLAM (DPSLAM) filter used in [9]. In [33], the authors present a SLAM algorithm

for AUVs that is based on extended Kalman filter (EKF). In [1] and [33] the measurement

function used is dependent only on the depth measurements from the DVL and the location

of the AUV at the particular time step. Correlation between the depth of two different

points in the horizontal plane is not considered in the observation model. Barkby et al. in

[1] used a Gaussian process(GP) based SLAM algorithm for AUVs where the measurement

model is learned using a Gaussian process over the terrain. A GP model gives a predictive

distribution for the depth of the ocean at any location not yet visited by AUV. Using this


model, a predictive map of the seafloor can be generated with an uncertainty measure. GPs

have also been used for planning informative underwater survey missions [3]. One major

drawback of using GP as observation model is the high computational complexity, with

respect to total number of observations, associated with learning the model (refer Chapter

2).

1.2 Localization using GP based observation model

A GP-based observation model in a state-space inference problem models the noisy observa-

tions at the latent states of the system. Gaussian processes are a class of random processes

defined by their mean and covariance function [23]. The covariance function expresses the

correlation of measurement zi and zj at two different states xi and xj respectively.

GPs have been a popular choice for modeling the non-linearity between the observations and

the states for state-space inference problems. The states for an AUV in this thesis refers to

the location in horizontal plane and observations are the depth measurements as described

in chapter 4. GP-based observation models are used in [13], [14] and [28] for state-space

inference in a non-linear dynamical system. For a better estimate of the value of a state,

the GP model needs to be updated with additional observations as more states are explored.

However, learning and updating a GP model is expensive as it requires an inversion of a

large covariance matrix on every update with new data. Calculating the inverse of a matrix

has a time-complexity of O(e3), where e is the dimension of matrix.


To scale the Gaussian process model, we use local Gaussian processes (LGP) [16], [19].

Instead of a one single Gaussian process model with all the data points, multiple Gaussian

process models over subsets of data points are employed. To create a subset, data points

are clustered based on their nearness to each other. Then the prediction at a given point is

calculated as the weighted prediction of GP models corresponding to nearby clusters. Since

the computational complexity for learning a GP model increases with the increase in data

points, the number of data points in each cluster is limited. This bounds the worst case time

complexity for learning as well as updating a GP model.

For localization, a particle filter based approach [13], using the LGP model as the measure-

ment model and a linear dynamical model as the prediction model, is used. As the AUV

moves and gathers more depth data, the predicted map of the seafloor is updated after

discrete time intervals using the LGP model.

1.3 Outline and contributions of thesis

The main contribution of the thesis is to develop a method for localizing an AUV using

altitude measurements from a multibeam DVL. The algorithm presented here requires a prior

sparse bathymetric map of the area where the AUV is scheduled to survey. The localization

algorithm can be run on-line and the run time complexity is bounded for inference on a

stream of measurements. This thesis also provides a method to quantify the cost of running

an AUV into the shore for a certain path between two points given that the AUV always


maintains a constant depth from the water surface.

The words “learning” and “inference” are used quite often in this work. To make it clear

to the reader, following terminology is used in this work. Given a set of samples from a

stochastic process, “learning” refers to choosing the parameters of a model that best fits the

samples. Given a set of samples from a stochastic process, “inference” refers to choosing a

configuration (or model) for the process and estimating the latent variables of the model.

Chapter 2 is a short primer on Gaussian processes in general and describes how does the

inference and learning proceed in a GP model. It then builds up on to local Gaussian

processes which is an approximation of GPs over a large amount of data when GPs scale

poorly.

Chapter 3 shows how to do inference over states in a non-linear state space model where the

observation model is modeled by GP or LGP model.

Chapter 4 defines a terrain-aided navigation problem in the context of an AUV and describes

how the state space inference method developed in Chapter 3 can be used for localizing

the AUV. The algorithm proposed in Chapter 4 for localizing the AUV is computationally

tractable for real-time applications. This algorithm, having linear computational complexity

with respect to number of observations made, can be used in the on-line setting to localize

the AUV . A method to quantify the cost related to a path from point A to point B with

the AUV at a constant depth from the surface of water is also described.

Chapter 5 shows experimental results of the method on toy dataset as well as data from field


trials in Panama City, Florida. Chapter 6 concludes this thesis.

Chapter 2

Gaussian Processes

This chapter gives an overview of Gaussian processes in general. Gaussian processes (GP) are

a class of stochastic process that provide an elegant method for Bayesian non-linear regres-

sion. At their core, GPs model the relationship between latent variables to the measurements

(or observations).

A Gaussian process is defined as a collection of random variables, any number of which have

a joint Gaussian distribution [23]. Here these random variables can be considered as the

realizations (latent function), f(x), of the input x. We can interpret GPs as a simple and

general class of models of functions. Since these realizations in a GP are distributed according

to a multivariate normal distribution, it means that characterization of these realizations can

be completely described by : 1) a mean vector µ or mean function µ(x) and 2) covariance

8

Abhilash Chowdhary Gaussian Processes 9

z1

f1

x1

z2

x2

f∗

x∗

z∗

fn

xn

zn

Figure 2.1: Graphical model for Gaussian Processes. zi denote the observations, xi denote

the inputs(or location) and fi denote the latent realizations from the GP model.

matrix Σ or a covariance function K(·, ·).

f(X) |X ∼ N (µ(X), K(X,X)) (2.1)

where X = [x1,x2, . . .xN ], f(X) = [f(x1), f(x2), . . . f(xN)] and

E[f(x)] = µ(x) (2.2)

Cov(f(x), f(x∗)) = k(x,x∗) (2.3)

For a GP, the choice of the covariance function k(·, ·) : R× R 7→ R in (2.3) is dependent on

the correlation between x and x∗.

We refer to (2.1) as the prior for a GP, which can also represented by

f(X) ∼ GP(µ(X), K(X,X)) (2.4)

where K(·, ·) : RN×q × RM×q 7→ RN×M is the covariance matrix and q is the dimension of

input x. As (2.4) defines the prior for the process being observed, mean vector µ(X) is

usually taken as zero if not much prior information is available. Even with a zero mean


prior, as more observations are made, the posterior mean shifts towards the true mean of

the observed process.

2.1 Sampling from GP

As seen from (2.1), all the function realizations are distributed according to normal dis-

tribution and hence generating more samples for a GP is equivalent to sampling from the

given distribution. In other words, these randomly generated samples are from the prior

distribution over the observations, which is GP.

2.2 GP Posterior

Random samples generated in Section 2.1 from a Gaussian process prior are usually of not

much value. While doing inference, we are more concerned about the posterior predictive

distribution of function value f(x∗) at a new location x∗. According to the prior, f(x∗)

and f(X) are distributed according to a multivariate normal distribution. From conditional

distribution of two joint normally distributed random variables, it can be shown [23]

p(f(x∗) |x∗, f(X),X) ∼ N (µ∗(x∗),σ∗(x

∗)) (2.5)


where

µ∗(x∗) = µ(x∗) +K(x∗,X)K(X,X)−1(f(X)− µ(X)) (2.6)

σ∗(x∗) = k(x∗,x∗)−K(x∗,X)K(X,X)−1K(X,x∗) (2.7)

The posterior predictive distribution of the function f(x∗) in (2.5) is a normal with mean

(2.6) and covariance (2.7).

2.3 Nonparametric Model

The distribution from (2.5) does not have any other parameters other than the data points

D = (X, f(X)). Since the distribution in (2.5) does not have any model parameters, it

is a nonparametric model. As the number of data points increases, the number of model

parameters increases. However, there are parameters of the covariance function which are

termed as hyperparameters, and they play an important role in the behavior of a GP model.

2.4 Covariance Function

The appropriateness of a Gaussian Process model is entirely dependent on the choice of

covariance function k(xi,xj), which describes the correlation between the observations zi

and zj . A popular covariance function to model geospatial data is the radial basis function


(RBF) kernel [23]

k(x,x′) = σ2f exp

(−1

2

(x− x′)2

2`2

)(2.8)

where ` is a length scale that determines the strength of correlation between points and σf

is the signal variance. As seen from (2.8), when two points are separated by a large distance,

the covariance will tend to zero, indicating the independence of measurements at locations

that are far apart.

2.5 Inference with Noisy Observation

In a realistic setting, we usually cannot observe the function values f(x), and hence they are

latent functional realizations from the GP. Generally, we have access to the noisy version of

these functional values which we can model as

z = f(x) + εz (2.9)

where z is the noisy observation (or measurement) with noise εz. The distribution of the

noise εz depends on the stochastic process we are observing. If we consider εz to have a

Gaussian distribution with variance σ2n,

εz ∼ N (0, σ2n) (2.10)

then the distribution of observations Z given the functional values f(X) and locations X is

expressed as

Z | f(X),X ∼ N (f(X), σ2nI) (2.11)


With addition of noise, we’re interested in posterior predictive distribution p(z∗ |x∗,Z,X)

of the actual observation z∗ at the location x∗. The covariance matrix in (2.3) changes to

Cov(f(x), f(x′)) = k(x,x′) + σ2nI (2.12)

Due to the statistics of the observation noise in (2.10), we have the likelihood of our obser-

vations Z

Z ∼ N (µ(X), K(X,X) + σ2nI) (2.13)

Following the same approach as in Section 2.2, the posterior predictive distribution for z∗

can be calculated using (2.12) and (2.13)

p(z∗ |x∗,Z,X) ∼ N (µ∗(x∗),σ∗(x

∗)) (2.14)

where

µ∗(x∗) = µ(x∗) +K(x∗,X)[K(X,X) + σ2

nI]−1(Z− µ(X)) (2.15)

σ∗(x∗) = k(x∗,x∗)−K(x∗,X)[K(X,X) + σ2

nI]−1K(X,x∗) (2.16)

(2.15) and (2.16) are similar to (2.6) and (2.7) respectively, with addition of noise to the

input covariance matrix K(X,X). Figure 2.1 shows the graphical model of a GP.

2.5.1 Bayesian Perspective

An interesting thing to notice is, if we look through a Bayesian point of view, (2.11) resembles

a likelihood function for the observations Z and (2.1) and (2.4) are the priors for the latent

function values f(X) modeled via a GP.


Given the likelihood and prior, the marginal likelihood , p(Z |X), of the observation can

easily be calculated by integrating out the latent function values f(X)

p(Z |X) =

∫p(Z | f(X),X)p(f(X) |X)df (2.17)

2.5.2 Hyperparameter Learning

As shown in Section 2.3, GPs are nonparametric models with data points being its parame-

ter. However, there are free parameters which arise from the covariance function k(·, ·) and

the observation noise. For a RBF covariance function as described in (2.8), these param-

eters include the length scale `, the signal variance σ2f and the noise variance σ2

n for each

dimension of the input. The change in these parameters is reflected in the predictive mean

and the predictive variance of the GP (2.15) and (2.16). Let θ = {`, σ2f , σ

2n} represent the

hyperparameters in a GP, (2.17) can be rewritten as

p(Z |X,θ) =

∫p(Z | f(X),X)p(f(X) |X)df (2.18)

If under the GP model, the prior for f(x) has mean µ(x) equal to zero, (2.1) becomes

f(X) |X ∼ N (0, K(X,X)) (2.19)

Also, if the likelihood as defined in (2.11) is a Gaussian distribution, the log marginal likeli-

hood in (2.18) can be calculated in closed form [23]

log p(Z |X,θ) = −1

2Z>K−1

z Z− 1

2log |Kz| −

n

2log 2π (2.20)


where Kz = K + σ2nI is the covariance matrix for the noisy observations Z ( and K is the

covariance matrix for noise-free latent function values f(X)). Since the marginal likelihood

in (2.20) is not dependent on the latent function values, estimation of θ can be done by

optimizing the marginal log likelihood in (2.20). For finding the values of θ that maximize

the marginal likelihood, the partial derivatives of (2.20) are set to zero. Calculating the

derivatives requires the computation of the inverse of Kz, and it requires time O(N3) where

N is the number of data points in the GP model.

While maximizing, it is possible for the marginal likelihood to reach a local optima. However,

practical experience with simple covariance functions seem to indicate that the local maxima

are not a devastating problem, but they certainly do exist [23].

There are other frequentist approaches like cross-validation for optimizing the hyperparam-

eters. However, we do not use those for training our GP models in this thesis.

2.6 Multi-Dimension Input

All the equations mentioned above remain the same when we have multi-dimensional input

X except for the expression for RBF kernel in (2.8). For a q dimensional input Xnq, the

length scale parameter is represented by diagonal matrix Wqq where each diagonal element

represents the inverse square of lengthscale in each dimension. Hence the expression for the


kernel becomes

k(x1,x2) = σ2f exp

(−(x1 − x2)W(x1 − x2)>

2

)(2.21)

For learning the value of W, the same approach is used as in Section 2.5.2. The only

difference is instead of having a single lengthscale parameter to learn, we’ll have m number

of lengthscale parameters, one for each dimension of the input.

2.7 GP with uncertain inputs

In Sections 2.5 and 2.2, we considered inputs x ∈ Rq to be certain and without noise. In a

real world scenario, actual inputs x are noisy

x = x + εx (2.22)

where εx ∼ N (0,Σx). Here the noise in each input dimension is independent of the noise

in other input dimensions and Σx is a diagonal matrix. With a GP prior over f(x), the

observation z can be written

z = f(x + εx) + εz (2.23)

Using the approach from [15] and after considering the Taylor expansion for f in (2.23), the

distribution over the observation z is expressed

p(z | f,x) ∼ N (f(x), σ2n + ∂>

fΣx∂f ) (2.24)


where ∂f = ∂f∂x

. Using (2.24) as the likelihood and taking a zero mean (µ(X) = 0) GP prior

over f(X), the predictive posterior mean and variance can be expressed [15]

E[z∗ |X,Z,x∗] = −K(x∗,X)[K(X,X) + σ2nI + diag{∆>

fΣx∆f}]

−1Z

(2.25)

V[z∗ |X,Z,x∗] = k(x∗,x∗)−K(x∗,X)[K(X,X) + σ2nI + diag{∆>

fΣx∆f}]

−1K(X,x∗)

(2.26)

where diag{∆>f

Σx∆f} is the additional term from the uncertainty of the input points X and

∆f represents the derivative of the GP function values with respect to the input vector x∗.

2.8 GP Limitations

As seen in Section 2.5.2, the matrix inversion needed for learning the hyperparameters in

(2.20) requires computational O(N3) where N is the number of data points in the model.

Similarly, in (2.15) and (2.16), the matrix inversion needed for inference is O(N3). This

is an issue when dealing with datasets having N > 10000. Also, the space complexity for

storing a N x N covariance matrix K is O(N2). High space-time complexity is a problem

when learning and inference are performed on a limited number of machines with limited

computational power. It also limits applications that require real-time inference. To tackle

these shortcomings, various approximate solutions can be used. One such approximation

is sparse approximation of the GP [25], where only a few induced points out of the whole

training data are used for learning the hyperparameters. Another is locally weighted GP


[20], where instead of learning a single GP model over the whole data, data is clustered into

various data sets and different GP models are trained on each of these data sets. In the local

approximations, whole training set is used for training while in sparse approximations only

a few data points (induced points) are used for training and inference.

Another shortcoming is the necessity of selecting an appropriate covariance function before

training a GP. Selection of kernels is usually done manually. However, some work has been

done to automate kernel learning process, too [8].

2.9 Local Gaussian Processes

As described in Section 2.8, GPs scale very poorly with increase in the data points. Poor

scalability is especially a bottleneck if GP-based inference is used in real-time scenario where

the number of data points increase with time, and inference is performed after re-learning

the hyperparameters after every time step.

The main issue with learning of hyperparameters in a GP is the calculation of the inverse of

the covariance matrix K. Hence most of the fast approximations have the main purpose of

either making the covariance matrix sparse or reducing its size. Sparsity of the covariance

matrix can be induced by selecting a covariance function k(·, ·) that evaluates to zero for

most pairs of inputs. The size of the covariance matrix K can be reduced either by only

considering a certain number of points which maximize the likelihood or by considering all

the data points but as a part of different GP models which are learned separately.


In this work, we use local approximation for GP where instead of using only a handful of

points for inference and learning, we use all the data points but in various independent local

GP models.

2.9.1 Partitioning Data

We use local Gaussian process (LGP) model in a similar way as used in [17] and [20].

Instead of a one single Gaussian process model that encompasses all the data points D =

{X,Z}, the LGP approach uses multiple Gaussian processes that each model a subset of

available data points. To create a subset, the data points D are clustered based on Euclidean

distance to each other and a new set of data points D = {D[1] ∪ D[2] ∪ D[3].. ∪ D[M ]} is

created such that X = {X[1] ∪ X[2] ∪ X[3].. ∪ X[M ]} , Z = {Z[1] ∪ Z[2] ∪ Z[3].. ∪ Z[M ]}

and D[i] = {X[i],Z[i]} where D[i] represents the data points in the ith cluster, and X[i] and

Z[i] represent the inputs and the observations respectively in that cluster. The clustering

of the dataset is done based on the nearness of a each input vector point x to other data

points. In this work, k-means [11] based clustering approach is used to cluster the data

points into M clusters. The implementation provided in [22] is used for clustering D which

has a linear time complexity with respect to N where N is the total number of data points.

After partitioning, GPs are fit on each of the clustered datasets. If the maximum number

of elements in a subset is nmax, worst case time complexity of learning all the GP models is

O(n3max

Nnmax

) ∼ O(N) (if nmax � N) where N is the total number of data points.


Figure 2.2: This figure shows inference and training in local GP model. Data set is clustered

into various subset and GP models are learned separately on each subset. Inference at the

yellow dot is a weighted mean of the GP inferences of the nearest (M = 4) clusters

The RBF kernel (2.8) models the covariance between the two observations such that the

correlation between them decreases exponentially as the Euclidean distance increases. Hence,

an input vector x would have little effect on the predicted value znew at location xnew if the

Euclidean distance between x and xnew is large. Therefore, the clustering step does not

affect the inherent correlation between the input vectors because an input vector x is bound

to be only correlated to the points from the same local region.


2.9.2 Prediction

After partitioning the data into various clusters, we predict the observation z∗ at a location

x∗. The value of z∗ varies depending on which subset of GP models is used to compute it.

The measure, vk, to which a certain GP model effects the prediction is given by Gaussian

distance between the query point, x∗, and the centroid, ck, of the cluster on which the specific

GP model is fit. The expression for vk is given by

vk = exp(− 1

2(x∗ − ck)>Wk(x∗ − ck)

)(2.27)

where W is the diagonal matrix of lengthscales in each dimension of input as mentioned in

the Section 2.6

The prediction at a desired query location x∗ is calculated as the weighted prediction z from

k nearest GP models. The weighted prediction z can be calculated in a manner similar to

locally weighted regression [29]

z = E{zi | X} =M∑i=1

zip(i | X) (2.28)

where zi is the local prediction of the each local GP model and p(i | X) is the probability of

the model i given X [18]. Using Bayes theorem, the probability of the model i given X can

be expressed as

p(i | X) = p(i,X)/k∑

i=1

p(i,X) = vi/k∑

j=1

vi (2.29)

This gives

z =

∑ki=1 vizi∑ki=1 vi

(2.30)


Figure 2.2 shows the inference and training in a LGP with M = 4. If we limit the size of

each cluster to nmax, then the total time complexity in learning the LGP model over whole

data set would be O(kn3max)� O(N3) where N is the total number of data points.

Algorithm 1 Algorithm for partitioning and learning local GP models from prior data

1: procedure PartitionAndLearnLGP

2: D ← X,Z M ← input

3: D[1] ∪ D[2] ∪ D[3].. ∪ D[M ]← kMeans(D,M)

4: for t = 1 to M do

5: ct ← calculateCentroid(D[t])

6: LGPt ← learnGPModel(D[t])

7: LGP ← LGP 1 ∪ LGP 2 ∪ LGP 3.. ∪ LGPM

8: D ← D[1] ∪ D[2] ∪ D[3].. ∪ D[M ]

9: return D,LGP

Algorithm 2 Algorithm for prediction at a location in LGP

1: procedure PredictLGP


3: LGP ← PartitionAndLearnLGP (D,M)

4: k ← input . Number of nearest models to consider

5: x∗, z∗ ← input . New data point

6: (c1, v1), (c2, v2), ...(ck, vk)← findKNearestModels(LGP )

7: . vi represents distance measure for each model

8: for i = 1 to k do

9: zi ← predictFromGP (LGPci)

10: z =∑k

i=1 vizi∑ki=1 vi

11: return z


2.9.3 Updating Local Models

One of the benefits of local models is the reduced computational complexity in learning the

hyperparameters in comparison to just a single GP model fit over the data. However, if

the number of data points are not constrained in a cluster, then computational complexity

would almost equal that of a normal GP regression trained on the whole data set.

To reduce the computational cost, the size of each of the clusters is limited to a specific

number nmax � N , where N is the total number of data points. To add a stream of data

points to the LGP model in a real-time manner, an approach similar to [20] and [17] is

followed in our work. A new data point (xnew, znew) is added to the cluster having the

center nearest to the new data point. The nearness of a cluster i is inversely proportional

to the value of vi (2.27). This requires evaluating vi with respect to each cluster which

is computational O(N). However, if the data points are clustered according to a specific

structure, more optimal algorithms can be used as described in [30]. If for a new data point,

the distance measure vi to each cluster i is less than minimum threshold vmin, then a new

cluster is created containing just the new data point (xnew, znew). After adding the new

data point to the nearest cluster k, hyperparameters of the GP model corresponding to the

cluster k are re-learned by maximizing the likelihood, as in Section 2.5.2. Re-learning the

hyperparameters requires calculating the inverse of the updated covariance matrix K(·, ·) for

the cluster k. The update is done by factorizing K(·, ·) using Cholesky decomposition and

then finding the updated inverse in O(n2max) as done in [20] and [17]. Therefore, the whole

process of updating a LGP model with a new data point is worst case O(Nn2max). Algorithm


3 describes the steps involved in updating the LGP model with a new data point.

Algorithm 3 Algorithm for incremental updating of a LGP with new data point

1: procedure On− lineUpdateLGP2: {LGP1, LGP2...LGPM} ← PartitionAndLearnLGP (X,Z)


4: while {xnew, znew} ← DataStream do

5: vk, ck ← calculateMaximumDistance((v1, c1), (v2, c2)...(vM , cM))

6: . get the centroid ck of model having maximum distance measure vk

7: if vk > vmin then

8: n← numberOfDataPoints(D[k])

9: D[k]← {D[k] ∪ (xnew, znew)}10: Kn×n ← getCovarianceMatrix(LGPk)

11: Kn+1×n+1 ← updateCovarianceWithNewDataPoint(Kn×n, (xnew, znew))

12: LGPk ← reLearnHyperparametersOfLGP (Kn+1×n+1,D[k])

13: else

14: D[M + 1]← {xnew, znew}15: cM+1 ← xnew

16: LGPM+1 ← learnGPModel(D[M + 1])

17: D ← D[1] ∪ D[2] ∪ D[3].. ∪ D[M ] ∪ D[M + 1]

18: LGP ← LGP 1 ∪ LGP 2 ∪ LGP 3.. ∪ LGPM ∪ LGPM+1

19: return D,LGP

Chapter 3

State Space Inference Using Gaussian

Processes

Consider the scenario of tracking the location of an autonomous underwater vehicle (AUV)

equipped with a Doppler velocity log (DVL) which measures the altitude of the AUV above

the seafloor. The location x of the AUV is unobserved and it’s the latent system state. The

measurement of altitude z is noisy. We assume there is a spatial correlation between the

depth of seafloor at two different locations.

A state-space model assumes a Markovian process on a sequence of latent states xt [28]. The

transition between the consecutive states xt−1 and xt is modeled by a transition function

g : RP → RP . For each latent state xt there is an observation zt which is modeled with

a measurement function h : RP → RD. The state space model in NLDS or LDS can be

depicted analytically as

25

Abhilash Chowdhary State Space Inference Using Gaussian Processes 26

xt = g(xt−1) + εt, xt ∈ RP (3.1)

εt ∼ N (0,Q) (3.2)

zt = zt + νt, zt ∈ RD (3.3)

νt ∼ N (0,R) (3.4)

zt = h(xt) (3.5)

The equation (3.1) depicts the prediction model of the latent state for the Markov process

and εt in (3.2) is the prediction or process noise of the system. The observation model of

the observation zt corresponding to each latent state xt is depicted in (3.3) and (3.5) and νt

in (3.4) is the observation or measurement noise of the sensor.

xt−2

zt−2

xt−1

zt−1

xt

zt

xt+1

zt+1

Figure 3.1: Graphical model for a sequential Markov process with latent states and obser-

vation

3.1 Inference

A time series which follows a hidden Markov model (HMM) is described by (3.1) and (3.3).

The state space model described in Figure 3.1 is an example of HMM. The state xt at current


time step is only dependent on the state xt−1 at the previous time step. The transition

probability p(xt |xt−1) can be evaluated using (3.1) and (3.2)

p(xt |xt−1) ∼ N (f(xt−1),Q) (3.6)

From 3.1 we can see that the observations zt are conditionally independent given the state

xt. The marginal distribution p(zt |xt) can be evaluated using (3.3), (3.4), (3.1) and (3.2)

p(zt |xt) ∼ N (h(xt),R) (3.7)

The aim of inference is to recursively estimate the posterior distribution p(x1:t | z1:t) and

the marginal distribution p(xt | z1:t). The technique of evaluating the marginal distribution

p(xt | z1:t) is called filtering.

The recursive calculation of the marginal distribution consists of two steps: the state predic-

tion step and the measurement update step. The measurement update step then can again

be divided into measurement prediction and Bayesian update step.

In the prediction step, p(xt | z1:t−1) is evaluated by marginalizing the prior states xt−1 from

the state transition probability p(xt |xt−1) in (3.6)

p(xt | z1:t−1) =

∫p(xt |xt−1)p(xt−1 | z1:t−1)dxt−1 (3.8)

In the measurement prediction step, observed state p(zt | z1:t−1) is predicted given p(xt | z1:t−1)

p(zt | z1:t−1) =

∫p(zt |xt)p(xt | z1:t−1)dxt (3.9)


In the final step of Bayes’ update, we apply Bayes’ rule to find p(xt | zt) from p(zt |xt) and

p(xt |x1:t−1)

p(xt | zt) =p(zt |xt)p(xt | z1:t−1)

p(zt | z1:t−1)(3.10)

p(xt | zt) = ηp(zt |xt)p(xt | z1:t−1) (3.11)

p(xt | z1:t) = η p(zt |xt)

∫p(xt |xt−1)p(xt−1 | z1:t−1)dxt−1 (3.12)

where η is the normalization constant given by p(zt | z1:t−1). The expression in (3.12) can

be evaluated analytically for the case of LDS with Gaussian prediction and measurement

noise, as in (3.2) and (3.4). Kalman filters [32] work well in certain cases (e.g., systems with

unimodal noise) for evaluating (3.12). Otherwise, sample based approaches such as particle

filter [26] are appropriate.

Algorithm 4 Bayesian filtering to perform exact inference in state space model

1: procedure BayesF ilter

2: x0 ← initialState, p(x1 |x0)← µ0,Σ0

3: for t = 1 to T do

4: Predict the next state given previous observations: p(xt | z1:t−1) . Equation (3.8)

5: Measurement prediction step : calculate p(zt | z1:t−1) . Equation (3.9)

6: Bayes’ Update step : calculate p(xt | z1:t) . Equation (3.12)

7: (µt,Σt)← p(xt | z1:t)

8: return µ1:T ,Σ1:T

Kalman filters and extended Kalman filters rely on the assumption that the noise in the

measurements and prediction steps are Gaussian. That is not usually the case in the real


world scenario, and hence they fail to take into account all the salient statistical features of

the processes under consideration [7]. For example, the integral in (3.12) becomes analytically

intractable if the prediction and the measurement model are not Gaussian.

To tackle these shortcomings of analytical approximate solutions for non-linear or non-

Gaussian cases, simulation based techniques for inference in filtering are used. They provide

convenient and attractive approach to calculate the posterior distribution in (3.12). These

simulation based filters have a specific name in the literature, sequential Monte Carlo meth-

ods [7].

3.2 Particle Filters

Sequential Monte Carlo methods are also regarded as particle filters in the literature. In

particle filtering based state estimation, the probability distribution over the latent state xt

is represented by a set of particles. Each particle represents a possible value for the state

[27]. In particle filtering, the integral in (3.12) is replaced with the sum of the probability

measures for each of the particles.

3.2.1 Basic Description for AUVs

Consider an AUV which has a prior model of the depth around the given area. To localize

the AUV, we represent its position with a number of particles in the map where each of

those particles is a probable location of the AUV. Each particle has a full description of

the possible future states. As the AUV receives new depth measurements, it calculates the


likelihood of the observation given each particle location using the map model, in this case

a GP. Particles with low likelihood for the observation are discarded as this means they

are inconsistent with it. Particles with higher likelihood are re-sampled and propagated

according to the prediction distribution as given in (3.6). Finally, assuming the depth is

modeled correctly and the values of the measurement noises are close to the real values,

most particles converge to true location of the AUV.

3.2.2 Inference using Particle Filters

The conditional distribution p(xt | z1:t) is known as the belief state. It is an estimate of the

AUV’s current state at time step t and is expressed as a set of weighted samples(particles)

Xt = {xit, w

it}i=1,...,NP

where xit is the ith particle for the current state, wi

t is the corresponding

weight for the particle for current belief and NP is the number of particles used for estimating

the state. Under such a representation, belief p(xt | z1:t) in (3.12) can be approximated as

[7]:

p(xt | z1:t) = η p(zt |xt)

NP∑i=1

p(xit |xi

t−1)wit−1 (3.13)

For estimating the states sequentially using particle filters according to the approach outlined

in Algorithm 4, we start with the initial estimate of the state x0 with randomly distributed M

particles. Here each particle represents the possible value for the state. Then we recursively

follow the following steps to get sequential estimate of belief p(xt | z1:t) at each time step:

1. Generate another set of particles by propagating the current set using the transition


probability given in (3.6) xit ∼ p(xi

t |xit−1)

2. Each of the newly generated particles xit is weighted by the likelihood of the most

recent measurement zt given the sampled state xit. The likelihood is given by the

measurement model p(zt |xit) as given in (3.7). After this step we have a set Xt of the

particles and their weights.

3. Importance sampling of the particles in the set Xt is done where particles with less

weights are thrown away and particles with higher weights are duplicated. Re-sampling

avoids the scenario where after some time only one particle has non-zero weight.

In most of the cases, the state of the process at each time step is approximated as the

sample mean of the particles E[xt] = ΣNPi=1w

itx

it.

Algorithm 5 Estimating states using Particle Filter with importance sampling

1: procedure ParticleF ilter

2: {xi0}i=1,..,NP

← initialState, p(xt |xt−1) ← transitionDistribution, p(zt |xt) ←measurementModel


4: Importance Sampling :

5: Sample {xit}i=1,..,NP

∼ p(xt |xt−1)

6: Evaluate importance weights {wit}i=1,..,NP

← p(zt |xit)

7: Xt = {xit, w

it}i=1,...,NP

8: Selection:

9: Re-sample with replacement N particles from the set Xt using the weights

10: µt ← E[xt] = ΣNPi=1w

itx

it, Σt ← V[xt]



3.3 GP based Particle Filters

This section describes particle based filtering scenario where the observations are modeled

as a Gaussian process model.

3.3.1 GP as Observation Model

Particle filters, that use GP to model the observation given state variable, have been used in

the past for inference over various state space models [13], [14]. As GPs are a nonparametric

model, they have infinite dimensional parameter space. The effective complexity of the GP

model adpats to the data. This results in GPs being one of the better choices where there is

a dearth of training data and the data points are scattered.

A GP model is fit on training points which are known a priori for the system or are sampled

from the observation of the system. Given the training setD ∈ {(x0, z0), (x1, z1), ...(xm, zm)} =

{X,Z}, where xi represents the true state and zi represents the observation from the prior

data, we fit a Gaussian process regression over it. As a result, the observation likelihood

p(zt |xt) required in (3.13) is given by the predictive distribution of the GP model as given

in (2.14). This gives

p(zt |xt) ∼ N (µGP (xt,D),σGP (xt,D)) (3.14)


where

µGP (xt) = −K(xt,X)[K(X,X) + σ2nI]−1Z (3.15)

σGP (xt) = K(xt,xt)−K(xt,X)[K(X,X) + σ2nI]−1K(X,xt) (3.16)

here we consider zero mean GP prior. Using (3.14) as the observation model, the particle

filtering technique can be used as mentioned in Algorithm 6 to get the estimate of the state

x1:T

Algorithm 6 Estimating states using Particle Filter with GP based observation model

1: procedure GP − ParticleF ilter2: D ← X,Z get training data for observation model

3: {xi0}i=1,..,NP

← initialState, p(xt |xt−1) ← transitionDistribution, p(zt |xt) ←N (µGP (xt,D),σGP (xt,D))




∼ p(xt |xt−1)


← p(zt |xit)

8: Xt = {xit, w

it}i=1,...,NP

9: Selection:

10: Resample with replacement N particles from the set Xt using the weights


itx

it, Σt ← V[xt]


3.3.2 On-line updating of observation model

In a real world scenario, having a prior training data D which spans the whole domain of

possible states x is unfeasible.Usually, the prior training data spans the data points near to


xt−1

zt−1

State Variables

Observations

xt

zt

g(.)

h(.)Gaussian Process Model

xt+1

zt+1

Figure 3.2: Graphical model for a state-space model with observation model modeled as a

GP

the starting state of the system. As a result, we need to be able to learn the observation

model represented by GP as we explore more states.

In Algorithm 7, line (20) shows us the calculation for with the estimated mean and the

variance of the current state of the system. Our goal is to use this estimate xt ∼ N (µt,Σt)

and the observation zt made by the system at current time step to re-learn the observation

model, in this case GP. As a result, at each time step we receive an input-output pair of

(xt ∼ N (µt,Σt), zt ∼ N (zt,νt)) that can be added to the GP model. As these inputs are

uncertain, we can use (2.25) and (2.26) from Section 2.7 to find the predictive mean and

variance of observation model p(zt+1 |xt+1) at next time step. Training data Dt at each

time step can be represented as Dt ∈ {Xt,Zt} where Xt ∈ X ∪ x1:t−1 and Zt ∈ Z ∪ z1:t−1.


Predictive mean and variance for zt, calculated in a manner similar to (2.25) and (2.26), are

µGP (xt) = −K(xt,Xt)[K(Xt,Xt) + σ2nI + diag{∆>z ΣXt∆z}]−1Zt (3.17)

σGP (xt) = K(xt,xt)−K(xt,Xt)[K(Xt,Xt) + σ2nI + diag{∆>z ΣXt∆z}]−1K(Xt,xt) (3.18)

where diag{∆>z ΣXt∆z} is the additional term from the uncertainty of the input points Xt.

The vector ∆z represents the derivative of the GP function values with respect to the input

vector xt, and ΣXt is a diagonal matrix with diagonal elements as the noise associated to

each input dimension in Xt. As more points are added to the GP, we have to update

the input vector Xt, output vector Zt and the inverse of the modified covariance matrix

(K(Xt,Xt) +σ2nI + diag{∆>z ΣXt∆z}) for the model. Updating the input and output vectors

is fairly straightforward. However, updating the inverse of the covariance matrix is a bit more

complicated. It can be done by updating the Cholesky factorization of the covariance matrix

[24] and [18]. The computational complexity of this update is O((N + t − 1)2) where N is

the size of prior training input X. For on-line inference, this is manageable upto a threshold

value of t. For the systems where on-line learning has to be done for a long duration of time,

this approach scales poorly with time.

After incorporating more data points into the GP model, as the system encounters more

states, the hyperparameters no longer represent the optimal model. To reduce the bias

of the prediction from the observation model, we need to re-learn the hyperparameters

after including a set of data points into the model. This is again done by maximizing the

likelihood as in Section 2.5.2. If the dimension of each input vector xt is q, we have q extra

hyperparameters compared to the GP model discussed in Section 2.5.2 - one noise variance


parameter per input dimension.

Algorithm 7 GP based Particle Filter with on-line updating

1: procedure GP − ParticleF ilter2: X1,Z1 ← X,Z, D1 ← X1,Z1 get training data for observation model

3: {xi0}i=1,..,NP

← initialState, p(xt |xt−1)← transitionDistribution




∼ p(xt |xt−1)

7: zt ← observation

8: p(zt |xt)← N (µGP (xt,Dt),σGP (xt,Dt))


← p(zt |xit)

10: Xt = {xit, w

it}i=1,...,NP

11: Selection:



itx

it, Σt ← V[xt]

14: Xt+1 ← Xt ∪ µt, Zt+1 ← Zt ∪ zt

15: Dt+1 ← Xt+1,Zt+1

16: UpdateGramMatrixOfGP (Dt+1)

17: if t%m == 0 then

18: [θ,ΣXt ]←MaximumLikelihoodEstimate(p(zt |xt))

19: . Learn Hyperparameters

20: return µ1:T ,Σ1:T ,

3.4 Local GP based Particle Filters

In the previous section we saw how we can use a GP as an observation model for a state space

model and use particle filtering to infer the hidden states. However, if we were to update

the GP on the go, as we encounter more states, the time complexity scales proportional to

O(t2) where t is the number of states encountered so far. Therefore there is a need to have


an observation model whose computational complexity does not scale quadratically with the

number of states encountered.

xt−2

zt−2

xt−1

zt−1

State Variables

Observations

xt

zt

g(.)

LGPi−1 LGPiLocal Gaussian Processes

LGP model

xt+1

zt+1

xt+2

zt+2

h(.)

LGPi+1

Figure 3.3: Graphical model for a state-space model with LGP as the observation model.Here

xt denote the latent states of the system and zt denote the observations made by the system.

The nodes LGPi model the local GP models using a subset of the observations and corre-

sponding states. h() is the observation model which consists of all the local GP models. g()

is the prediction model of the system.

In Section 2.9, we saw that if the dataset is spatially correlated, local approximation of

Gaussian processes can handle the issue of poor scalability of GP models by bounding the

time complexity for large amount of data even when the data is streamed and incremental

updates need to be performed. As a result, it makes sense to use LGP model as the obser-

vation model in state space inference when we need to update the model at each new state.


Figure 3.3 shows the graphical model for a state space model with LGP as the observation

model.

In Chapter 2, LGP is described as a collection of GPs modeled on disjoint datasets. A

prior training data set D ∈ {X,Z} is used to learn a LGP model as in Algorithm 1. This

serves as our observation model as was the case in Section 3.3 where we used a normal

GP model fit on D as the observation model. We follow the approach outlined by Algo-

rithm 8 for state estimation with LGP as observation model in particle filter. We start

with fitting a local GPmodel LGP over the prior data D1. This results in D being split

into M clusters {D1[1],D1[2], ...D1[M ]} where subscript denotes the time step for each iter-

ation of the particle filter. Fitting a LGP model on this data results in different GP models

{LGP1, LGP2, ..., LGPM}, where each of them is learned on a different subset of the parti-

tioned data. As our system proceeds with time, with each particle update and observation

lines 7 and 8, the particle weight is calculated based on the estimate of p(zt |xit) using esti-

mate from the nearest local model (Algorithm 2). From lines 15 to 21, importance sampling

of the particles is done and the mean estimate of the state is calculated using the re-sampled

states. Based on this mean estimate µt, we find the nearest local model from it. Subse-

quently, the new input output pair of data point, (µt, zt) is added to the nearest model

and its gram matrix is updated (lines 18 to 20) using the same factorization techniques as

mentioned in previous section .


Algorithm 8 Local GP based Particle Filter with on-line updating

1: procedure LGP − ParticleF ilter2: X1,Z1 ← X,Z, D1 ← X1,Z1 get training data for observation model

3: LGP, {D1[1],D1[2], ...D1[M ]} ← PartitionAndLearnLGP (D1,M)

4: {xi0}i=1,..,NP





∼ p(xt |xt−1)

8: zt ← observation

9: for i = 1 to N do

10: µLGPk(xi

t,Dt),σLGPk(xi

t,Dt)← PredictLGP (xit, )

11: p(zt |xit)← N (µLGPk

(xit,Dt),σLGPk

(xit,Dt))


← p(zt |xit)

13: Xt = {xit, w

it}i=1,...,NP

14: Selection:

15: Re-sample with replacement N particles from the set Xt using the weights


itx

it, Σt ← V[xt]

17: k ← findNearestModel(µt, LGP )

18: Xt+1[k]← Xt[k] ∪ µt, Zt+1[k]← Zt]k] ∪ zt

19: Dt+1[k]← Xt+1[k],Zt+1[k]

20: UpdateGramMatrixOfGP (Dt+1[k])

21: for j = 1 to M and j 6= k do

22: Xt+1[j]← Xt[j], Zt+1[j]← Zt]j]

23: Dt+1[j]← Xt+1[j],Zt+1[j]

24: if mth new point for model k since last update then




Chapter 4

Terrain-Aided Navigation

This chapter describes how an AUV fitted with sensors such as AHRS (attitude and heading

reference system ) and DVL (Doppler Velocity Log) uses the state space inference algorithms

described in the previous chapters to navigate underwater. In this chapter a scalable and

on-line Gaussian process based terrain-aided navigation (TAN) algorithm is discussed which

is specific to the sensors (DVL and AHRS) used in the Virginia Tech 690s (Figure 4.1) .

4.1 The Terrain-Aided Navigation Problem

The terrain-aided navigation problem refers to the generalized problem of localization using

an a priori known map. Localization is done with the help of different sensors which measure

various characteristics of the terrain and reference them to information in the a priori map

[4].

Terrain-aided navigation is particularly suitable in the areas where GPS is denied or GPS

readings are noisy like under water, indoor spaces, etc. In this work, an AHRS provides

40

Abhilash Chowdhary Terrain-Aided Navigation 41

Figure 4.1: The Virginia Tech 690s during Panama City trials

attitude measurements and a DVL provides body velocity and range measurements to the

seafloor. Using attitude and velocity, the future state of the AUV is predicted using a

kinematic model.

Let us define the terrain-aided navigation problem and state space variables with respect to

an AUV with AHRS and DVL as its main sensor unit and a priori depth map of the sea

bed near the initial location.

4.1.1 Problem Statement

Let

• pt = [x y z φ θ ψ] be the pose of the robot at time t with respect to north-east down


(NED) frame

• vt = [u v w p p r] be the robot velocity with respect to robot fixed frame

• M be the a priori map of the terrain with depth points at various locations.

• xt be the state vector of the robot which is estimated over time. In this work it is only

the position of the robot in X and Y coordinates in world frame (NED).

• zt be the observation acquired by the robot. In this work, observations are altitude

measurements from multibeam DVL on the z axis of inertial earth fixed frame.

The aim of terrain-aided navigation is to estimate the states xt over a period of time by

matching the observation zt against the a priori map M [4].

4.2 State Space Model for TAN

As the problem of TAN requires to infer the latent states xt over a period of time given

observations zt, this could well be modeled as a state space model. For inference, we could

use a Bayesian filtering algorithm as described in the previous chapters. The prediction

model and observation model of the filtering mechanism to estimate the states is described

below.


4.2.1 Prediction Model

In this work, we do not estimate the value of z and are only concerned with the 2-D local-

ization of the AUV in the X-Y plane of earth inertial frame. For terrain-aided navigation

algorithm to work well, we assume that the AUV has a zero pitch for the whole mission. As

a result, the AUV’s state is taken as xt = [x y]. In a particle filtering scenario, the particle

set can be defined as

xt =

x1t ... xNP

t

y1t ... yNP

t

(4.1)

The prediction model for the AUV is expressed

xt =f(xt−1) + εt (4.2)

f(xt−1) =xt−1 + ∆txt−1 (4.3)

xt−1 =[ut−1 vt−1] (4.4)

where (4.3) is based on the kinematic model of the AUV, and ut and vt are the velocity

components along X-axis and Y-axis respectively. The values of ut and vt at each time step

are the measurements from the DVL. Using (4.3) and (4.4), (4.2) can be written

xt = xt−1 + ∆t[ut−1 vt−1] + εt (4.5)

where εt denotes the noise in the prediction of the current state from previous. In a manner

similar to (3.2), we take εt as a Gaussian noise with zero mean εt ∼ N (0,Q). Thus the

current state xt is from a normal distribution with mean f(xt−1) and variance Q that is.

p(xt |xt−1) ∼ N (xt−1 + ∆t[ut−1 vt−1],Q) (4.6)


4.2.2 Observation Model

An observation model yields the conditional distribution p(zt |xt). To get p(zt |xt), we fit a

Gaussian process model over the prior map M. A GP model provides a mapping from the

current state space to the observation space. The prior map M being too sparse over the area

of mission, cannot directly be used as the mapping function between these two spaces. In this

work we use LGP to model the prior depth readings from the given map M in a manner simi-

lar to Section 3.4. The LGP model is initialized and learned as mentioned in Section 2.9. For

this case, data setD is represented by the M ∈ {((x1, y1), d1), ((x2, yt2), d2) . . . ((xN , yN), dN)}

where (xi, yi) represents the location of the depth reading di in north east down (NED) frame.

The initial input to the GP model can be represented as X1 ∈ {(x1, y1), (x2, y2) . . . (xN , yN)}

where the subscript 1 denotes that this is the training set before the estimation process be-

gins. The output for this training set of the GP can be written as Z1 ∈ {d1, d2 . . . dN}. So the

a priori training set can be represented as D1 ∈ {X1,Z1}. This data set is then partitioned

into M clusters based on the inputs X0. The partitioned data sets can be represented simi-

larly as in Section 3.4 {D1[1],D1[2], ...D1[M ]} where D1[i] represents the partitioned training

data for each of the cluster.

A GP model is learned on each of the partitioned training sets D1[i] i = 1, . . . ,M . Each of

the GP models is denoted LGPi, for the ith partition. The mean prediction and the predictive

variance at a test point xt at time step t is computed µLGPi(xt,Dt[i]) and σLGPi

(xt,Dt[i])

respectively for each local GP model LGPi. The final predictive distribution of the measure-


ment zt given xt is a weighted measure of distributions from the k nearest LGP models

p(zt |xt,Dt) ∝k∑

i=1

viN (µLGPi(xt,Dt[i]),σLGPi

(xt,Dt[i])) (4.7)

where the k nearest neighbors are found based on the nearness measure vi as computed

in (2.27). Concisely, (4.7) describes the observation model for the terrain-aided navigation

algorithm used in this work.

4.2.3 Multibeam DVL

DVL used in this work has four beams which gives four range measurements from the sea

floor. Each of the beams are at predetermined angles from the body frame z-axis. Let

r1, . . . , r4 represent the range measurements from each of these beams of the DVL. Using the

geometry related to the angle of the beam with respect to each of the axis of the coordinate

frames, we calculate the vertical component of each range measurement and the location on

the seafloor corresponding to each range measurements. Thus we compute the depth at four

separate locations x1,t, . . .x4,t. These depth measurements are h1, . . . h4.

4.3 Particle Filtering strategy for multibeam DVL

For incorporating the measurements h1, . . . h4 the prediction and the LGP model remains

same as in Section 4.2.1 and Section 4.2.2. However, likelihood of the observations p(zt |xt)

changes. In this section, re-modeling of likelihood in a particle filter to incorporate multibeam

measurements is shown.


4.3.1 Likelihood for re-sampling

In Section 3.2.2 we used likelihood p(zt |xit) of the observation zt as the weight for re-

sampling each particle xit. When using multibeam DVL, observation at each time step t

is zt = [h1, h2 . . . h4]. The likelihood then becomes p(h1, h2 . . . h4 |xit). In our observation

model, all the altitude measurements h1, h2 . . . h4 at time step t are independent of each

other given the previous observations(Zt), and previous states(Xt) because the LGP model

is learned on (Xt,Zt). This likelihood can be factorized as below:

p(zt |xit) = p(h1, h2 . . . h4 |xi

t,Xt,Zt)

= p(h1, h2 . . . h4 |xi1,t,x

i2,t . . .x

i4,t,Xt,Zt)

= p(h1 |xi1,t,x

i2,t . . .x

i4,t,Xt,Zt)p(h2 |xi

1,t,xi2,t . . .x

i4,t,Xt,Zt) . . . p(h4 |xi

1,t,xi2,t . . .x

i4,t,Xt,Zt)

= p(h1 |xi1,t,Xt,Zt)p(h2 |xi

2,t,Xt,Zt) . . . p(h4 |xi4,t,Xt,Zt)

∴ p(zt |xit) = p(h1 |xi

1,t,Xt,Zt)p(h2 |xi2,t,Xt,Zt) . . . p(hb |xi

b,t,Xt,Zt) (4.8)

where Xt and Zt denote the observations prior to time step t. Here each of the terms

{p(hl |xl,t,Xt,Zt)}4l=1 is evaluated from the LGP model using (4.7).

4.3.2 Inference

The inference in a TAN problem refers to estimating the state variables xt of the system over

a period of time T such that 1 ≤ t ≤ T . In this work we use the particle filtering framework

with LGP as the observation model for estimation of xt (section 3.4). Re-sampling of the

particles is done based on the value of p(zt |xt) which is calculated as shown above.


The recursive procedure for estimating the states can be summarized as :

1. Generate another set of particles by propagating the current set using the transition

probability given in 4.6 xit ∼ p(xi

t |xit−1)

2. For each of the particles xit we have four points xi

1,t,xi2,t . . .x

i4,t. And each of these

points in X-Y plane correspond to an altitude measurements from each of the four

beams of the DVL. The weight of each of the newly generated particles xit is given

by the likelihood p(zt |xit). After this step we have a set Xt of the particles and there

weights.

3. Importance sampling of the particles in the set Xt is done where particles with less

weights are thrown away and particles with higher weights are duplicated. Resampling

avoids the scenario where after some time only one particle has non-zero weight.

We approximate the state of the process at each time step by the sample mean of the

particles E[xt] =∑N

i=1witx

it.

Detailed procedure for performing inference and learning is shown in Algorithm 9.

4.4 Cost of running into the shore

To plan a path for the AUV, we need to quantify the cost associated to the path so that the

path with the minimum cost is selected. For many underwater missions, an AUV needs to

maintain a constant depth of dthreshold from the surface of water. When surveying near the


Algorithm 9 Local GP based Particle Filter (LGP-PF) with multibeam DVL measurements

1: procedure LGP − ParticleF ilter −MultiBeam

2: X1,Z1 ← X,Z, D1 ← X1,Z1 get training data for observation model

3: LGP, {D1[1],D1[2], ...D1[M ]} ← PartitionAndLearnLGP (D1,M)

4: {xi0}i=1,..,NP





∼ p(xt |xt−1)

8: zt = [h1, h2 . . . hb]← observation

9: for i = 1 to NP do

10: [LGP1, LGP2 . . . LGPk]← kNearestLGPModels(xit, LGP )

11: for l = 1 to k do

12: vl ← exp(− 1

2(x− ck)>Wk(x− ck)

)13: µLGPl

(xit,Dt),σLGPl

(xit,Dt)← PredictLGP (xi

t, Dt)

14: for j = 1 to b do

15: p(hj |xij,t)←

k∑l=1

vlN (µLGPl(xi

j,t,Dt[l]),σLGPi(xi

j,t,Dt[l]))


← p(h1 |xi1,t)p(h2 |xi

2,t) . . . p(h4 |xi4,t)

17: Xt = {xit, w

it}i=1,...,NP

18: Selection:



itx

it, Σt ← V[xt]

21: k ← findNearestModel(µt, LGP )

22: Xt+1[k]← Xt[k] ∪ µt, Zt+1[k]← Zt]k] ∪ zt

23: Dt+1[k]← Xt+1[k],Zt+1[k]

24: UpdateGramMatrixOfGP (Dt+1[k])

25: for j = 1 to M and j 6= k do

26: Xt+1[j]← Xt[j], Zt+1[j]← Zt]j]

27: Dt+1[j]← Xt+1[j],Zt+1[j]

28: if mth new point for model k since last update then





shore, this becomes challenging due to the risk of AUV running into the shore. The cost of

a path then can be quantified as the probability of the AUV running into the shore given

the path between two points. Here the aim is not to come up with various paths which

are feasible, but quantify the cost associated to each path given a list of paths between two

points.

A path P from point A to point B on North-East Down frame for an AUV can be discretized

into a finite number (say q) of path points (c1, c2 . . . cq) where each path points ci denotes the

location in X-Y coordinates of the discretized point on the NED frame. Using the observation

model (GP or LGP) already fit on the a priori map, we can find the predictive distribution

of depth zci at each of the path point ci (2.14). The cost of running into the ground at

ci then can be quantified as P (zci < dthreshold | ci), the probability of predicted depth being

greater than the depth threshold dthreshold. This is given by

lci = P (zci < dthreshold | ci) =

∫ dthreshold

−∞p(z | ci) dz (4.9)

where p(z | ci) is evaluated using (4.7). As each of P (zci < dthreshold | ci) is a normal distri-

bution, (4.9) can be rewritten [31]

lci = Φ(dthreshold − µci

σci

)(4.10)

where Φ(x) denotes the cumulative distribution function of standard normal distribution

and, µci and σci are the mean and variances of p(zci | ci).

Chapter 5

Experimental Evaluation

This chapter provides results from experimental evaluation of the LGP-PF based localization

(Algorithm 9) of the AUV. At first, the LGP-PF method is evaluated in a simulated setting

with a synthetic prior map data and simulated trajectories of the AUV over the map. The

algorithm is tested in a real world setting with actual depth data and trajectory of the AUV

from trials performed near Panama City, Florida in St. Andrew Bay.

5.1 Simulation based Experiments

For simulation based experiments, a grid of 600×300 meters, as shown in Figure 5.1, was

initialized with ground truth depth values. There are total of 180,000 points on the grid

with ground truth depth measurements. The depth values range from 0 to 85 meters. A

prior mapM was created with 1900 random points by adding a Gaussian noise of zero mean

and one standard deviation to each ground truth depth measurement. Using this sparse

collection of depth points in M, a GP/LGP model is fit over the whole grid. This becomes

50

Abhilash Chowdhary Experimental Evaluation 51

the observation model for localizing the AUV, as explained in Section 4.2.2.

Figure 5.1: 3D plot of 600×300 grid depicting the terrain of the seafloor. 0 on the Z-axis

represents the bottom most point of the floor from the surface.

5.1.1 Comparison of Local and Sparse Approximation based meth-

ods

The mean prediction and predictive variance for both local approximation based LGP model

and sparse approximation based GP model are compared. The LGP model used for synthetic

experiments has 30 local GP models. The sparse approximation implementation for the GP

is used from [10]. The root mean squared error for the prediction of depth over the whole


grid comes out to be 7.27 meters for LGP and 4.17 meters for sparse approximation(Figure

5.2). Although, the error for LGP is almost greater by a factor of 2, the run time of the LGP

algorithm is more than 5 times faster than the sparse approximation based implementation.

Figure 5.2: Comparison of depth prediction over the synthetic data of Figure 5.1 using 1900

random points with known depth values. Figures on the left show the mean depth prediction

and bias associated with it, for an LGP model with 30 clusters and Gaussian(RBF) kernel.

Figures on the right show the mean depth prediction and bias associated with it, for an

sparse approximation for GP over all the 1900 points. The root mean squared errors in

depth prediction for LGP and sparse GP based model were 7.27 meters and 4.17 meters

respectively.

.


5.1.2 Simulating AUV’s Trajectory

The AUV is represented as a point object in the grid, and the speed of the AUV is taken as a

constant, 1 m/s. Ten straight line paths are sampled for the AUV over the grid starting from

a random points. With the starting position of each of these paths known and a LGP model

fit over prior map as the observation model, trajectories of the AUV are estimated using

LGP-PF algorithm with sampled variance in the prediction model. Using these location

estimates and corresponding depth values from the grid, the LGP model is re-learned. With

the re-learned LGP model, the trajectories for each of the sampled ten paths are again

estimated using LGP-PF with sampled variance in the prediction model. Figure 5.3 (a)

shows the ten sampled paths. Figure 5.3 (b) shows the estimated trajectories using LGP-PF

with prior LGP model and Figure 5.3 (c) shows the estimated trajectories using LGP-PF

with LGP model re-learned using estimates from 5.3 (b). The Euclidean error et at time t

for a sampled path is expressed

et =√

(xt − xt)2 + (yt − yt)2 (5.1)

where (xt, yt) is the location of AUV in the sampled path at t and (xt, yt) is the estimated

location of AUV using LGP-PF at time t for the sampled path. Our intuition is that the

errors for trajectory estimates from LGP-PF with re-learned LGP model should be less than

the errors for trajectory estimates from LGP-PF with prior LGP model. Figure 5.3 (d)

shows the averaged Euclidean error (5.1) for the estimated trajectories in Figure 5.3 (b) and

Figure 5.3 (c). We see that the trajectory estimates using re-learned LGP model gives less


Figure 5.3: Shows the comparison of ten estimated trajectories from LGP-PF with prior

LGP model and re-learned LGP model.

error than the ones using prior LGP model. Figure 5.3 (e) shows the Euclidean errors (5.1)

for each of the estimated trajectories in Figure 5.3 (c).

5.1.3 Cost of Running into the Shore

We calculate the cost of the AUV to run into the shore while moving at a depth of 30 meters

below the surface. This risk denotes the probability of AUV running into the shore while at

a depth of 30 meters at that point. Figure 5.4 shows the plot of cost of AUV running into


Figure 5.4: Plot of cost of an AUV running into the shore, moving at a depth of 30 meters

below the surface of water.

the shore in the sampled grid if it were moving at a depth of 30 meters from the surface of

water. Red colored regions represent higher risk areas and blue colored regions represents

areas incurring lower costs.

5.2 Real world Experiments

5.2.1 The Virginia Tech 690s AUV

The Virginia Tech 690s AUV, that was used for field trials, is described in this section.

The sensors in the AUV that are used for these experiments are listed in the Table 5.2.1.

The Microstrain 3DM-GX3-25 AHRS reports the attitude (roll, pitch and yaw) of the AUV.


Measurement Sensor

Attitude Microstrain 3DM-GX3-25 AHRS

Altitude from bottom and Velocity NavQuest 600 Micro DVL

Absolute Position Linx RXM-GPS-R4 GPS receiver

Depth from water surface Keller Series 30X pressure sensor

Table 5.1: Sensors in the Virginia Tech 690s

For the kinematic model, only yaw is used in our navigation algorithm. However, roll,

pitch and yaw values are also used for transforming the state variables from body frame

to NED frame. NavQuest 600 Micro DVL is a small four beam DVL that outputs the

four range measurements to the bottom of the water body and also the velocity of the

AUV in body frame. The Linx RXM-GPS-R4 GPS measures position of the vehicle in

geodetic coordinates. This position consists of latitude(λ), longitude(φ), and altitude(h).

These coordinates are transformed to NED frame to get the position of the AUV in NED

frame. The transformation matrix is generated using the roll, pitch and yaw readings from

the AHRS. Keller Series 30X pressure sensor measures the surrounding absolute pressure.

The depth of the AUV is directly proportional to the pressure measured by the sensor and

therefore this can be used to find the depth from the surface. The readings from this sensor

are used to control the AUV at a constant depth below the surface of water.

5.2.2 Field Trials

The Virginia Tech 690s was deployed near Panama City, Florida, in St. Andrew Bay. The

AUV was commanded to go from one location to another between four different way-points.


Figure 5.5: Prior depth map with data points represented as black dots (4834 in number).

The contour plot shows predictive mean depth from sparse GP model fit on the training

data points from first and second mission.

There were two way-point missions that were conducted. The first mission was a surface

mission where the AUV did not dive and remained on the surface. This was done to acquire

a prior depth map of the floor along with position from the GPS. The second mission was

carried out at a depth of 2 meters from the surface of water. The way-points were distributed

in a diamond pattern with the adjacent ones being about 260 meters away from each other.

The AUV surfaced at four of the way-points to get GPS measurements.

To infer and model depth at unknown location, LGP model is fit on the depth data points

and their corresponding GPS location from the first mission. However, this did not cover

much of the AUV’s path in the second mission. Hence, some of the location estimates

from the range based navigation approach in [12] (which was already implemented on to the


Figure 5.6: Prior depth map with data points represented as black dots (4834 in number).

The contour plot shows predictive mean depth from LGP model fit on the training data

points from first and second mission.

AUV’s processing unit) paired with the depth readings from DVL, were also used to train

the model. These data points were taken at a frequency of 0.5 Hz. Figure 5.5 shows sparse

GP based prior map and the training data points from the first and second run used to build

the model. Total number of input data points used for training are 4834. Figure 5.6 shows

sparse GP based prior map and the training data points from the first and second run used

to build the model. Although the prediction of depth is quite smooth using sparse GP model,

computationally this model is expensive as it has to select the optimal subset of inducing

input data points for the given data set. And this is also not feasible for on-line learning

approach where more input data points are added continuously with time. Therefore, for

estimating the position of AUV we only use the prior map learned from LGP model.


Figure 5.7: The Virginia Tech 690s AUV trajectory for Panama City trials as estimated by

LGP-PF. It is also compared to range-based solution in [12]. GPS updates are shown as

red dots and the numbers next to them denote the sequence of these updates. Blue dots

represent range based estimate. Yellow dots represent LGP-PF based estimate.


We tested our LGP-PF algorithm (Algorithm 9) off-line on a 1.6 GHz Intel Core i5 computer

with the data from the second mission. The implementation of the algorithm was done in

Python and GPy library [10] was used for inference and learning of Gaussian processes.

Our algorithm was tested against the location estimates from the range based navigation

approach in [12]. As the AUV was under water over the course of mission and only resurfaced

at the 4 way-points, the location estimate from our LGP-PF algorithm was also compared

to the estimates from the range based solution. With the data from the sensors, depth and

velocity measurements from DVL and yaw readings from AHRS, AUV is localized using

LGP-PF algorithm. The LGP-PF based localization approach used in this work does not

use the GPS updates for correcting its location estimate AUV unlike [12].

The DVL used in the AUV has four beams that provides four different altitude measurements

above the seafloor at a single time step. Figure 5.7 shows the estimated trajectory from LGP-

PF compared to the estimation from [12] and four GPS updates when the AUV resurfaces. In

the kinematic model of LGP-PF, the variance used was 0.05meter2. For this implementation,

50 particles were used for the estimation in the particle filter. For learning the LGP model,

prior depth points were clustered into 30 different clusters and RBF covariance kernel (2.8)

was used for GP model over all these clusters.


Figure 5.8: This figure shows three different areas on the predicted depth map from LGP.

White region denotes where the prediction was zero and is neglected. Black dots represent

the down-sampled prior depth data points used for learning the LGP model.

Based on the number of prior data points and the terrain profile of the region of field

trials, the predicted depth map is divided into four different regions of north, south, east

and west. Figure 5.8 shows these four regions on a contour plot of mean predicted depth

using the LGP model along with down-sampled prior depth readings as black dots. Along a

trajectory with significant depth variation the LGP-PF algorithm excludes the unimportant


Figure 5.9: This figure shows the AUV starting in the south (initial start point) and it

resurfaces in north with GPS update. The error in estimation is about 10 meters.

particles for AUV’s position and as a result the location estimation is likely to have less

errors. However, along a trajectory with low depth variation, particles in LGP-PF algorithm

tend to have similar weights and the AUV’s state is distributed uniformly over the state

space after importance sampling which results in location estimate to have high error values.

Figures 5.9, 5.10, 5.11 and 5.12 show the location estimates of the AUV using LGP-PF, and

the GPS updates from each of the four resurfaces of the AUV.


Figure 5.10: This figure shows the AUV starting from south and going to east via west and

north. The error in estimation is about 25 meters with respect to GPS update.


Figure 5.11: This figure shows the AUV starting from west and going to north via east. The

error in estimation increases to about 30 meters with respect to GPS update.


Figure 5.12: This figure shows the AUV starting from the south and going to east via west.

The error in estimation drops to about 8 meters with respect to GPS update.


In Figure 5.9, the AUV goes from south to north. As the predicted depth variation along

the path from south to north is significant, the estimation of state has low error relative to

the GPS update.

In Figure 5.10, the AUV goes from south to east passing through the northern and the

western regions. Because of starting from the southern region, which has almost no depth

variability, AUV initially accumulates significant error. In this trajectory while going from

west to north, the depth gradient is non-zero which decreases the error in LGP-PF. How-

ever, the northern region has almost non-varying predicted depth which results in AUV

accumulating more error in its location estimate. In the end while going from north to east,

the depth variability is again very low, which combined with already accumulated error,

increases the location estimation error to considerable amount (around 25 meters).

In Figure 5.11, the AUV goes from west to east passing through the northern region. Al-

though the depth variation while going from west to east is significant, due to low depth

variation from east to north and relatively non-varying depth in the northern region, the lo-

cation estimate error with respect to GPS updates is considerably large (around 30 meters).

In Figure 5.12, the AUV goes from south to east passing through the western region. Be-

cause of almost non-varying depth between south and west, the initial error is bound to be

significant. However, as the AUV traverses from west to east, the errors start converging

because of significant variation in depth along the path. This combined with AUV’s final

movement across the eastern region, which has varying predicted depth, lowers the error in

AUV’s estimated location with respect to GPS update when it resurfaces. The error drops


Figure 5.13: This figure shows the errors in location estimate using LGP-PF relative to GPS

updates at each of the AUV resurface. Negative error means AUV is underwater and does

not have GPS fix.

down to about 8 meters.

Figure 5.13 shows errors in the location estimates of the AUV using LGP-PF with respect

to four GPS updates over the whole trajectory. The error rises for the first three GPS

updates and it drops down to considerably low values for the last GPS update. Terrain-

aided navigation algorithm discussed in this thesis is not GPS aided at any of the GPS

update events and the correction in the AUV’s location estimate is solely done via LGP-PF.

This shows that using a LGP-PF based localization approach, the accumulated error along

a trajectory can be reduced to considerably low values if the terrain has significant depth

variation. This makes LGP based filtering approach a good estimation algorithm where


the terrain has significant depth variation. The results can be improved by increasing the

number of particles in the LGP-PF algorithm at the cost of increased computational time.

The LGP-PF based terrain-aided navigation algorithm starts accumulating errors if the

terrain is flat and therefore may not be a good method of navigation for the environment

with low depth variance in the terrain. With terrain profile being flat, the likelihood for

all the particles in the particle filter at a time step remains the same and therefore the

estimated state of the AUV gets uniformly distributed over a region in state space. This

results in poor approximation of state distribution. Therefore, to get a good approximation

in a terrain with high depth variance, the importance sampling in algorithm 9 needs a

weighting function which does not just rely on the likelihood of measured depth given LGP

model. This could be done by using additional sensors like inertial measurement unit (IMU)

and fusing their measurements as well in the observation model of LGP-PF.

Chapter 6

Conclusions

This work proposes a state-space inference algorithm, LGP-PF, using non-parametric filter-

ing technique with local approximation of Gaussian processes as the observation model over

the measurements. A LGP-PF based implementation of terrain-aided navigation algorithm

is also presented. To the best of our knowledge, this is the first work in using LGP based

observation model in a terrain-aided navigation problem for AUVs. This work shows that

given a terrain with high depth variance, the error in location estimate of LGP-PF based

terrain-aided navigation converges to acceptable low values. For a terrain with non-varying

depth, the algorithm’s estimate of AUV’s location has significant errors as compared to other

navigation algorithms. The proposed terrain-aided navigation algorithm was evaluated on

datasets from field trials near Panama City, Florida.

69

Abhilash Chowdhary Conclusions 70

6.1 Future Work

In this work, the experimental evaluation was done in a relatively less feature-rich terrain.

One of the future additions to this work would be testing this algorithm in an environment

with very rich terrain profile. Also, in the localization algorithm described in this work,

the observation model is only based on the measurements from a single sensor (multibeam

DVL in this case). By fusing more measurements from different sensors like IMU etc., the

estimation could be improved. Also the states of the filter only include the 2-D position

of the AUV. The states which are directly observable from the sensors, for example body

velocity, attitude, and depth, are not included. By including these states in another Bayesian

filter (like in [2]), the errors in their estimate could be reduced.

Bibliography

[1] Stephen Barkby, Stefan Williams, Oscar Pizarro, and Michael Jakuba. Bathymetric par-

ticle filter slam using trajectory maps. The International Journal of Robotics Research,

page 0278364912459666, 2012.

[2] Stephen Barkby, Stefan B Williams, Oscar Pizarro, and Michael V Jakuba. A featureless

approach to efficient bathymetric slam using distributed particle mapping. Journal of

Field Robotics, 28(1):19–39, 2011.

[3] Asher Bender, Stefan B Williams, and Oscar Pizarro. Autonomous exploration of large-

scale benthic environments. In Robotics and Automation (ICRA), 2013 IEEE Interna-

tional Conference on, pages 390–396. IEEE, 2013.

[4] Sebastian Carreno, Philip Wilson, Pere Ridao, and Yvan Petillot. A survey on terrain

based navigation for auvs. In OCEANS 2010, pages 1–7. IEEE, 2010.

[5] Brian Claus and Ralf Bachmayer. Terrain-aided navigation for an underwater glider.

Journal of Field Robotics, 32(7):935–951, 2015.

71


[6] Shandor Dektor and Stephen Rock. Improving robustness of terrain-relative navigation

for auvs in regions with flat terrain. In 2012 IEEE/OES Autonomous Underwater

Vehicles (AUV), pages 1–7. IEEE, 2012.

[7] Arnaud Doucet, Nando De Freitas, and Neil Gordon. An introduction to sequential

monte carlo methods. In Sequential Monte Carlo methods in practice, pages 3–14.

Springer, 2001.

[8] David Duvenaud. Automatic model construction with Gaussian processes. PhD thesis,

University of Cambridge, 2014.

[9] Austin Eliazar and Ronald Parr. Dp-slam: Fast, robust simultaneous localization and

mapping without predetermined landmarks. In IJCAI, volume 3, pages 1135–1142,

2003.

[10] GPy. GPy: A gaussian process framework in python. http://github.com/

SheffieldML/GPy, since 2012.

[11] John A Hartigan and Manchek A Wong. Algorithm as 136: A k-means clustering algo-

rithm. Journal of the Royal Statistical Society. Series C (Applied Statistics), 28(1):100–

108, 1979.

[12] Rami S Jabari and Daniel J Stilwell. Range-based auv navigation expressed in geodetic

coordinates. In OCEANS 2016 MTS/IEEE Monterey, pages 1–8. IEEE, 2016.

http://github.com/SheffieldML/GPy

http://github.com/SheffieldML/GPy


[13] Jonathan Ko and Dieter Fox. Gp-bayesfilters: Bayesian filtering using gaussian process

prediction and observation models. In Intelligent Robots and Systems, 2008. IROS 2008.

IEEE/RSJ International Conference on, pages 3471–3476. IEEE, 2008.

[14] Jonathan Ko and Dieter Fox. Learning gp-bayesfilters via gaussian process latent vari-

able models. Autonomous Robots, 30(1):3–23, 2011.

[15] Andrew McHutchon and Carl E Rasmussen. Gaussian process training with input noise.

In Advances in Neural Information Processing Systems, pages 1341–1349, 2011.

[16] Duy Nguyen-Tuong and Jan Peters. Local gaussian process regression for real-time

model-based robot control. In 2008 IEEE/RSJ International Conference on Intelligent

Robots and Systems, pages 380–385. IEEE, 2008.

[17] Duy Nguyen-Tuong and Jan Peters. Local gaussian process regression for real-time

model-based robot control. In Intelligent Robots and Systems, 2008. IROS 2008.

IEEE/RSJ International Conference on, pages 380–385. IEEE, 2008.

[18] Duy Nguyen-Tuong, Matthias Seeger, and Jan Peters. Model learning with local gaus-

sian process regression. Advanced Robotics, 23(15):2015–2034, 2009.

[19] Duy Nguyen-Tuong, Matthias Seeger, and Jan Peters. Real-time local gp model learn-

ing. In From Motor Learning to Interaction Learning in Robots, pages 193–207. Springer,

2010.


[20] Duy Nguyen-Tuong, Matthias Seeger, and Jan Peters. Real-time local gp model learn-

ing. In From Motor Learning to Interaction Learning in Robots, pages 193–207. Springer,

2010.

[21] Ingemar Nygren and Magnus Jansson. Terrain navigation for underwater vehicles using

the correlator method. IEEE Journal of Oceanic Engineering, 29(3):906–915, 2004.

[22] Fabian Pedregosa, Gael Varoquaux, Alexandre Gramfort, Vincent Michel, Bertrand

Thirion, Olivier Grisel, Mathieu Blondel, Peter Prettenhofer, Ron Weiss, Vincent

Dubourg, et al. Scikit-learn: Machine learning in python. Journal of Machine Learning

Research, 12(Oct):2825–2830, 2011.

[23] Carl Edward Rasmussen. Gaussian processes for machine learning. 2006.

[24] Matthias Seeger. Low rank updates for the cholesky decomposition. Technical report,

2004.

[25] Edward Snelson and Zoubin Ghahramani. Sparse gaussian processes using pseudo-

inputs. Advances in neural information processing systems, 18:1257, 2006.

[26] Sebastian Thrun. Particle filters in robotics. In Proceedings of the Eighteenth conference

on Uncertainty in artificial intelligence, pages 511–518. Morgan Kaufmann Publishers

Inc., 2002.

[27] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press,

2005.


[28] Ryan Turner, Marc Deisenroth, and Carl Rasmussen. State-space inference and learning

with gaussian processes. In Proceedings of the Thirteenth International Conference on

Artificial Intelligence and Statistics, pages 868–875, 2010.

[29] Sethu Vijayakumar, Aaron D’souza, and Stefan Schaal. Incremental online learning in

high dimensions. Neural computation, 17(12):2602–2634, 2005.

[30] Roger Weber, Hans-Jorg Schek, and Stephen Blott. A quantitative analysis and per-

formance study for similarity-search methods in high-dimensional spaces. In VLDB,

volume 98, pages 194–205, 1998.

[31] Eric W Weisstein. Normal distribution. 2002.

[32] Greg Welch and Gary Bishop. An introduction to the kalman filter. 1995.

[33] Stefan Williams, Gamini Dissanayake, and Hugh Durrant-Whyte. Towards terrain-aided

navigation for underwater robotics. Advanced Robotics, 15(5):533–549, 2001.

terrain aided navigation for autonomous underwater vehicles … · 2020. 1. 20. · abhilash...

Documents