Terrain Aided Navigation for Autonomous Underwater Vehicles with Local Gaussian Processes

Home , Gaussian process

Abhilash Chowdhary

Thesis submitted to the Faculty of the

Virginia Polytechnic Institute and State University

in partial fulﬁllment of the requirements for the degree of

Masters of Science

Computer Engineering

Daniel J. Stilwell, Chair

Ryan K. Williams

Pratap Tokekar

May 10, 2017

Blacksburg, Virginia

Keywords: Autonomous Underwater Vehicles, Navigation, Gaussian Processes

Gaussian Processes

Abhilash Chowdhary

(ABSTRACT)

Navigation of autonomous underwater vehicles (AUVs) in the 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 seaﬂoor 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 ﬁeld 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 seaﬂoor 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 ﬁeld 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

1 Introduction1

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 Processes8

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 ﬁgure 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 seaﬂoor. 0 on the Z-axis

represents the bottom most point of the ﬂoor 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 ﬁt on

the training data points from ﬁrst 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 ﬁt on the

training data points from ﬁrst 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 ﬁgure shows three diﬀerent 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 ﬁgure 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 ﬁgure 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 ﬁgure 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 ﬁgure 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 ﬁgure 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 ﬁx...... 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 suﬀer from very high attenuation in water, an AUV has to come to the surface to update its location using global positioning 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 diﬀerent 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. Abhilash Chowdhary Introduction 3

1.1 Related Work

1.1.1 Terrain Based Navigation

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

Particle ﬁlter 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 seaﬂoor 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 Abhilash Chowdhary Introduction 4 model, a predictive map of the seaﬂoor 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 observations at the latent states of the system. Gaussian processes are a class of random processes deﬁned by their mean and covariance function [23]. The covariance function expresses the correlation of measurement zi and zj at two diﬀerent 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 chapter4. 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. Abhilash Chowdhary Introduction 5

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 ﬁlter based approach [13], using the LGP model as the measurement 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 seaﬂoor 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 Abhilash Chowdhary Introduction 6 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 ﬁts the samples. Given a set of samples from a stochastic process, “inference” refers to choosing a conﬁguration (or model) for the process and estimating the latent variables of the model.

Chapter2 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.

Chapter3 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.

Chapter4 deﬁnes a terrain-aided navigation problem in the context of an AUV and describes how the state space inference method developed in Chapter3 can be used for localizing the AUV. The algorithm proposed in Chapter4 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.

Chapter5 shows experimental results of the method on toy dataset as well as data from ﬁeld Abhilash Chowdhary Introduction 7 trials in Panama City, Florida. Chapter6 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 regression. At their core, GPs model the relationship between latent variables to the measurements

(or observations).

A Gaussian process is deﬁned 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 z2 z∗ zn

f1 f∗ fn

x1 x2 x∗ xn

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) deﬁnes 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 Abhilash Chowdhary Gaussian Processes 10 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 distribution 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) Abhilash Chowdhary Gaussian Processes 11 where

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

∗ ∗ ∗ ∗ −1 ∗ σ∗(x ) = k(x , x ) − K(x , X)K(X, X) K(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 Abhilash Chowdhary Gaussian Processes 12

(RBF) kernel [23]

1 (x − x0)2 k(x, x0) = σ2 exp − (2.8) f 2 2`2

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

2 Gaussian distribution with variance σn,

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

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

expressed as

2 Z | f(X), X ∼ N (f(X), σnI) (2.11) Abhilash Chowdhary Gaussian Processes 13

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

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

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

2 Z ∼ N (µ(X),K(X, X) + σnI) (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

∗ ∗ ∗ 2 −1 µ∗(x ) = µ(x ) + K(x , X)[K(X, X) + σnI] (Z − µ(X)) (2.15)

∗ ∗ ∗ ∗ 2 −1 ∗ σ∗(x ) = k(x , x ) − K(x , X)[K(X, X) + σnI] K(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. Abhilash Chowdhary Gaussian Processes 14

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)

Z 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-

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

dimension of the input. The change in these parameters is reﬂected in the predictive mean

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

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

Z 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 deﬁned in (2.11) is a Gaussian distribution, the log marginal likelihood in (2.18) can be calculated in closed form [23]

1 1 n log p(Z | X, θ) = − Z>K−1Z − log |K | − log 2π (2.20) 2 z 2 z 2 Abhilash Chowdhary Gaussian Processes 15

2 where Kz = K + σnI 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 ﬁnding the values of θ that maximize the marginal likelihood, the partial derivatives of (2.20) are set to zero. Calculating the

3 derivatives requires the computation of the inverse of Kz, and it requires time O(N ) 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 Abhilash Chowdhary Gaussian Processes 16 kernel becomes

(x − x )W(x − x )> k(x , x ) = σ2 exp − 1 2 1 2 (2.21) 1 2 f 2

For learning the value of W, the same approach is used as in Section 2.5.2. The only diﬀerence 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

2 > p(z | f, x) ∼ N (f(x), σn + ∂fˆ Σx∂fˆ) (2.24) Abhilash Chowdhary Gaussian Processes 17

∂f where ∂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]

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

(2.25)

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

(2.26) where diag{∆>Σ ∆ } is the additional term from the uncertainty of the input points X and fˆ x fˆ

∗ ∆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(N 3) 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(N 3). 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(N 2). 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 Abhilash Chowdhary Gaussian Processes 18

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

various data sets and diﬀerent 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 diﬀerent GP models which are learned separately. Abhilash Chowdhary Gaussian Processes 19

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 ﬁt 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(n3 N ) ∼ O(N) (if n N) where N is the total number of data points. max nmax max Abhilash Chowdhary Gaussian Processes 20

Figure 2.2: This ﬁgure 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 eﬀect on the predicted value znew at location xnew if the

Euclidean distance between x and xnew is large. Therefore, the clustering step does not aﬀect 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. Abhilash Chowdhary Gaussian Processes 21

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 eﬀects the prediction is given by Gaussian

∗ distance between the query point, x , and the centroid, ck, of the cluster on which the speciﬁc

GP model is ﬁt. The expression for vk is given by

1 v = exp − (x∗ − c )>W (x∗ − c ) (2.27) k 2 k k k

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]

M X zˆ = E{z¯i | X} = z¯ip(i | X) (2.28) i=1

where z¯i 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

k k X X p(i | X) = p(i, X)/ p(i, X) = vi/ vi (2.29) i=1 j=1 This gives

Pk v z¯ zˆ = i=1 i i (2.30) Pk i=1 vi Abhilash Chowdhary Gaussian Processes 22

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

3 3 data set would be O(knmax) O(N ) where N is the total number of data points.

Algorithm 1 Algorithm for partitioning and learning local GP models from prior data 1: procedure P artitionAndLearnLGP 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 ← learnGP Model(D[t])

7: LGP ← LGP 1 ∪ LGP 2 ∪ LGP 3.. ∪ LGP M 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 P redictLGP 2: D ← X, Z M ← input 3: LGP ← P artitionAndLearnLGP (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 ← predictF romGP (LGPci ) Pk i=1 viz¯i 10: zˆ = Pk i=1 vi 11: return zˆ Abhilash Chowdhary Gaussian Processes 23

2.9.3 Updating Local Models

One of the beneﬁts of local models is the reduced computational complexity in learning the

hyperparameters in comparison to just a single GP model ﬁt 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 speciﬁc

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 speciﬁc 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

2 then ﬁnding the updated inverse in O(nmax) as done in [20] and [17]. Therefore, the whole

2 process of updating a LGP model with a new data point is worst case O(Nnmax). Algorithm Abhilash Chowdhary Gaussian Processes 24

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 − lineUpdateLGP

2: {LGP1, LGP2...LGPM } ← P artitionAndLearnLGP (X, Z) 3: D ← X, Z M ← input

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 ← numberOfDataP oints(D[k])

9: D[k] ← {D[k] ∪ (xnew, znew)}

10: Kn×n ← getCovarianceMatrix(LGPk)

11: Kn+1×n+1 ← updateCovarianceW ithNewDataP oint(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 ← learnGP Model(D[M + 1]) 17: D ← D[1] ∪ D[2] ∪ D[3].. ∪ D[M] ∪ D[M + 1]

18: LGP ← LGP 1 ∪ LGP 2 ∪ LGP 3.. ∪ LGP M ∪ LGP M+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

P P g : R → R . 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

P xt = g(xt−1) + t, xt ∈ R (3.1)

t ∼ N (0, Q) (3.2)

D zt = zˆt + νt, zt ∈ R (3.3)

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

zˆt = 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 xt−1 xt xt+1

zt−2 zt−1 zt zt+1

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

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 Abhilash Chowdhary State Space Inference Using Gaussian Processes 27

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 ﬁltering.

The recursive calculation of the marginal distribution consists of two steps: the state prediction 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)

Z 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)

Z p(zt | z1:t−1) = p(zt | xt)p(xt | z1:t−1)dxt (3.9) Abhilash Chowdhary State Space Inference Using Gaussian Processes 28

In the ﬁnal step of Bayes’ update, we apply Bayes’ rule to ﬁnd p(xt | zt) from p(zt | xt) and p(xt | x1:t−1)

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

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

Z 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 ﬁlters [32] work well in certain cases (e.g., systems with unimodal noise) for evaluating (3.12). Otherwise, sample based approaches such as particle

ﬁlter [26] are appropriate.

Algorithm 4 Bayesian ﬁltering 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 ﬁlters and extended Kalman ﬁlters rely on the assumption that the noise in the measurements and prediction steps are Gaussian. That is not usually the case in the real Abhilash Chowdhary State Space Inference Using Gaussian Processes 29 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 methods [7].

3.2 Particle Filters

Sequential Monte Carlo methods are also regarded as particle ﬁlters in the literature. In particle ﬁltering 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 ﬁltering, 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 Abhilash Chowdhary State Space Inference Using Gaussian Processes 30 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)

i i i i Xt = {xt, wt}i=1,...,NP where xt is the ith particle for the current state, wt 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]:

NP X i i i p(xt | z1:t) = η p(zt | xt) p(xt | xt−1)wt−1 (3.13) i=1

For estimating the states sequentially using particle ﬁlters according to the approach outlined in Algorithm4, 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 Abhilash Chowdhary State Space Inference Using Gaussian Processes 31

i i i probability given in (3.6) xt ∼ p(xt | xt−1)

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

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

i measurement model p(zt | xt) 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

NP i i sample mean of the particles E[xt] = Σi=1wtxt.

Algorithm 5 Estimating states using Particle Filter with importance sampling 1: procedure P articleF ilter i 2: {x0}i=1,..,NP ← initialState, p(xt | xt−1) ← transitionDistribution, p(zt | xt) ← measurementModel 3: for t = 1 to T do 4: Importance Sampling: i 5: Sample {xt}i=1,..,NP ∼ p(xt | xt−1) i i 6: Evaluate importance weights {wt}i=1,..,NP ← p(zt | xt) i i 7: Xt = {xt, wt}i=1,...,NP 8: Selection:

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

NP i i 10: µt ← E[xt] = Σi=1wtxt, Σt ← V[xt]

11: return µ1:T , Σ1:T Abhilash Chowdhary State Space Inference Using Gaussian Processes 32

3.3 GP based Particle Filters

This section describes particle based ﬁltering scenario where the observations are modeled

as a Gaussian process model.

3.3.1 GP as Observation Model

Particle ﬁlters, 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 inﬁnite dimensional parameter space. The eﬀective 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 ﬁt on training points which are known a priori for the system or are sampled

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

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

data, we ﬁt 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) Abhilash Chowdhary State Space Inference Using Gaussian Processes 33 where

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

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

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

ﬁltering technique can be used as mentioned in Algorithm6 to get the estimate of the state x1:T

Algorithm 6 Estimating states using Particle Filter with GP based observation model 1: procedure GP − P articleF ilter 2: D ← X, Z get training data for observation model i 3: {x0}i=1,..,NP ← initialState, p(xt | xt−1) ← transitionDistribution, p(zt | xt) ←

N (µGP (xt, D), σGP (xt, D)) 4: for t = 1 to T do 5: Importance Sampling: i 6: Sample {xt}i=1,..,NP ∼ p(xt | xt−1) i i 7: Evaluate importance weights {wt}i=1,..,NP ← p(zt | xt) i i 8: Xt = {xt, wt}i=1,...,NP 9: Selection:

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

NP i i 11: µt ← E[xt] = Σi=1wtxt, Σt ← V[xt]

12: return µ1:T , Σ1:T

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 Abhilash Chowdhary State Space Inference Using Gaussian Processes 34

g(.)

State Variables xt−1 xt xt+1

Observations zt−1 zt zt+1

Gaussian Process Model h(.)

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 Algorithm7, 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 (zˆt, ν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 ﬁnd 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. Abhilash Chowdhary State Space Inference Using Gaussian Processes 35

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

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

2 > −1 σGP (xt) = K(xt, xt) − K(xt, Xt)[K(Xt, Xt) + σnI + diag{∆zˆ ΣXt ∆zˆ}] K(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 modiﬁed covariance matrix

2 > (K(Xt, Xt) + σnI + 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 Abhilash Chowdhary State Space Inference Using Gaussian Processes 36 parameter per input dimension.

Algorithm 7 GP based Particle Filter with on-line updating 1: procedure GP − P articleF ilter

2: X1, Z1 ← X, Z, D1 ← X1, Z1 get training data for observation model i 3: {x0}i=1,..,NP ← initialState, p(xt | xt−1) ← transitionDistribution 4: for t = 1 to T do 5: Importance Sampling: i 6: Sample {xt}i=1,..,NP ∼ p(xt | xt−1)

7: zt ← observation

8: p(zt | xt) ← N (µGP (xt, Dt), σGP (xt, Dt)) i i 9: Evaluate importance weights {wt}i=1,..,NP ← p(zt | xt) i i 10: Xt = {xt, wt}i=1,...,NP 11: Selection:

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

NP i i 13: µt ← E[xt] = Σi=1wtxt, Σ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 ﬁltering 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 Abhilash Chowdhary State Space Inference Using Gaussian Processes 37

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

number of states encountered.

g(.)

State Variables xt−2 xt−1 xt xt+1 xt+2

Observations zt−2 zt−1 zt zt+1 zt+2

Local Gaussian Processes LGPi−1 LGPi LGPi+1

LGP model h(.)

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 corresponding 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. Abhilash Chowdhary State Space Inference Using Gaussian Processes 38

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

model.

In Chapter2, 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 Algorithm1. This

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

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

rithm8 for state estimation with LGP as observation model in particle ﬁlter. We start

with ﬁtting 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 ﬁlter. Fitting a LGP model on this data results in diﬀerent GP models

{LGP1, LGP2, ..., LGPM }, where each of them is learned on a diﬀerent subset of the partitioned data. As our system proceeds with time, with each particle update and observation

i lines7 and8, the particle weight is calculated based on the estimate of p(zt | xt) using esti-

mate from the nearest local model (Algorithm2). 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 ﬁnd 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 . Abhilash Chowdhary State Space Inference Using Gaussian Processes 39

Algorithm 8 Local GP based Particle Filter with on-line updating 1: procedure LGP − P articleF ilter

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

3: LGP, {D1[1], D1[2], ...D1[M]} ← P artitionAndLearnLGP (D1,M) i 4: {x0}i=1,..,NP ← initialState, p(xt | xt−1) ← transitionDistribution 5: for t = 1 to T do 6: Importance Sampling: i 7: Sample {xt}i=1,..,NP ∼ p(xt | xt−1)

8: zt ← observation 9: for i = 1 to N do i i i 10: µLGPk (xt, Dt), σLGPk (xt, Dt) ← P redictLGP (xt, ) i i i 11: p(zt | xt) ← N (µLGPk (xt, Dt), σLGPk (xt, Dt)) i i 12: Evaluate importance weights {wt}i=1,..,NP ← p(zt | xt) i i 13: Xt = {xt, wt}i=1,...,NP 14: Selection:

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

NP i i 16: µt ← E[xt] = Σi=1wtxt, Σ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

25: [θ, ΣXt ] ← MaximumLikelihoodEstimate(p(zt | xt)) 26: . Learn Hyperparameters

27: return µ1:T , Σ1:T , Chapter 4

Terrain-Aided Navigation

This chapter describes how an AUV ﬁtted 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 speciﬁc 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 diﬀerent 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 seaﬂoor. Using attitude and velocity, the future state of the AUV is predicted using a kinematic model.

Let us deﬁne 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 Abhilash Chowdhary Terrain-Aided Navigation 42

(NED) frame

• vt = [u v w p p r] be the robot velocity with respect to robot ﬁxed 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 ﬁxed 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 ﬁltering algorithm as described in the previous chapters. The prediction model and observation model of the ﬁltering mechanism to estimate the states is described below. Abhilash Chowdhary Terrain-Aided Navigation 43

4.2.1 Prediction Model

In this work, we do not estimate the value of z and are only concerned with the 2-D localization 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 ﬁltering scenario, the particle set can be deﬁned as   x1 ... xNP  t t  xt =   (4.1)  1 NP  yt ... yt The prediction model for the AUV is expressed

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

f(xt−1) =xt−1 + ∆tx˙ t−1 (4.3)

x˙ t−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) Abhilash Chowdhary Terrain-Aided Navigation 44

4.2.2 Observation Model

An observation model yields the conditional distribution p(zt | xt). To get p(zt | xt), we ﬁt 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 similar to Section 3.4. The LGP model is initialized and learned as mentioned in Section 2.9. For this case, data set D 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 similarly 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

th the GP models is denoted LGPi, for the i 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 ﬁnal predictive distribution of the measure- Abhilash Chowdhary Terrain-Aided Navigation 45

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

k X p(zt | xt, Dt) ∝ viN (µLGPi (xt, Dt[i]), σLGPi (xt, Dt[i])) (4.7) i=1

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

ﬂoor. 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 seaﬂoor 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 ﬁlter to incorporate multibeam measurements is shown. Abhilash Chowdhary Terrain-Aided Navigation 46

4.3.1 Likelihood for re-sampling

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

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

i is zt = [h1, h2 . . . h4]. The likelihood then becomes p(h1, h2 . . . h4 | xt). 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:

i i p(zt | xt) = p(h1, h2 . . . h4 | xt, Xt, Zt)

i i i = p(h1, h2 . . . h4 | x1,t, x2,t ... x4,t, Xt, Zt)

i i i i i i i i i = p(h1 | x1,t, x2,t ... x4,t, Xt, Zt)p(h2 | x1,t, x2,t ... x4,t, Xt, Zt) . . . p(h4 | x1,t, x2,t ... x4,t, Xt, Zt)

i i i = p(h1 | x1,t, Xt, Zt)p(h2 | x2,t, Xt, Zt) . . . p(h4 | x4,t, Xt, Zt)

i i i i ∴ p(zt | xt) = p(h1 | x1,t, Xt, Zt)p(h2 | x2,t, Xt, Zt) . . . p(hb | xb,t, Xt, Zt) (4.8)

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

4 {p(hl | xl,t, Xt, Zt)}l=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 ﬁltering 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. Abhilash Chowdhary Terrain-Aided Navigation 47

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

i i i probability given in 4.6 xt ∼ p(xt | xt−1)

i i i i 2. For each of the particles xt we have four points x1,t, x2,t ... x4,t. And each of these

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

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

i by the likelihood p(zt | xt). 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

PN i i particles E[xt] = i=1 wtxt.

Detailed procedure for performing inference and learning is shown in Algorithm9.

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 Abhilash Chowdhary Terrain-Aided Navigation 48

Algorithm 9 Local GP based Particle Filter (LGP-PF) with multibeam DVL measurements 1: procedure LGP − P articleF ilter − MultiBeam

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

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

9: for i = 1 to NP do i 10: [LGP1, LGP2 . . . LGPk] ← kNearestLGP Models(xt, LGP ) 11: for l = 1 to k do 1 > 12: vl ← exp − 2 (x − ck) Wk(x − ck) i i i 13: µLGPl (xt, Dt), σLGPl (xt, Dt) ← P redictLGP (xt,Dt) 14: for j = 1 to b do k i P i i 15: p(hj | xj,t) ← vlN (µLGPl (xj,t, Dt[l]), σLGPi (xj,t, Dt[l])) l=1 i i i i 16: Evaluate importance weights {wt}i=1,..,NP ← p(h1 | x1,t)p(h2 | x2,t) . . . p(h4 | x4,t) i i 17: Xt = {xt, wt}i=1,...,NP 18: Selection:

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

NP i i 20: µt ← E[xt] = Σi=1wtxt, Σ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

29: [θ, ΣXt ] ← MaximumLikelihoodEstimate(p(zt | xt)) 30: . Learn Hyperparameters

31: return µ1:T , Σ1:T , Abhilash Chowdhary Terrain-Aided Navigation 49

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

a path then can be quantiﬁed 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 ﬁnite 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 ﬁt on the a priori map, we can ﬁnd 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 quantiﬁed as P (zci < dthreshold | ci), the probability of predicted depth being

greater than the depth threshold dthreshold. This is given by

Z dthreshold

lci = P (zci < dthreshold | ci) = 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 distribution, (4.9) can be rewritten [31]

dthreshold − µci lci = Φ (4.10) σci

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

(Algorithm9) of the AUV. At ﬁrst, 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 map M 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 ﬁt 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 seaﬂoor. 0 on the Z-axis represents the bottom most point of the ﬂoor from the surface.

5.1.1 Comparison of Local and Sparse Approximation based methods

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 Abhilash Chowdhary Experimental Evaluation 52 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. . Abhilash Chowdhary Experimental Evaluation 53

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

ﬁt 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

p 2 2 et = (xt − xˆt) + (yt − yˆt) (5.1)

where (xt, yt) is the location of AUV in the sampled path at t and (ˆxt, yˆt) 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 Abhilash Chowdhary Experimental Evaluation 54

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 Abhilash Chowdhary Experimental Evaluation 55

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 ﬁeld 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. Abhilash Chowdhary Experimental Evaluation 56

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 ﬁnd 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 diﬀerent way-points. Abhilash Chowdhary Experimental Evaluation 57

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 ﬁt on the training data points from ﬁrst and second mission.

There were two way-point missions that were conducted. The ﬁrst 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 ﬂoor 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 ﬁt on the depth data points and their corresponding GPS location from the ﬁrst 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 Abhilash Chowdhary Experimental Evaluation 58

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 ﬁt on the training data points from ﬁrst 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 ﬁrst 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 ﬁrst 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. Abhilash Chowdhary Experimental Evaluation 59

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. Abhilash Chowdhary Experimental Evaluation 60

We tested our LGP-PF algorithm (Algorithm9) oﬀ-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 diﬀerent altitude measurements

above the seaﬂoor 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.05 meter2. For this implementation,

50 particles were used for the estimation in the particle ﬁlter. For learning the LGP model, prior depth points were clustered into 30 diﬀerent clusters and RBF covariance kernel (2.8) was used for GP model over all these clusters. Abhilash Chowdhary Experimental Evaluation 61

Figure 5.8: This ﬁgure shows three diﬀerent 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 Abhilash Chowdhary Experimental Evaluation 62

Figure 5.9: This ﬁgure 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. Abhilash Chowdhary Experimental Evaluation 63

Figure 5.10: This ﬁgure 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. Abhilash Chowdhary Experimental Evaluation 64

Figure 5.11: This ﬁgure 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. Abhilash Chowdhary Experimental Evaluation 65

Figure 5.12: This ﬁgure 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. Abhilash Chowdhary Experimental Evaluation 66

In Figure 5.9, the AUV goes from south to north. As the predicted depth variation along the path from south to north is signiﬁcant, 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 signiﬁcant 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 signiﬁcant, due to low depth variation from east to north and relatively non-varying depth in the northern region, the location 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 Abhilash Chowdhary Experimental Evaluation 67

Figure 5.13: This ﬁgure 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 ﬁx. 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 ﬁrst 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 Abhilash Chowdhary Experimental Evaluation 68 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 algorithm9 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 filtering 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 proﬁle. 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 diﬀerent sensors like IMU etc., the estimation could be improved. Also the states of the ﬁlter 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

ﬁlter (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 ﬁlter 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 eﬃcient 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 Abhilash Chowdhary Conclusions 72

[6] Shandor Dektor and Stephen Rock. Improving robustness of terrain-relative navigation

for auvs in regions with ﬂat 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. Abhilash Chowdhary Conclusions 73

[13] Jonathan Ko and Dieter Fox. Gp-bayesﬁlters: Bayesian ﬁltering 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-bayesﬁlters 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. Abhilash Chowdhary Conclusions 74

[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, Ga¨elVaroquaux, 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 ﬁlters in robotics. In Proceedings of the Eighteenth conference

on Uncertainty in artiﬁcial intelligence, pages 511–518. Morgan Kaufmann Publishers

Inc., 2002.

[27] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press,

2005. Abhilash Chowdhary Conclusions 75

[28] Ryan Turner, Marc Deisenroth, and Carl Rasmussen. State-space inference and learning

with gaussian processes. In Proceedings of the Thirteenth International Conference on

Artiﬁcial 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-J¨orgSchek, 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 ﬁlter. 1995.

[33] Stefan Williams, Gamini Dissanayake, and Hugh Durrant-Whyte. Towards terrain-aided

navigation for underwater robotics. Advanced Robotics, 15(5):533–549, 2001.