Post on 17-Aug-2020
Terrain Aided Navigation for Autonomous Underwater Vehicles
with Local Gaussian Processes
Abhilash Chowdhary
Thesis submitted to the Faculty of the
Virginia Polytechnic Institute and State University
in partial fulfillment of the requirements for the degree of
Masters of Science
in
Computer Engineering
Daniel J. Stilwell, Chair
Ryan K. Williams
Pratap Tokekar
May 10, 2017
Blacksburg, Virginia
Keywords: Autonomous Underwater Vehicles, Navigation, Gaussian Processes
Copyright 2017, Abhilash Chowdhary
Terrain Aided Navigation for Autonomous Underwater Vehicles with Local
Gaussian Processes
Abhilash Chowdhary
(ABSTRACT)
Navigation of autonomous underwater vehicles (AUVs) in the subsea environment is partic-
ularly challenging due to the unavailability of GPS because of rapid attenuation of electro-
magnetic waves in water. As a result, the AUV requires alternative methods for position
estimation. This thesis describes a terrain-aided navigation approach for an AUV where,
with the help of a prior depth map, the AUV localizes itself using altitude measurements
from a multibeam DVL. The AUV simultaneously builds a probabilistic depth map of the
seafloor as it moves to unmapped locations.
The main contribution of this thesis is a new, scalable, and on-line terrain-aided navigation
solution for AUVs which does not require the assistance of a support surface vessel. Simu-
lation results on synthetic data and experimental results from AUV field trials in Panama
City, Florida are also presented.
Terrain Aided Navigation for Autonomous Underwater Vehicles with Local
Gaussian Processes
Abhilash Chowdhary
(GENERAL AUDIENCE ABSTRACT)
Navigation of autonomous underwater vehicles (AUVs) in subsea environment is particularly
challenging due to the unavailability of GPS because of rapid attenuation of electromagnetic
waves in water. As a result, the AUV requires alternative methods for position estimation.
This thesis describes a terrain-aided navigation approach for an AUV where, with the help of
a prior depth map, the AUV localizes itself using altitude measurements from a multibeam
DVL. The AUV simultaneously builds a probabilistic depth map of the seafloor as it moves
to unmapped locations.
The main contribution of this thesis is a new, scalable, and on-line terrain-aided navigation
solution for AUVs which does not require assistance of a support surface vessel. Simulation
results on synthetic data and experimental results from AUV field trials in Panama City,
Florida are also presented.
To my Mother, Father and Brother
iv
Acknowledgments
I would like to thank my advisor, Dr. Daniel Stilwell, for advice, feedback and support, and
for being an example throughout my Masters. I want to thank you for letting me pursue
the thesis work on the topic of my liking and always being there to guide me in the right
direction. I’m also grateful to other members in my thesis committee, Dr. Ryan Williams
and Dr. Pratap Tokekar, for their guidance and help from the initial days of my research
work.
Two years of my Masters would not have been an easy ride without my fellow labmates
Autonomous Systems and Controls Lab (ASCL) at Virginia Tech. I would like to thank
Scott, Rand, Rami, Jack, Harun, Stephen, Nick, Larkin and Kepler for many hours of
stimulating discussions, sharing some unforgettable laughs and proofreading this work.
I would also like to thank my friends Tommy, Lily, Josh, Oscar, Jennifer, Siddharth, Sarthak,
Tapas, Chris, Ramakrishna and Dixika for their support and memorable time in Blacksburg.
I would also like to thank my friends Jasmeet and Ashish for being great hosts, every time
I visited Bay Area during this time.
v
Finally, I would like to thank my parents and my brother Abhishek for their caring support
and love.
vi
Contents
Contents x
List of Figures xi
List of Figures xiv
List of Tables xv
List of Tables xv
1 Introduction 1
1.1 Related Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.1.1 Terrain Based Navigation . . . . . . . . . . . . . . . . . . . . . . . . 3
1.1.2 SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2 Localization using GP based observation model . . . . . . . . . . . . . . . . 4
vii
1.3 Outline and contributions of thesis . . . . . . . . . . . . . . . . . . . . . . . 5
2 Gaussian Processes 8
2.1 Sampling from GP . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.2 GP Posterior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.3 Nonparametric Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.4 Covariance Function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.5 Inference with Noisy Observation . . . . . . . . . . . . . . . . . . . . . . . . 12
2.5.1 Bayesian Perspective . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.5.2 Hyperparameter Learning . . . . . . . . . . . . . . . . . . . . . . . . 14
2.6 Multi-Dimension Input . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.7 GP with uncertain inputs . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.8 GP Limitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.9 Local Gaussian Processes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
2.9.1 Partitioning Data . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.9.2 Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.9.3 Updating Local Models . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3 State Space Inference Using Gaussian Processes 25
viii
3.1 Inference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.2 Particle Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.2.1 Basic Description for AUVs . . . . . . . . . . . . . . . . . . . . . . . 29
3.2.2 Inference using Particle Filters . . . . . . . . . . . . . . . . . . . . . . 30
3.3 GP based Particle Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.3.1 GP as Observation Model . . . . . . . . . . . . . . . . . . . . . . . . 32
3.3.2 On-line updating of observation model . . . . . . . . . . . . . . . . . 33
3.4 Local GP based Particle Filters . . . . . . . . . . . . . . . . . . . . . . . . . 36
4 Terrain-Aided Navigation 40
4.1 The Terrain-Aided Navigation Problem . . . . . . . . . . . . . . . . . . . . . 40
4.1.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
4.2 State Space Model for TAN . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
4.2.1 Prediction Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
4.2.2 Observation Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
4.2.3 Multibeam DVL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
4.3 Particle Filtering strategy for multibeam DVL . . . . . . . . . . . . . . . . . 45
4.3.1 Likelihood for re-sampling . . . . . . . . . . . . . . . . . . . . . . . . 46
ix
4.3.2 Inference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
4.4 Cost of running into the shore . . . . . . . . . . . . . . . . . . . . . . . . . . 47
5 Experimental Evaluation 50
5.1 Simulation based Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . 50
5.1.1 Comparison of Local and Sparse Approximation based methods . . . 51
5.1.2 Simulating AUV’s Trajectory . . . . . . . . . . . . . . . . . . . . . . 53
5.1.3 Cost of Running into the Shore . . . . . . . . . . . . . . . . . . . . . 54
5.2 Real world Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
5.2.1 The Virginia Tech 690s AUV . . . . . . . . . . . . . . . . . . . . . . 55
5.2.2 Field Trials . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
6 Conclusions 69
6.1 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
Bibliography 71
x
List of Figures
2.1 Graphical model for Gaussian Processes. zi denote the observations, xi denote
the inputs(or location) and fi denote the latent realizations from the GP model. 9
2.2 This figure shows inference and training in local GP model. Data set is clus-
tered into various subset and GP models are learned separately on each subset.
Inference at the yellow dot is a weighted mean of the GP inferences of the
nearest (M = 4) clusters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.1 Graphical model for a sequential Markov process with latent states and ob-
servation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.2 Graphical model for a state-space model with observation model modeled as
a GP . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
xi
3.3 Graphical model for a state-space model with LGP as the observation model.Here
xt denote the latent states of the system and zt denote the observations made
by the system. The nodes LGPi model the local GP models using a subset
of the observations and corresponding states. h() is the observation model
which consists of all the local GP models. g() is the prediction model of the
system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
4.1 The Virginia Tech 690s during Panama City trials . . . . . . . . . . . . . . . 41
5.1 3D plot of 600×300 grid depicting the terrain of the seafloor. 0 on the Z-axis
represents the bottom most point of the floor from the surface. . . . . . . . . 51
5.2 Comparison of depth prediction over the synthetic data of Figure 5.1 using
1900 random points with known depth values. Figures on the left show the
mean depth prediction and bias associated with it, for an LGP model with 30
clusters and Gaussian(RBF) kernel. Figures on the right show the mean depth
prediction and bias associated with it, for an sparse approximation for GP over
all the 1900 points. The root mean squared errors in depth prediction for LGP
and sparse GP based model were 7.27 meters and 4.17 meters respectively. . 52
5.3 Shows the comparison of ten estimated trajectories from LGP-PF with prior
LGP model and re-learned LGP model. . . . . . . . . . . . . . . . . . . . . . 54
xii
5.4 Plot of cost of an AUV running into the shore, moving at a depth of 30 meters
below the surface of water. . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
5.5 Prior depth map with data points represented as black dots (4834 in number).
The contour plot shows predictive mean depth from sparse GP model fit on
the training data points from first and second mission. . . . . . . . . . . . . 57
5.6 Prior depth map with data points represented as black dots (4834 in number).
The contour plot shows predictive mean depth from LGP model fit on the
training data points from first and second mission. . . . . . . . . . . . . . . 58
5.7 The Virginia Tech 690s AUV trajectory for Panama City trials as estimated
by LGP-PF. It is also compared to range-based solution in [12]. GPS updates
are shown as red dots and the numbers next to them denote the sequence
of these updates. Blue dots represent range based estimate. Yellow dots
represent LGP-PF based estimate. . . . . . . . . . . . . . . . . . . . . . . . 59
5.8 This figure shows three different areas on the predicted depth map from LGP.
White region denotes where the prediction was zero and is neglected. Black
dots represent the down-sampled prior depth data points used for learning the
LGP model. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
5.9 This figure shows the AUV starting in the south (initial start point) and it
resurfaces in north with GPS update. The error in estimation is about 10
meters. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
xiii
5.10 This figure shows the AUV starting from south and going to east via west and
north. The error in estimation is about 25 meters with respect to GPS update. 63
5.11 This figure shows the AUV starting from west and going to north via east.
The error in estimation increases to about 30 meters with respect to GPS
update. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
5.12 This figure shows the AUV starting from the south and going to east via west.
The error in estimation drops to about 8 meters with respect to GPS update. 65
5.13 This figure shows the errors in location estimate using LGP-PF relative to
GPS updates at each of the AUV resurface. Negative error means AUV is
underwater and does not have GPS fix. . . . . . . . . . . . . . . . . . . . . 67
xiv
List of Tables
5.1 Sensors in the Virginia Tech 690s . . . . . . . . . . . . . . . . . . . . . . . . 56
xv
Chapter 1
Introduction
Autonomous underwater vehicles (AUVs) have to localize themselves under water for car-
rying out various missions. Since electromagnetic waves suffer from very high attenuation
in water, an AUV has to come to the surface to update its location using global position-
ing system (GPS). To complement the use of GPS for AUVs, other navigation systems are
used, like inertial navigation system (INS), Doppler velocity logs (DVL), ultrashort-baseline
(USBL), and long-baseline (LBL) acoustic positioning systems.
For navigation, systems like INS coupled with a DVL give an un-bounded error for localizing
the AUV [2]. Systems that use acoustic ranging for navigation, such as LBL and USBL,
require that one or more acoustic reference nodes be deployed along with the AUV, and
moreover, the AUV must remain within the acoustic range of the reference nodes. Generally,
a support vessel is used for installing acoustic reference nodes, which adds to the operational
cost the AUV. Alternative to range based navigation, an AUV can localize itself by just
1
Abhilash Chowdhary Introduction 2
using information extracted from the terrain with the help of different sensors, and thereby
reducing the cost of operation. As a result of reduced cost of operation, terrain based
navigation solutions may be more helpful in certain applications.
Algorithms that utilize terrain for navigation can be divided into two categories: the first
is localizing the AUV in an a priori generated map of the ocean floor, and the second is
localizing the AUV while creating the map for the ocean floor without the need of a prior
map. The first kind of algorithms form a category called terrain-aided navigation (TAN)
problem. Terrain-aided navigation methods provide localization for an autonomous vehicle,
with respect to a priori known map and a sequence of real-time measurements of the local
height of the terrain. Choice of the method used for measuring the local height of the terrain
depends on the approach being used.
An alternative approach is to generate a map of the environment (in this case a bathymetric
map) while simultaneously localizing itself on the map generated. Because the accuracy of
navigation is dependent on the accuracy of the map generated and vice-versa, this approach
enforces a consistency in maps generated [27]. The technique of simultaneous localization
and mapping is known as simultaneous localization and mapping (SLAM) [27] and has been
extensively used, and researched in context of aerial and ground robots. However, there is
comparatively less research done on SLAM in subsea environment.
Abhilash Chowdhary Introduction 3
1.1 Related Work
1.1.1 Terrain Based Navigation
A linear Kalman filter based approach with sonar for depth measurements is used in [21].
Particle filter based methods for localization have also been used for localization using similar
observation models in [6], [5]. [21], [6] and [5] require prior digital elevation maps (DEM) of
the seafloor for them to work, and they do not model the correlation between the depth at
any two locations in and outside the map.
1.1.2 SLAM
Barkby et al. in [2], addressed the problem of simultaneously localizing and mapping via
AUV for a flat seafloor. The SLAM technique used in [2] follows from the distributed
particle SLAM (DPSLAM) filter used in [9]. In [33], the authors present a SLAM algorithm
for AUVs that is based on extended Kalman filter (EKF). In [1] and [33] the measurement
function used is dependent only on the depth measurements from the DVL and the location
of the AUV at the particular time step. Correlation between the depth of two different
points in the horizontal plane is not considered in the observation model. Barkby et al. in
[1] used a Gaussian process(GP) based SLAM algorithm for AUVs where the measurement
model is learned using a Gaussian process over the terrain. A GP model gives a predictive
distribution for the depth of the ocean at any location not yet visited by AUV. Using this
Abhilash Chowdhary Introduction 4
model, a predictive map of the seafloor can be generated with an uncertainty measure. GPs
have also been used for planning informative underwater survey missions [3]. One major
drawback of using GP as observation model is the high computational complexity, with
respect to total number of observations, associated with learning the model (refer Chapter
2).
1.2 Localization using GP based observation model
A GP-based observation model in a state-space inference problem models the noisy observa-
tions at the latent states of the system. Gaussian processes are a class of random processes
defined by their mean and covariance function [23]. The covariance function expresses the
correlation of measurement zi and zj at two different states xi and xj respectively.
GPs have been a popular choice for modeling the non-linearity between the observations and
the states for state-space inference problems. The states for an AUV in this thesis refers to
the location in horizontal plane and observations are the depth measurements as described
in chapter 4. GP-based observation models are used in [13], [14] and [28] for state-space
inference in a non-linear dynamical system. For a better estimate of the value of a state,
the GP model needs to be updated with additional observations as more states are explored.
However, learning and updating a GP model is expensive as it requires an inversion of a
large covariance matrix on every update with new data. Calculating the inverse of a matrix
has a time-complexity of O(e3), where e is the dimension of matrix.
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 filter based approach [13], using the LGP model as the measure-
ment model and a linear dynamical model as the prediction model, is used. As the AUV
moves and gathers more depth data, the predicted map of the seafloor is updated after
discrete time intervals using the LGP model.
1.3 Outline and contributions of thesis
The main contribution of the thesis is to develop a method for localizing an AUV using
altitude measurements from a multibeam DVL. The algorithm presented here requires a prior
sparse bathymetric map of the area where the AUV is scheduled to survey. The localization
algorithm can be run on-line and the run time complexity is bounded for inference on a
stream of measurements. This thesis also provides a method to quantify the cost of running
an AUV into the shore for a certain path between two points given that the AUV always
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 fits the
samples. Given a set of samples from a stochastic process, “inference” refers to choosing a
configuration (or model) for the process and estimating the latent variables of the model.
Chapter 2 is a short primer on Gaussian processes in general and describes how does the
inference and learning proceed in a GP model. It then builds up on to local Gaussian
processes which is an approximation of GPs over a large amount of data when GPs scale
poorly.
Chapter 3 shows how to do inference over states in a non-linear state space model where the
observation model is modeled by GP or LGP model.
Chapter 4 defines a terrain-aided navigation problem in the context of an AUV and describes
how the state space inference method developed in Chapter 3 can be used for localizing
the AUV. The algorithm proposed in Chapter 4 for localizing the AUV is computationally
tractable for real-time applications. This algorithm, having linear computational complexity
with respect to number of observations made, can be used in the on-line setting to localize
the AUV . A method to quantify the cost related to a path from point A to point B with
the AUV at a constant depth from the surface of water is also described.
Chapter 5 shows experimental results of the method on toy dataset as well as data from field
Abhilash Chowdhary Introduction 7
trials in Panama City, Florida. Chapter 6 concludes this thesis.
Chapter 2
Gaussian Processes
This chapter gives an overview of Gaussian processes in general. Gaussian processes (GP) are
a class of stochastic process that provide an elegant method for Bayesian non-linear regres-
sion. At their core, GPs model the relationship between latent variables to the measurements
(or observations).
A Gaussian process is defined as a collection of random variables, any number of which have
a joint Gaussian distribution [23]. Here these random variables can be considered as the
realizations (latent function), f(x), of the input x. We can interpret GPs as a simple and
general class of models of functions. Since these realizations in a GP are distributed according
to a multivariate normal distribution, it means that characterization of these realizations can
be completely described by : 1) a mean vector µ or mean function µ(x) and 2) covariance
8
Abhilash Chowdhary Gaussian Processes 9
z1
f1
x1
z2
x2
f∗
x∗
z∗
fn
xn
zn
Figure 2.1: Graphical model for Gaussian Processes. zi denote the observations, xi denote
the inputs(or location) and fi denote the latent realizations from the GP model.
matrix Σ or a covariance function K(·, ·).
f(X) |X ∼ N (µ(X), K(X,X)) (2.1)
where X = [x1,x2, . . .xN ], f(X) = [f(x1), f(x2), . . . f(xN)] and
E[f(x)] = µ(x) (2.2)
Cov(f(x), f(x∗)) = k(x,x∗) (2.3)
For a GP, the choice of the covariance function k(·, ·) : R× R 7→ R in (2.3) is dependent on
the correlation between x and x∗.
We refer to (2.1) as the prior for a GP, which can also represented by
f(X) ∼ GP(µ(X), K(X,X)) (2.4)
where K(·, ·) : RN×q × RM×q 7→ RN×M is the covariance matrix and q is the dimension of
input x. As (2.4) defines the prior for the process being observed, mean vector µ(X) is
usually taken as zero if not much prior information is available. Even with a zero mean
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 dis-
tribution and hence generating more samples for a GP is equivalent to sampling from the
given distribution. In other words, these randomly generated samples are from the prior
distribution over the observations, which is GP.
2.2 GP Posterior
Random samples generated in Section 2.1 from a Gaussian process prior are usually of not
much value. While doing inference, we are more concerned about the posterior predictive
distribution of function value f(x∗) at a new location x∗. According to the prior, f(x∗)
and f(X) are distributed according to a multivariate normal distribution. From conditional
distribution of two joint normally distributed random variables, it can be shown [23]
p(f(x∗) |x∗, f(X),X) ∼ N (µ∗(x∗),σ∗(x
∗)) (2.5)
Abhilash Chowdhary Gaussian Processes 11
where
µ∗(x∗) = µ(x∗) +K(x∗,X)K(X,X)−1(f(X)− µ(X)) (2.6)
σ∗(x∗) = k(x∗,x∗)−K(x∗,X)K(X,X)−1K(X,x∗) (2.7)
The posterior predictive distribution of the function f(x∗) in (2.5) is a normal with mean
(2.6) and covariance (2.7).
2.3 Nonparametric Model
The distribution from (2.5) does not have any other parameters other than the data points
D = (X, f(X)). Since the distribution in (2.5) does not have any model parameters, it
is a nonparametric model. As the number of data points increases, the number of model
parameters increases. However, there are parameters of the covariance function which are
termed as hyperparameters, and they play an important role in the behavior of a GP model.
2.4 Covariance Function
The appropriateness of a Gaussian Process model is entirely dependent on the choice of
covariance function k(xi,xj), which describes the correlation between the observations zi
and zj . A popular covariance function to model geospatial data is the radial basis function
Abhilash Chowdhary Gaussian Processes 12
(RBF) kernel [23]
k(x,x′) = σ2f exp
(−1
2
(x− x′)2
2`2
)(2.8)
where ` is a length scale that determines the strength of correlation between points and σf
is the signal variance. As seen from (2.8), when two points are separated by a large distance,
the covariance will tend to zero, indicating the independence of measurements at locations
that are far apart.
2.5 Inference with Noisy Observation
In a realistic setting, we usually cannot observe the function values f(x), and hence they are
latent functional realizations from the GP. Generally, we have access to the noisy version of
these functional values which we can model as
z = f(x) + εz (2.9)
where z is the noisy observation (or measurement) with noise εz. The distribution of the
noise εz depends on the stochastic process we are observing. If we consider εz to have a
Gaussian distribution with variance σ2n,
εz ∼ N (0, σ2n) (2.10)
then the distribution of observations Z given the functional values f(X) and locations X is
expressed as
Z | f(X),X ∼ N (f(X), σ2nI) (2.11)
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
Cov(f(x), f(x′)) = k(x,x′) + σ2nI (2.12)
Due to the statistics of the observation noise in (2.10), we have the likelihood of our obser-
vations Z
Z ∼ N (µ(X), K(X,X) + σ2nI) (2.13)
Following the same approach as in Section 2.2, the posterior predictive distribution for z∗
can be calculated using (2.12) and (2.13)
p(z∗ |x∗,Z,X) ∼ N (µ∗(x∗),σ∗(x
∗)) (2.14)
where
µ∗(x∗) = µ(x∗) +K(x∗,X)[K(X,X) + σ2
nI]−1(Z− µ(X)) (2.15)
σ∗(x∗) = k(x∗,x∗)−K(x∗,X)[K(X,X) + σ2
nI]−1K(X,x∗) (2.16)
(2.15) and (2.16) are similar to (2.6) and (2.7) respectively, with addition of noise to the
input covariance matrix K(X,X). Figure 2.1 shows the graphical model of a GP.
2.5.1 Bayesian Perspective
An interesting thing to notice is, if we look through a Bayesian point of view, (2.11) resembles
a likelihood function for the observations Z and (2.1) and (2.4) are the priors for the latent
function values f(X) modeled via a GP.
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)
p(Z |X) =
∫p(Z | f(X),X)p(f(X) |X)df (2.17)
2.5.2 Hyperparameter Learning
As shown in Section 2.3, GPs are nonparametric models with data points being its parame-
ter. However, there are free parameters which arise from the covariance function k(·, ·) and
the observation noise. For a RBF covariance function as described in (2.8), these param-
eters include the length scale `, the signal variance σ2f and the noise variance σ2
n for each
dimension of the input. The change in these parameters is reflected in the predictive mean
and the predictive variance of the GP (2.15) and (2.16). Let θ = {`, σ2f , σ
2n} represent the
hyperparameters in a GP, (2.17) can be rewritten as
p(Z |X,θ) =
∫p(Z | f(X),X)p(f(X) |X)df (2.18)
If under the GP model, the prior for f(x) has mean µ(x) equal to zero, (2.1) becomes
f(X) |X ∼ N (0, K(X,X)) (2.19)
Also, if the likelihood as defined in (2.11) is a Gaussian distribution, the log marginal likeli-
hood in (2.18) can be calculated in closed form [23]
log p(Z |X,θ) = −1
2Z>K−1
z Z− 1
2log |Kz| −
n
2log 2π (2.20)
Abhilash Chowdhary Gaussian Processes 15
where Kz = K + σ2nI is the covariance matrix for the noisy observations Z ( and K is the
covariance matrix for noise-free latent function values f(X)). Since the marginal likelihood
in (2.20) is not dependent on the latent function values, estimation of θ can be done by
optimizing the marginal log likelihood in (2.20). For finding the values of θ that maximize
the marginal likelihood, the partial derivatives of (2.20) are set to zero. Calculating the
derivatives requires the computation of the inverse of Kz, and it requires time O(N3) where
N is the number of data points in the GP model.
While maximizing, it is possible for the marginal likelihood to reach a local optima. However,
practical experience with simple covariance functions seem to indicate that the local maxima
are not a devastating problem, but they certainly do exist [23].
There are other frequentist approaches like cross-validation for optimizing the hyperparam-
eters. However, we do not use those for training our GP models in this thesis.
2.6 Multi-Dimension Input
All the equations mentioned above remain the same when we have multi-dimensional input
X except for the expression for RBF kernel in (2.8). For a q dimensional input Xnq, the
length scale parameter is represented by diagonal matrix Wqq where each diagonal element
represents the inverse square of lengthscale in each dimension. Hence the expression for the
Abhilash Chowdhary Gaussian Processes 16
kernel becomes
k(x1,x2) = σ2f exp
(−(x1 − x2)W(x1 − x2)>
2
)(2.21)
For learning the value of W, the same approach is used as in Section 2.5.2. The only
difference is instead of having a single lengthscale parameter to learn, we’ll have m number
of lengthscale parameters, one for each dimension of the input.
2.7 GP with uncertain inputs
In Sections 2.5 and 2.2, we considered inputs x ∈ Rq to be certain and without noise. In a
real world scenario, actual inputs x are noisy
x = x + εx (2.22)
where εx ∼ N (0,Σx). Here the noise in each input dimension is independent of the noise
in other input dimensions and Σx is a diagonal matrix. With a GP prior over f(x), the
observation z can be written
z = f(x + εx) + εz (2.23)
Using the approach from [15] and after considering the Taylor expansion for f in (2.23), the
distribution over the observation z is expressed
p(z | f,x) ∼ N (f(x), σ2n + ∂>
fΣx∂f ) (2.24)
Abhilash Chowdhary Gaussian Processes 17
where ∂f = ∂f∂x
. Using (2.24) as the likelihood and taking a zero mean (µ(X) = 0) GP prior
over f(X), the predictive posterior mean and variance can be expressed [15]
E[z∗ |X,Z,x∗] = −K(x∗,X)[K(X,X) + σ2nI + diag{∆>
fΣx∆f}]
−1Z
(2.25)
V[z∗ |X,Z,x∗] = k(x∗,x∗)−K(x∗,X)[K(X,X) + σ2nI + diag{∆>
fΣx∆f}]
−1K(X,x∗)
(2.26)
where diag{∆>f
Σx∆f} is the additional term from the uncertainty of the input points X and
∆f represents the derivative of the GP function values with respect to the input vector x∗.
2.8 GP Limitations
As seen in Section 2.5.2, the matrix inversion needed for learning the hyperparameters in
(2.20) requires computational O(N3) where N is the number of data points in the model.
Similarly, in (2.15) and (2.16), the matrix inversion needed for inference is O(N3). This
is an issue when dealing with datasets having N > 10000. Also, the space complexity for
storing a N x N covariance matrix K is O(N2). High space-time complexity is a problem
when learning and inference are performed on a limited number of machines with limited
computational power. It also limits applications that require real-time inference. To tackle
these shortcomings, various approximate solutions can be used. One such approximation
is sparse approximation of the GP [25], where only a few induced points out of the whole
training data are used for learning the hyperparameters. Another is locally weighted GP
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 different GP models are trained on each of these data sets. In the local
approximations, whole training set is used for training while in sparse approximations only
a few data points (induced points) are used for training and inference.
Another shortcoming is the necessity of selecting an appropriate covariance function before
training a GP. Selection of kernels is usually done manually. However, some work has been
done to automate kernel learning process, too [8].
2.9 Local Gaussian Processes
As described in Section 2.8, GPs scale very poorly with increase in the data points. Poor
scalability is especially a bottleneck if GP-based inference is used in real-time scenario where
the number of data points increase with time, and inference is performed after re-learning
the hyperparameters after every time step.
The main issue with learning of hyperparameters in a GP is the calculation of the inverse of
the covariance matrix K. Hence most of the fast approximations have the main purpose of
either making the covariance matrix sparse or reducing its size. Sparsity of the covariance
matrix can be induced by selecting a covariance function k(·, ·) that evaluates to zero for
most pairs of inputs. The size of the covariance matrix K can be reduced either by only
considering a certain number of points which maximize the likelihood or by considering all
the data points but as a part of different GP models which are learned separately.
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 fit on each of the clustered datasets. If the maximum number
of elements in a subset is nmax, worst case time complexity of learning all the GP models is
O(n3max
Nnmax
) ∼ O(N) (if nmax � N) where N is the total number of data points.
Abhilash Chowdhary Gaussian Processes 20
Figure 2.2: This figure shows inference and training in local GP model. Data set is clustered
into various subset and GP models are learned separately on each subset. Inference at the
yellow dot is a weighted mean of the GP inferences of the nearest (M = 4) clusters
The RBF kernel (2.8) models the covariance between the two observations such that the
correlation between them decreases exponentially as the Euclidean distance increases. Hence,
an input vector x would have little effect on the predicted value znew at location xnew if the
Euclidean distance between x and xnew is large. Therefore, the clustering step does not
affect the inherent correlation between the input vectors because an input vector x is bound
to be only correlated to the points from the same local region.
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 effects the prediction is given by Gaussian
distance between the query point, x∗, and the centroid, ck, of the cluster on which the specific
GP model is fit. The expression for vk is given by
vk = exp(− 1
2(x∗ − ck)>Wk(x∗ − ck)
)(2.27)
where W is the diagonal matrix of lengthscales in each dimension of input as mentioned in
the Section 2.6
The prediction at a desired query location x∗ is calculated as the weighted prediction z from
k nearest GP models. The weighted prediction z can be calculated in a manner similar to
locally weighted regression [29]
z = E{zi | X} =M∑i=1
zip(i | X) (2.28)
where zi is the local prediction of the each local GP model and p(i | X) is the probability of
the model i given X [18]. Using Bayes theorem, the probability of the model i given X can
be expressed as
p(i | X) = p(i,X)/k∑
i=1
p(i,X) = vi/k∑
j=1
vi (2.29)
This gives
z =
∑ki=1 vizi∑ki=1 vi
(2.30)
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
data set would be O(kn3max)� O(N3) where N is the total number of data points.
Algorithm 1 Algorithm for partitioning and learning local GP models from prior data
1: procedure PartitionAndLearnLGP
2: D ← X,Z M ← input
3: D[1] ∪ D[2] ∪ D[3].. ∪ D[M ]← kMeans(D,M)
4: for t = 1 to M do
5: ct ← calculateCentroid(D[t])
6: LGPt ← learnGPModel(D[t])
7: LGP ← LGP 1 ∪ LGP 2 ∪ LGP 3.. ∪ LGPM
8: D ← D[1] ∪ D[2] ∪ D[3].. ∪ D[M ]
9: return D,LGP
Algorithm 2 Algorithm for prediction at a location in LGP
1: procedure PredictLGP
2: D ← X,Z M ← input
3: LGP ← PartitionAndLearnLGP (D,M)
4: k ← input . Number of nearest models to consider
5: x∗, z∗ ← input . New data point
6: (c1, v1), (c2, v2), ...(ck, vk)← findKNearestModels(LGP )
7: . vi represents distance measure for each model
8: for i = 1 to k do
9: zi ← predictFromGP (LGPci)
10: z =∑k
i=1 vizi∑ki=1 vi
11: return z
Abhilash Chowdhary Gaussian Processes 23
2.9.3 Updating Local Models
One of the benefits of local models is the reduced computational complexity in learning the
hyperparameters in comparison to just a single GP model fit over the data. However, if
the number of data points are not constrained in a cluster, then computational complexity
would almost equal that of a normal GP regression trained on the whole data set.
To reduce the computational cost, the size of each of the clusters is limited to a specific
number nmax � N , where N is the total number of data points. To add a stream of data
points to the LGP model in a real-time manner, an approach similar to [20] and [17] is
followed in our work. A new data point (xnew, znew) is added to the cluster having the
center nearest to the new data point. The nearness of a cluster i is inversely proportional
to the value of vi (2.27). This requires evaluating vi with respect to each cluster which
is computational O(N). However, if the data points are clustered according to a specific
structure, more optimal algorithms can be used as described in [30]. If for a new data point,
the distance measure vi to each cluster i is less than minimum threshold vmin, then a new
cluster is created containing just the new data point (xnew, znew). After adding the new
data point to the nearest cluster k, hyperparameters of the GP model corresponding to the
cluster k are re-learned by maximizing the likelihood, as in Section 2.5.2. Re-learning the
hyperparameters requires calculating the inverse of the updated covariance matrix K(·, ·) for
the cluster k. The update is done by factorizing K(·, ·) using Cholesky decomposition and
then finding the updated inverse in O(n2max) as done in [20] and [17]. Therefore, the whole
process of updating a LGP model with a new data point is worst case O(Nn2max). Algorithm
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− lineUpdateLGP2: {LGP1, LGP2...LGPM} ← PartitionAndLearnLGP (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← numberOfDataPoints(D[k])
9: D[k]← {D[k] ∪ (xnew, znew)}10: Kn×n ← getCovarianceMatrix(LGPk)
11: Kn+1×n+1 ← updateCovarianceWithNewDataPoint(Kn×n, (xnew, znew))
12: LGPk ← reLearnHyperparametersOfLGP (Kn+1×n+1,D[k])
13: else
14: D[M + 1]← {xnew, znew}15: cM+1 ← xnew
16: LGPM+1 ← learnGPModel(D[M + 1])
17: D ← D[1] ∪ D[2] ∪ D[3].. ∪ D[M ] ∪ D[M + 1]
18: LGP ← LGP 1 ∪ LGP 2 ∪ LGP 3.. ∪ LGPM ∪ LGPM+1
19: return D,LGP
Chapter 3
State Space Inference Using Gaussian
Processes
Consider the scenario of tracking the location of an autonomous underwater vehicle (AUV)
equipped with a Doppler velocity log (DVL) which measures the altitude of the AUV above
the seafloor. The location x of the AUV is unobserved and it’s the latent system state. The
measurement of altitude z is noisy. We assume there is a spatial correlation between the
depth of seafloor at two different locations.
A state-space model assumes a Markovian process on a sequence of latent states xt [28]. The
transition between the consecutive states xt−1 and xt is modeled by a transition function
g : RP → RP . For each latent state xt there is an observation zt which is modeled with
a measurement function h : RP → RD. The state space model in NLDS or LDS can be
depicted analytically as
25
Abhilash Chowdhary State Space Inference Using Gaussian Processes 26
xt = g(xt−1) + εt, xt ∈ RP (3.1)
εt ∼ N (0,Q) (3.2)
zt = zt + νt, zt ∈ RD (3.3)
νt ∼ N (0,R) (3.4)
zt = h(xt) (3.5)
The equation (3.1) depicts the prediction model of the latent state for the Markov process
and εt in (3.2) is the prediction or process noise of the system. The observation model of
the observation zt corresponding to each latent state xt is depicted in (3.3) and (3.5) and νt
in (3.4) is the observation or measurement noise of the sensor.
xt−2
zt−2
xt−1
zt−1
xt
zt
xt+1
zt+1
Figure 3.1: Graphical model for a sequential Markov process with latent states and obser-
vation
3.1 Inference
A time series which follows a hidden Markov model (HMM) is described by (3.1) and (3.3).
The state space model described in Figure 3.1 is an example of HMM. The state xt at current
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 filtering.
The recursive calculation of the marginal distribution consists of two steps: the state predic-
tion step and the measurement update step. The measurement update step then can again
be divided into measurement prediction and Bayesian update step.
In the prediction step, p(xt | z1:t−1) is evaluated by marginalizing the prior states xt−1 from
the state transition probability p(xt |xt−1) in (3.6)
p(xt | z1:t−1) =
∫p(xt |xt−1)p(xt−1 | z1:t−1)dxt−1 (3.8)
In the measurement prediction step, observed state p(zt | z1:t−1) is predicted given p(xt | z1:t−1)
p(zt | z1:t−1) =
∫p(zt |xt)p(xt | z1:t−1)dxt (3.9)
Abhilash Chowdhary State Space Inference Using Gaussian Processes 28
In the final step of Bayes’ update, we apply Bayes’ rule to find p(xt | zt) from p(zt |xt) and
p(xt |x1:t−1)
p(xt | zt) =p(zt |xt)p(xt | z1:t−1)
p(zt | z1:t−1)(3.10)
p(xt | zt) = ηp(zt |xt)p(xt | z1:t−1) (3.11)
p(xt | z1:t) = η p(zt |xt)
∫p(xt |xt−1)p(xt−1 | z1:t−1)dxt−1 (3.12)
where η is the normalization constant given by p(zt | z1:t−1). The expression in (3.12) can
be evaluated analytically for the case of LDS with Gaussian prediction and measurement
noise, as in (3.2) and (3.4). Kalman filters [32] work well in certain cases (e.g., systems with
unimodal noise) for evaluating (3.12). Otherwise, sample based approaches such as particle
filter [26] are appropriate.
Algorithm 4 Bayesian filtering to perform exact inference in state space model
1: procedure BayesF ilter
2: x0 ← initialState, p(x1 |x0)← µ0,Σ0
3: for t = 1 to T do
4: Predict the next state given previous observations: p(xt | z1:t−1) . Equation (3.8)
5: Measurement prediction step : calculate p(zt | z1:t−1) . Equation (3.9)
6: Bayes’ Update step : calculate p(xt | z1:t) . Equation (3.12)
7: (µt,Σt)← p(xt | z1:t)
8: return µ1:T ,Σ1:T
Kalman filters and extended Kalman filters rely on the assumption that the noise in the
measurements and prediction steps are Gaussian. That is not usually the case in the real
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 meth-
ods [7].
3.2 Particle Filters
Sequential Monte Carlo methods are also regarded as particle filters in the literature. In
particle filtering based state estimation, the probability distribution over the latent state xt
is represented by a set of particles. Each particle represents a possible value for the state
[27]. In particle filtering, the integral in (3.12) is replaced with the sum of the probability
measures for each of the particles.
3.2.1 Basic Description for AUVs
Consider an AUV which has a prior model of the depth around the given area. To localize
the AUV, we represent its position with a number of particles in the map where each of
those particles is a probable location of the AUV. Each particle has a full description of
the possible future states. As the AUV receives new depth measurements, it calculates the
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)
Xt = {xit, w
it}i=1,...,NP
where xit is the ith particle for the current state, wi
t is the corresponding
weight for the particle for current belief and NP is the number of particles used for estimating
the state. Under such a representation, belief p(xt | z1:t) in (3.12) can be approximated as
[7]:
p(xt | z1:t) = η p(zt |xt)
NP∑i=1
p(xit |xi
t−1)wit−1 (3.13)
For estimating the states sequentially using particle filters according to the approach outlined
in Algorithm 4, we start with the initial estimate of the state x0 with randomly distributed M
particles. Here each particle represents the possible value for the state. Then we recursively
follow the following steps to get sequential estimate of belief p(xt | z1:t) at each time step:
1. Generate another set of particles by propagating the current set using the transition
Abhilash Chowdhary State Space Inference Using Gaussian Processes 31
probability given in (3.6) xit ∼ p(xi
t |xit−1)
2. Each of the newly generated particles xit is weighted by the likelihood of the most
recent measurement zt given the sampled state xit. The likelihood is given by the
measurement model p(zt |xit) as given in (3.7). After this step we have a set Xt of the
particles and their weights.
3. Importance sampling of the particles in the set Xt is done where particles with less
weights are thrown away and particles with higher weights are duplicated. Re-sampling
avoids the scenario where after some time only one particle has non-zero weight.
In most of the cases, the state of the process at each time step is approximated as the
sample mean of the particles E[xt] = ΣNPi=1w
itx
it.
Algorithm 5 Estimating states using Particle Filter with importance sampling
1: procedure ParticleF ilter
2: {xi0}i=1,..,NP
← initialState, p(xt |xt−1) ← transitionDistribution, p(zt |xt) ←measurementModel
3: for t = 1 to T do
4: Importance Sampling :
5: Sample {xit}i=1,..,NP
∼ p(xt |xt−1)
6: Evaluate importance weights {wit}i=1,..,NP
← p(zt |xit)
7: Xt = {xit, w
it}i=1,...,NP
8: Selection:
9: Re-sample with replacement N particles from the set Xt using the weights
10: µt ← E[xt] = ΣNPi=1w
itx
it, Σt ← V[xt]
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 filtering scenario where the observations are modeled
as a Gaussian process model.
3.3.1 GP as Observation Model
Particle filters, that use GP to model the observation given state variable, have been used in
the past for inference over various state space models [13], [14]. As GPs are a nonparametric
model, they have infinite dimensional parameter space. The effective complexity of the GP
model adpats to the data. This results in GPs being one of the better choices where there is
a dearth of training data and the data points are scattered.
A GP model is fit on training points which are known a priori for the system or are sampled
from the observation of the system. Given the training setD ∈ {(x0, z0), (x1, z1), ...(xm, zm)} =
{X,Z}, where xi represents the true state and zi represents the observation from the prior
data, we fit a Gaussian process regression over it. As a result, the observation likelihood
p(zt |xt) required in (3.13) is given by the predictive distribution of the GP model as given
in (2.14). This gives
p(zt |xt) ∼ N (µGP (xt,D),σGP (xt,D)) (3.14)
Abhilash Chowdhary State Space Inference Using Gaussian Processes 33
where
µGP (xt) = −K(xt,X)[K(X,X) + σ2nI]−1Z (3.15)
σGP (xt) = K(xt,xt)−K(xt,X)[K(X,X) + σ2nI]−1K(X,xt) (3.16)
here we consider zero mean GP prior. Using (3.14) as the observation model, the particle
filtering technique can be used as mentioned in Algorithm 6 to get the estimate of the state
x1:T
Algorithm 6 Estimating states using Particle Filter with GP based observation model
1: procedure GP − ParticleF ilter2: D ← X,Z get training data for observation model
3: {xi0}i=1,..,NP
← initialState, p(xt |xt−1) ← transitionDistribution, p(zt |xt) ←N (µGP (xt,D),σGP (xt,D))
4: for t = 1 to T do
5: Importance Sampling :
6: Sample {xit}i=1,..,NP
∼ p(xt |xt−1)
7: Evaluate importance weights {wit}i=1,..,NP
← p(zt |xit)
8: Xt = {xit, w
it}i=1,...,NP
9: Selection:
10: Resample with replacement N particles from the set Xt using the weights
11: µt ← E[xt] = ΣNPi=1w
itx
it, Σ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
xt−1
zt−1
State Variables
Observations
xt
zt
g(.)
h(.)Gaussian Process Model
xt+1
zt+1
Figure 3.2: Graphical model for a state-space model with observation model modeled as a
GP
the starting state of the system. As a result, we need to be able to learn the observation
model represented by GP as we explore more states.
In Algorithm 7, line (20) shows us the calculation for with the estimated mean and the
variance of the current state of the system. Our goal is to use this estimate xt ∼ N (µt,Σt)
and the observation zt made by the system at current time step to re-learn the observation
model, in this case GP. As a result, at each time step we receive an input-output pair of
(xt ∼ N (µt,Σt), zt ∼ N (zt,νt)) that can be added to the GP model. As these inputs are
uncertain, we can use (2.25) and (2.26) from Section 2.7 to find the predictive mean and
variance of observation model p(zt+1 |xt+1) at next time step. Training data Dt at each
time step can be represented as Dt ∈ {Xt,Zt} where Xt ∈ X ∪ x1:t−1 and Zt ∈ Z ∪ z1:t−1.
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
µGP (xt) = −K(xt,Xt)[K(Xt,Xt) + σ2nI + diag{∆>z ΣXt∆z}]−1Zt (3.17)
σGP (xt) = K(xt,xt)−K(xt,Xt)[K(Xt,Xt) + σ2nI + diag{∆>z ΣXt∆z}]−1K(Xt,xt) (3.18)
where diag{∆>z ΣXt∆z} is the additional term from the uncertainty of the input points Xt.
The vector ∆z represents the derivative of the GP function values with respect to the input
vector xt, and ΣXt is a diagonal matrix with diagonal elements as the noise associated to
each input dimension in Xt. As more points are added to the GP, we have to update
the input vector Xt, output vector Zt and the inverse of the modified covariance matrix
(K(Xt,Xt) +σ2nI + diag{∆>z ΣXt∆z}) for the model. Updating the input and output vectors
is fairly straightforward. However, updating the inverse of the covariance matrix is a bit more
complicated. It can be done by updating the Cholesky factorization of the covariance matrix
[24] and [18]. The computational complexity of this update is O((N + t − 1)2) where N is
the size of prior training input X. For on-line inference, this is manageable upto a threshold
value of t. For the systems where on-line learning has to be done for a long duration of time,
this approach scales poorly with time.
After incorporating more data points into the GP model, as the system encounters more
states, the hyperparameters no longer represent the optimal model. To reduce the bias
of the prediction from the observation model, we need to re-learn the hyperparameters
after including a set of data points into the model. This is again done by maximizing the
likelihood as in Section 2.5.2. If the dimension of each input vector xt is q, we have q extra
hyperparameters compared to the GP model discussed in Section 2.5.2 - one noise variance
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 − ParticleF ilter2: X1,Z1 ← X,Z, D1 ← X1,Z1 get training data for observation model
3: {xi0}i=1,..,NP
← initialState, p(xt |xt−1)← transitionDistribution
4: for t = 1 to T do
5: Importance Sampling :
6: Sample {xit}i=1,..,NP
∼ p(xt |xt−1)
7: zt ← observation
8: p(zt |xt)← N (µGP (xt,Dt),σGP (xt,Dt))
9: Evaluate importance weights {wit}i=1,..,NP
← p(zt |xit)
10: Xt = {xit, w
it}i=1,...,NP
11: Selection:
12: Resample with replacement N particles from the set Xt using the weights
13: µt ← E[xt] = ΣNPi=1w
itx
it, Σt ← V[xt]
14: Xt+1 ← Xt ∪ µt, Zt+1 ← Zt ∪ zt
15: Dt+1 ← Xt+1,Zt+1
16: UpdateGramMatrixOfGP (Dt+1)
17: if t%m == 0 then
18: [θ,ΣXt ]←MaximumLikelihoodEstimate(p(zt |xt))
19: . Learn Hyperparameters
20: return µ1:T ,Σ1:T ,
3.4 Local GP based Particle Filters
In the previous section we saw how we can use a GP as an observation model for a state space
model and use particle filtering to infer the hidden states. However, if we were to update
the GP on the go, as we encounter more states, the time complexity scales proportional to
O(t2) where t is the number of states encountered so far. Therefore there is a need to have
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.
xt−2
zt−2
xt−1
zt−1
State Variables
Observations
xt
zt
g(.)
LGPi−1 LGPiLocal Gaussian Processes
LGP model
xt+1
zt+1
xt+2
zt+2
h(.)
LGPi+1
Figure 3.3: Graphical model for a state-space model with LGP as the observation model.Here
xt denote the latent states of the system and zt denote the observations made by the system.
The nodes LGPi model the local GP models using a subset of the observations and corre-
sponding states. h() is the observation model which consists of all the local GP models. g()
is the prediction model of the system.
In Section 2.9, we saw that if the dataset is spatially correlated, local approximation of
Gaussian processes can handle the issue of poor scalability of GP models by bounding the
time complexity for large amount of data even when the data is streamed and incremental
updates need to be performed. As a result, it makes sense to use LGP model as the obser-
vation model in state space inference when we need to update the model at each new state.
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 Chapter 2, LGP is described as a collection of GPs modeled on disjoint datasets. A
prior training data set D ∈ {X,Z} is used to learn a LGP model as in Algorithm 1. This
serves as our observation model as was the case in Section 3.3 where we used a normal
GP model fit on D as the observation model. We follow the approach outlined by Algo-
rithm 8 for state estimation with LGP as observation model in particle filter. We start
with fitting a local GPmodel LGP over the prior data D1. This results in D being split
into M clusters {D1[1],D1[2], ...D1[M ]} where subscript denotes the time step for each iter-
ation of the particle filter. Fitting a LGP model on this data results in different GP models
{LGP1, LGP2, ..., LGPM}, where each of them is learned on a different subset of the parti-
tioned data. As our system proceeds with time, with each particle update and observation
lines 7 and 8, the particle weight is calculated based on the estimate of p(zt |xit) using esti-
mate from the nearest local model (Algorithm 2). From lines 15 to 21, importance sampling
of the particles is done and the mean estimate of the state is calculated using the re-sampled
states. Based on this mean estimate µt, we find the nearest local model from it. Subse-
quently, the new input output pair of data point, (µt, zt) is added to the nearest model
and its gram matrix is updated (lines 18 to 20) using the same factorization techniques as
mentioned in previous section .
Abhilash Chowdhary State Space Inference Using Gaussian Processes 39
Algorithm 8 Local GP based Particle Filter with on-line updating
1: procedure LGP − ParticleF ilter2: X1,Z1 ← X,Z, D1 ← X1,Z1 get training data for observation model
3: LGP, {D1[1],D1[2], ...D1[M ]} ← PartitionAndLearnLGP (D1,M)
4: {xi0}i=1,..,NP
← initialState, p(xt |xt−1)← transitionDistribution
5: for t = 1 to T do
6: Importance Sampling :
7: Sample {xit}i=1,..,NP
∼ p(xt |xt−1)
8: zt ← observation
9: for i = 1 to N do
10: µLGPk(xi
t,Dt),σLGPk(xi
t,Dt)← PredictLGP (xit, )
11: p(zt |xit)← N (µLGPk
(xit,Dt),σLGPk
(xit,Dt))
12: Evaluate importance weights {wit}i=1,..,NP
← p(zt |xit)
13: Xt = {xit, w
it}i=1,...,NP
14: Selection:
15: Re-sample with replacement N particles from the set Xt using the weights
16: µt ← E[xt] = ΣNPi=1w
itx
it, Σt ← V[xt]
17: k ← findNearestModel(µt, LGP )
18: Xt+1[k]← Xt[k] ∪ µt, Zt+1[k]← Zt]k] ∪ zt
19: Dt+1[k]← Xt+1[k],Zt+1[k]
20: UpdateGramMatrixOfGP (Dt+1[k])
21: for j = 1 to M and j 6= k do
22: Xt+1[j]← Xt[j], Zt+1[j]← Zt]j]
23: Dt+1[j]← Xt+1[j],Zt+1[j]
24: if mth new point for model k since last update then
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 fitted with sensors such as AHRS (attitude and heading
reference system ) and DVL (Doppler Velocity Log) uses the state space inference algorithms
described in the previous chapters to navigate underwater. In this chapter a scalable and
on-line Gaussian process based terrain-aided navigation (TAN) algorithm is discussed which
is specific to the sensors (DVL and AHRS) used in the Virginia Tech 690s (Figure 4.1) .
4.1 The Terrain-Aided Navigation Problem
The terrain-aided navigation problem refers to the generalized problem of localization using
an a priori known map. Localization is done with the help of different sensors which measure
various characteristics of the terrain and reference them to information in the a priori map
[4].
Terrain-aided navigation is particularly suitable in the areas where GPS is denied or GPS
readings are noisy like under water, indoor spaces, etc. In this work, an AHRS provides
40
Abhilash Chowdhary Terrain-Aided Navigation 41
Figure 4.1: The Virginia Tech 690s during Panama City trials
attitude measurements and a DVL provides body velocity and range measurements to the
seafloor. Using attitude and velocity, the future state of the AUV is predicted using a
kinematic model.
Let us define the terrain-aided navigation problem and state space variables with respect to
an AUV with AHRS and DVL as its main sensor unit and a priori depth map of the sea
bed near the initial location.
4.1.1 Problem Statement
Let
• pt = [x y z φ θ ψ] be the pose of the robot at time t with respect to north-east down
Abhilash Chowdhary Terrain-Aided Navigation 42
(NED) frame
• vt = [u v w p p r] be the robot velocity with respect to robot fixed frame
• M be the a priori map of the terrain with depth points at various locations.
• xt be the state vector of the robot which is estimated over time. In this work it is only
the position of the robot in X and Y coordinates in world frame (NED).
• zt be the observation acquired by the robot. In this work, observations are altitude
measurements from multibeam DVL on the z axis of inertial earth fixed frame.
The aim of terrain-aided navigation is to estimate the states xt over a period of time by
matching the observation zt against the a priori map M [4].
4.2 State Space Model for TAN
As the problem of TAN requires to infer the latent states xt over a period of time given
observations zt, this could well be modeled as a state space model. For inference, we could
use a Bayesian filtering algorithm as described in the previous chapters. The prediction
model and observation model of the filtering mechanism to estimate the states is described
below.
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 local-
ization of the AUV in the X-Y plane of earth inertial frame. For terrain-aided navigation
algorithm to work well, we assume that the AUV has a zero pitch for the whole mission. As
a result, the AUV’s state is taken as xt = [x y]. In a particle filtering scenario, the particle
set can be defined as
xt =
x1t ... xNP
t
y1t ... yNP
t
(4.1)
The prediction model for the AUV is expressed
xt =f(xt−1) + εt (4.2)
f(xt−1) =xt−1 + ∆txt−1 (4.3)
xt−1 =[ut−1 vt−1] (4.4)
where (4.3) is based on the kinematic model of the AUV, and ut and vt are the velocity
components along X-axis and Y-axis respectively. The values of ut and vt at each time step
are the measurements from the DVL. Using (4.3) and (4.4), (4.2) can be written
xt = xt−1 + ∆t[ut−1 vt−1] + εt (4.5)
where εt denotes the noise in the prediction of the current state from previous. In a manner
similar to (3.2), we take εt as a Gaussian noise with zero mean εt ∼ N (0,Q). Thus the
current state xt is from a normal distribution with mean f(xt−1) and variance Q that is.
p(xt |xt−1) ∼ N (xt−1 + ∆t[ut−1 vt−1],Q) (4.6)
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 fit a
Gaussian process model over the prior map M. A GP model provides a mapping from the
current state space to the observation space. The prior map M being too sparse over the area
of mission, cannot directly be used as the mapping function between these two spaces. In this
work we use LGP to model the prior depth readings from the given map M in a manner simi-
lar to Section 3.4. The LGP model is initialized and learned as mentioned in Section 2.9. For
this case, data setD is represented by the M ∈ {((x1, y1), d1), ((x2, yt2), d2) . . . ((xN , yN), dN)}
where (xi, yi) represents the location of the depth reading di in north east down (NED) frame.
The initial input to the GP model can be represented as X1 ∈ {(x1, y1), (x2, y2) . . . (xN , yN)}
where the subscript 1 denotes that this is the training set before the estimation process be-
gins. The output for this training set of the GP can be written as Z1 ∈ {d1, d2 . . . dN}. So the
a priori training set can be represented as D1 ∈ {X1,Z1}. This data set is then partitioned
into M clusters based on the inputs X0. The partitioned data sets can be represented simi-
larly as in Section 3.4 {D1[1],D1[2], ...D1[M ]} where D1[i] represents the partitioned training
data for each of the cluster.
A GP model is learned on each of the partitioned training sets D1[i] i = 1, . . . ,M . Each of
the GP models is denoted LGPi, for the ith partition. The mean prediction and the predictive
variance at a test point xt at time step t is computed µLGPi(xt,Dt[i]) and σLGPi
(xt,Dt[i])
respectively for each local GP model LGPi. The final predictive distribution of the measure-
Abhilash Chowdhary Terrain-Aided Navigation 45
ment zt given xt is a weighted measure of distributions from the k nearest LGP models
p(zt |xt,Dt) ∝k∑
i=1
viN (µLGPi(xt,Dt[i]),σLGPi
(xt,Dt[i])) (4.7)
where the k nearest neighbors are found based on the nearness measure vi as computed
in (2.27). Concisely, (4.7) describes the observation model for the terrain-aided navigation
algorithm used in this work.
4.2.3 Multibeam DVL
DVL used in this work has four beams which gives four range measurements from the sea
floor. Each of the beams are at predetermined angles from the body frame z-axis. Let
r1, . . . , r4 represent the range measurements from each of these beams of the DVL. Using the
geometry related to the angle of the beam with respect to each of the axis of the coordinate
frames, we calculate the vertical component of each range measurement and the location on
the seafloor corresponding to each range measurements. Thus we compute the depth at four
separate locations x1,t, . . .x4,t. These depth measurements are h1, . . . h4.
4.3 Particle Filtering strategy for multibeam DVL
For incorporating the measurements h1, . . . h4 the prediction and the LGP model remains
same as in Section 4.2.1 and Section 4.2.2. However, likelihood of the observations p(zt |xt)
changes. In this section, re-modeling of likelihood in a particle filter to incorporate multibeam
measurements is shown.
Abhilash Chowdhary Terrain-Aided Navigation 46
4.3.1 Likelihood for re-sampling
In Section 3.2.2 we used likelihood p(zt |xit) of the observation zt as the weight for re-
sampling each particle xit. When using multibeam DVL, observation at each time step t
is zt = [h1, h2 . . . h4]. The likelihood then becomes p(h1, h2 . . . h4 |xit). In our observation
model, all the altitude measurements h1, h2 . . . h4 at time step t are independent of each
other given the previous observations(Zt), and previous states(Xt) because the LGP model
is learned on (Xt,Zt). This likelihood can be factorized as below:
p(zt |xit) = p(h1, h2 . . . h4 |xi
t,Xt,Zt)
= p(h1, h2 . . . h4 |xi1,t,x
i2,t . . .x
i4,t,Xt,Zt)
= p(h1 |xi1,t,x
i2,t . . .x
i4,t,Xt,Zt)p(h2 |xi
1,t,xi2,t . . .x
i4,t,Xt,Zt) . . . p(h4 |xi
1,t,xi2,t . . .x
i4,t,Xt,Zt)
= p(h1 |xi1,t,Xt,Zt)p(h2 |xi
2,t,Xt,Zt) . . . p(h4 |xi4,t,Xt,Zt)
∴ p(zt |xit) = p(h1 |xi
1,t,Xt,Zt)p(h2 |xi2,t,Xt,Zt) . . . p(hb |xi
b,t,Xt,Zt) (4.8)
where Xt and Zt denote the observations prior to time step t. Here each of the terms
{p(hl |xl,t,Xt,Zt)}4l=1 is evaluated from the LGP model using (4.7).
4.3.2 Inference
The inference in a TAN problem refers to estimating the state variables xt of the system over
a period of time T such that 1 ≤ t ≤ T . In this work we use the particle filtering framework
with LGP as the observation model for estimation of xt (section 3.4). Re-sampling of the
particles is done based on the value of p(zt |xt) which is calculated as shown above.
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
probability given in 4.6 xit ∼ p(xi
t |xit−1)
2. For each of the particles xit we have four points xi
1,t,xi2,t . . .x
i4,t. And each of these
points in X-Y plane correspond to an altitude measurements from each of the four
beams of the DVL. The weight of each of the newly generated particles xit is given
by the likelihood p(zt |xit). After this step we have a set Xt of the particles and there
weights.
3. Importance sampling of the particles in the set Xt is done where particles with less
weights are thrown away and particles with higher weights are duplicated. Resampling
avoids the scenario where after some time only one particle has non-zero weight.
We approximate the state of the process at each time step by the sample mean of the
particles E[xt] =∑N
i=1witx
it.
Detailed procedure for performing inference and learning is shown in Algorithm 9.
4.4 Cost of running into the shore
To plan a path for the AUV, we need to quantify the cost associated to the path so that the
path with the minimum cost is selected. For many underwater missions, an AUV needs to
maintain a constant depth of dthreshold from the surface of water. When surveying near the
Abhilash Chowdhary Terrain-Aided Navigation 48
Algorithm 9 Local GP based Particle Filter (LGP-PF) with multibeam DVL measurements
1: procedure LGP − ParticleF ilter −MultiBeam
2: X1,Z1 ← X,Z, D1 ← X1,Z1 get training data for observation model
3: LGP, {D1[1],D1[2], ...D1[M ]} ← PartitionAndLearnLGP (D1,M)
4: {xi0}i=1,..,NP
← initialState, p(xt |xt−1)← transitionDistribution
5: for t = 1 to T do
6: Importance Sampling :
7: Sample {xit}i=1,..,NP
∼ p(xt |xt−1)
8: zt = [h1, h2 . . . hb]← observation
9: for i = 1 to NP do
10: [LGP1, LGP2 . . . LGPk]← kNearestLGPModels(xit, LGP )
11: for l = 1 to k do
12: vl ← exp(− 1
2(x− ck)>Wk(x− ck)
)13: µLGPl
(xit,Dt),σLGPl
(xit,Dt)← PredictLGP (xi
t, Dt)
14: for j = 1 to b do
15: p(hj |xij,t)←
k∑l=1
vlN (µLGPl(xi
j,t,Dt[l]),σLGPi(xi
j,t,Dt[l]))
16: Evaluate importance weights {wit}i=1,..,NP
← p(h1 |xi1,t)p(h2 |xi
2,t) . . . p(h4 |xi4,t)
17: Xt = {xit, w
it}i=1,...,NP
18: Selection:
19: Resample with replacement N particles from the set Xt using the weights
20: µt ← E[xt] = ΣNPi=1w
itx
it, Σt ← V[xt]
21: k ← findNearestModel(µt, LGP )
22: Xt+1[k]← Xt[k] ∪ µt, Zt+1[k]← Zt]k] ∪ zt
23: Dt+1[k]← Xt+1[k],Zt+1[k]
24: UpdateGramMatrixOfGP (Dt+1[k])
25: for j = 1 to M and j 6= k do
26: Xt+1[j]← Xt[j], Zt+1[j]← Zt]j]
27: Dt+1[j]← Xt+1[j],Zt+1[j]
28: if mth new point for model k since last update then
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 quantified as the probability of the AUV running into the shore given
the path between two points. Here the aim is not to come up with various paths which
are feasible, but quantify the cost associated to each path given a list of paths between two
points.
A path P from point A to point B on North-East Down frame for an AUV can be discretized
into a finite number (say q) of path points (c1, c2 . . . cq) where each path points ci denotes the
location in X-Y coordinates of the discretized point on the NED frame. Using the observation
model (GP or LGP) already fit on the a priori map, we can find the predictive distribution
of depth zci at each of the path point ci (2.14). The cost of running into the ground at
ci then can be quantified as P (zci < dthreshold | ci), the probability of predicted depth being
greater than the depth threshold dthreshold. This is given by
lci = P (zci < dthreshold | ci) =
∫ dthreshold
−∞p(z | ci) dz (4.9)
where p(z | ci) is evaluated using (4.7). As each of P (zci < dthreshold | ci) is a normal distri-
bution, (4.9) can be rewritten [31]
lci = Φ(dthreshold − µci
σci
)(4.10)
where Φ(x) denotes the cumulative distribution function of standard normal distribution
and, µci and σci are the mean and variances of p(zci | ci).
Chapter 5
Experimental Evaluation
This chapter provides results from experimental evaluation of the LGP-PF based localization
(Algorithm 9) of the AUV. At first, the LGP-PF method is evaluated in a simulated setting
with a synthetic prior map data and simulated trajectories of the AUV over the map. The
algorithm is tested in a real world setting with actual depth data and trajectory of the AUV
from trials performed near Panama City, Florida in St. Andrew Bay.
5.1 Simulation based Experiments
For simulation based experiments, a grid of 600×300 meters, as shown in Figure 5.1, was
initialized with ground truth depth values. There are total of 180,000 points on the grid
with ground truth depth measurements. The depth values range from 0 to 85 meters. A
prior mapM was created with 1900 random points by adding a Gaussian noise of zero mean
and one standard deviation to each ground truth depth measurement. Using this sparse
collection of depth points in M, a GP/LGP model is fit over the whole grid. This becomes
50
Abhilash Chowdhary Experimental Evaluation 51
the observation model for localizing the AUV, as explained in Section 4.2.2.
Figure 5.1: 3D plot of 600×300 grid depicting the terrain of the seafloor. 0 on the Z-axis
represents the bottom most point of the floor from the surface.
5.1.1 Comparison of Local and Sparse Approximation based meth-
ods
The mean prediction and predictive variance for both local approximation based LGP model
and sparse approximation based GP model are compared. The LGP model used for synthetic
experiments has 30 local GP models. The sparse approximation implementation for the GP
is used from [10]. The root mean squared error for the prediction of depth over the whole
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
fit over prior map as the observation model, trajectories of the AUV are estimated using
LGP-PF algorithm with sampled variance in the prediction model. Using these location
estimates and corresponding depth values from the grid, the LGP model is re-learned. With
the re-learned LGP model, the trajectories for each of the sampled ten paths are again
estimated using LGP-PF with sampled variance in the prediction model. Figure 5.3 (a)
shows the ten sampled paths. Figure 5.3 (b) shows the estimated trajectories using LGP-PF
with prior LGP model and Figure 5.3 (c) shows the estimated trajectories using LGP-PF
with LGP model re-learned using estimates from 5.3 (b). The Euclidean error et at time t
for a sampled path is expressed
et =√
(xt − xt)2 + (yt − yt)2 (5.1)
where (xt, yt) is the location of AUV in the sampled path at t and (xt, yt) is the estimated
location of AUV using LGP-PF at time t for the sampled path. Our intuition is that the
errors for trajectory estimates from LGP-PF with re-learned LGP model should be less than
the errors for trajectory estimates from LGP-PF with prior LGP model. Figure 5.3 (d)
shows the averaged Euclidean error (5.1) for the estimated trajectories in Figure 5.3 (b) and
Figure 5.3 (c). We see that the trajectory estimates using re-learned LGP model gives less
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 field trials, is described in this section.
The sensors in the AUV that are used for these experiments are listed in the Table 5.2.1.
The Microstrain 3DM-GX3-25 AHRS reports the attitude (roll, pitch and yaw) of the AUV.
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 find the depth from the surface. The readings from this sensor
are used to control the AUV at a constant depth below the surface of water.
5.2.2 Field Trials
The Virginia Tech 690s was deployed near Panama City, Florida, in St. Andrew Bay. The
AUV was commanded to go from one location to another between four different way-points.
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 fit on the training
data points from first and second mission.
There were two way-point missions that were conducted. The first mission was a surface
mission where the AUV did not dive and remained on the surface. This was done to acquire
a prior depth map of the floor along with position from the GPS. The second mission was
carried out at a depth of 2 meters from the surface of water. The way-points were distributed
in a diamond pattern with the adjacent ones being about 260 meters away from each other.
The AUV surfaced at four of the way-points to get GPS measurements.
To infer and model depth at unknown location, LGP model is fit on the depth data points
and their corresponding GPS location from the first mission. However, this did not cover
much of the AUV’s path in the second mission. Hence, some of the location estimates
from the range based navigation approach in [12] (which was already implemented on to the
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 fit on the training data
points from first and second mission.
AUV’s processing unit) paired with the depth readings from DVL, were also used to train
the model. These data points were taken at a frequency of 0.5 Hz. Figure 5.5 shows sparse
GP based prior map and the training data points from the first and second run used to build
the model. Total number of input data points used for training are 4834. Figure 5.6 shows
sparse GP based prior map and the training data points from the first and second run used
to build the model. Although the prediction of depth is quite smooth using sparse GP model,
computationally this model is expensive as it has to select the optimal subset of inducing
input data points for the given data set. And this is also not feasible for on-line learning
approach where more input data points are added continuously with time. Therefore, for
estimating the position of AUV we only use the prior map learned from LGP model.
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 (Algorithm 9) off-line on a 1.6 GHz Intel Core i5 computer
with the data from the second mission. The implementation of the algorithm was done in
Python and GPy library [10] was used for inference and learning of Gaussian processes.
Our algorithm was tested against the location estimates from the range based navigation
approach in [12]. As the AUV was under water over the course of mission and only resurfaced
at the 4 way-points, the location estimate from our LGP-PF algorithm was also compared
to the estimates from the range based solution. With the data from the sensors, depth and
velocity measurements from DVL and yaw readings from AHRS, AUV is localized using
LGP-PF algorithm. The LGP-PF based localization approach used in this work does not
use the GPS updates for correcting its location estimate AUV unlike [12].
The DVL used in the AUV has four beams that provides four different altitude measurements
above the seafloor at a single time step. Figure 5.7 shows the estimated trajectory from LGP-
PF compared to the estimation from [12] and four GPS updates when the AUV resurfaces. In
the kinematic model of LGP-PF, the variance used was 0.05meter2. For this implementation,
50 particles were used for the estimation in the particle filter. For learning the LGP model,
prior depth points were clustered into 30 different clusters and RBF covariance kernel (2.8)
was used for GP model over all these clusters.
Abhilash Chowdhary Experimental Evaluation 61
Figure 5.8: This figure shows three different areas on the predicted depth map from LGP.
White region denotes where the prediction was zero and is neglected. Black dots represent
the down-sampled prior depth data points used for learning the LGP model.
Based on the number of prior data points and the terrain profile of the region of field
trials, the predicted depth map is divided into four different regions of north, south, east
and west. Figure 5.8 shows these four regions on a contour plot of mean predicted depth
using the LGP model along with down-sampled prior depth readings as black dots. Along a
trajectory with significant depth variation the LGP-PF algorithm excludes the unimportant
Abhilash Chowdhary Experimental Evaluation 62
Figure 5.9: This figure shows the AUV starting in the south (initial start point) and it
resurfaces in north with GPS update. The error in estimation is about 10 meters.
particles for AUV’s position and as a result the location estimation is likely to have less
errors. However, along a trajectory with low depth variation, particles in LGP-PF algorithm
tend to have similar weights and the AUV’s state is distributed uniformly over the state
space after importance sampling which results in location estimate to have high error values.
Figures 5.9, 5.10, 5.11 and 5.12 show the location estimates of the AUV using LGP-PF, and
the GPS updates from each of the four resurfaces of the AUV.
Abhilash Chowdhary Experimental Evaluation 63
Figure 5.10: This figure shows the AUV starting from south and going to east via west and
north. The error in estimation is about 25 meters with respect to GPS update.
Abhilash Chowdhary Experimental Evaluation 64
Figure 5.11: This figure shows the AUV starting from west and going to north via east. The
error in estimation increases to about 30 meters with respect to GPS update.
Abhilash Chowdhary Experimental Evaluation 65
Figure 5.12: This figure shows the AUV starting from the south and going to east via west.
The error in estimation drops to about 8 meters with respect to GPS update.
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 significant, the estimation of state has low error relative to
the GPS update.
In Figure 5.10, the AUV goes from south to east passing through the northern and the
western regions. Because of starting from the southern region, which has almost no depth
variability, AUV initially accumulates significant error. In this trajectory while going from
west to north, the depth gradient is non-zero which decreases the error in LGP-PF. How-
ever, the northern region has almost non-varying predicted depth which results in AUV
accumulating more error in its location estimate. In the end while going from north to east,
the depth variability is again very low, which combined with already accumulated error,
increases the location estimation error to considerable amount (around 25 meters).
In Figure 5.11, the AUV goes from west to east passing through the northern region. Al-
though the depth variation while going from west to east is significant, due to low depth
variation from east to north and relatively non-varying depth in the northern region, the lo-
cation estimate error with respect to GPS updates is considerably large (around 30 meters).
In Figure 5.12, the AUV goes from south to east passing through the western region. Be-
cause of almost non-varying depth between south and west, the initial error is bound to be
significant. However, as the AUV traverses from west to east, the errors start converging
because of significant variation in depth along the path. This combined with AUV’s final
movement across the eastern region, which has varying predicted depth, lowers the error in
AUV’s estimated location with respect to GPS update when it resurfaces. The error drops
Abhilash Chowdhary Experimental Evaluation 67
Figure 5.13: This figure shows the errors in location estimate using LGP-PF relative to GPS
updates at each of the AUV resurface. Negative error means AUV is underwater and does
not have GPS fix.
down to about 8 meters.
Figure 5.13 shows errors in the location estimates of the AUV using LGP-PF with respect
to four GPS updates over the whole trajectory. The error rises for the first three GPS
updates and it drops down to considerably low values for the last GPS update. Terrain-
aided navigation algorithm discussed in this thesis is not GPS aided at any of the GPS
update events and the correction in the AUV’s location estimate is solely done via LGP-PF.
This shows that using a LGP-PF based localization approach, the accumulated error along
a trajectory can be reduced to considerably low values if the terrain has significant depth
variation. This makes LGP based filtering approach a good estimation algorithm where
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 algorithm 9 needs a
weighting function which does not just rely on the likelihood of measured depth given LGP
model. This could be done by using additional sensors like inertial measurement unit (IMU)
and fusing their measurements as well in the observation model of LGP-PF.
Chapter 6
Conclusions
This work proposes a state-space inference algorithm, LGP-PF, using non-parametric filter-
ing technique with local approximation of Gaussian processes as the observation model over
the measurements. A LGP-PF based implementation of terrain-aided navigation algorithm
is also presented. To the best of our knowledge, this is the first work in using LGP based
observation model in a terrain-aided navigation problem for AUVs. This work shows that
given a terrain with high depth variance, the error in location estimate of LGP-PF based
terrain-aided navigation converges to acceptable low values. For a terrain with non-varying
depth, the algorithm’s estimate of AUV’s location has significant errors as compared to other
navigation algorithms. The proposed terrain-aided navigation algorithm was evaluated on
datasets from field trials near Panama City, Florida.
69
Abhilash Chowdhary Conclusions 70
6.1 Future Work
In this work, the experimental evaluation was done in a relatively less feature-rich terrain.
One of the future additions to this work would be testing this algorithm in an environment
with very rich terrain profile. Also, in the localization algorithm described in this work,
the observation model is only based on the measurements from a single sensor (multibeam
DVL in this case). By fusing more measurements from different sensors like IMU etc., the
estimation could be improved. Also the states of the filter only include the 2-D position
of the AUV. The states which are directly observable from the sensors, for example body
velocity, attitude, and depth, are not included. By including these states in another Bayesian
filter (like in [2]), the errors in their estimate could be reduced.
Bibliography
[1] Stephen Barkby, Stefan Williams, Oscar Pizarro, and Michael Jakuba. Bathymetric par-
ticle filter slam using trajectory maps. The International Journal of Robotics Research,
page 0278364912459666, 2012.
[2] Stephen Barkby, Stefan B Williams, Oscar Pizarro, and Michael V Jakuba. A featureless
approach to efficient bathymetric slam using distributed particle mapping. Journal of
Field Robotics, 28(1):19–39, 2011.
[3] Asher Bender, Stefan B Williams, and Oscar Pizarro. Autonomous exploration of large-
scale benthic environments. In Robotics and Automation (ICRA), 2013 IEEE Interna-
tional Conference on, pages 390–396. IEEE, 2013.
[4] Sebastian Carreno, Philip Wilson, Pere Ridao, and Yvan Petillot. A survey on terrain
based navigation for auvs. In OCEANS 2010, pages 1–7. IEEE, 2010.
[5] Brian Claus and Ralf Bachmayer. Terrain-aided navigation for an underwater glider.
Journal of Field Robotics, 32(7):935–951, 2015.
71
Abhilash Chowdhary Conclusions 72
[6] Shandor Dektor and Stephen Rock. Improving robustness of terrain-relative navigation
for auvs in regions with flat terrain. In 2012 IEEE/OES Autonomous Underwater
Vehicles (AUV), pages 1–7. IEEE, 2012.
[7] Arnaud Doucet, Nando De Freitas, and Neil Gordon. An introduction to sequential
monte carlo methods. In Sequential Monte Carlo methods in practice, pages 3–14.
Springer, 2001.
[8] David Duvenaud. Automatic model construction with Gaussian processes. PhD thesis,
University of Cambridge, 2014.
[9] Austin Eliazar and Ronald Parr. Dp-slam: Fast, robust simultaneous localization and
mapping without predetermined landmarks. In IJCAI, volume 3, pages 1135–1142,
2003.
[10] GPy. GPy: A gaussian process framework in python. http://github.com/
SheffieldML/GPy, since 2012.
[11] John A Hartigan and Manchek A Wong. Algorithm as 136: A k-means clustering algo-
rithm. Journal of the Royal Statistical Society. Series C (Applied Statistics), 28(1):100–
108, 1979.
[12] Rami S Jabari and Daniel J Stilwell. Range-based auv navigation expressed in geodetic
coordinates. In OCEANS 2016 MTS/IEEE Monterey, pages 1–8. IEEE, 2016.
Abhilash Chowdhary Conclusions 73
[13] Jonathan Ko and Dieter Fox. Gp-bayesfilters: Bayesian filtering using gaussian process
prediction and observation models. In Intelligent Robots and Systems, 2008. IROS 2008.
IEEE/RSJ International Conference on, pages 3471–3476. IEEE, 2008.
[14] Jonathan Ko and Dieter Fox. Learning gp-bayesfilters via gaussian process latent vari-
able models. Autonomous Robots, 30(1):3–23, 2011.
[15] Andrew McHutchon and Carl E Rasmussen. Gaussian process training with input noise.
In Advances in Neural Information Processing Systems, pages 1341–1349, 2011.
[16] Duy Nguyen-Tuong and Jan Peters. Local gaussian process regression for real-time
model-based robot control. In 2008 IEEE/RSJ International Conference on Intelligent
Robots and Systems, pages 380–385. IEEE, 2008.
[17] Duy Nguyen-Tuong and Jan Peters. Local gaussian process regression for real-time
model-based robot control. In Intelligent Robots and Systems, 2008. IROS 2008.
IEEE/RSJ International Conference on, pages 380–385. IEEE, 2008.
[18] Duy Nguyen-Tuong, Matthias Seeger, and Jan Peters. Model learning with local gaus-
sian process regression. Advanced Robotics, 23(15):2015–2034, 2009.
[19] Duy Nguyen-Tuong, Matthias Seeger, and Jan Peters. Real-time local gp model learn-
ing. In From Motor Learning to Interaction Learning in Robots, pages 193–207. Springer,
2010.
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, Gael Varoquaux, Alexandre Gramfort, Vincent Michel, Bertrand
Thirion, Olivier Grisel, Mathieu Blondel, Peter Prettenhofer, Ron Weiss, Vincent
Dubourg, et al. Scikit-learn: Machine learning in python. Journal of Machine Learning
Research, 12(Oct):2825–2830, 2011.
[23] Carl Edward Rasmussen. Gaussian processes for machine learning. 2006.
[24] Matthias Seeger. Low rank updates for the cholesky decomposition. Technical report,
2004.
[25] Edward Snelson and Zoubin Ghahramani. Sparse gaussian processes using pseudo-
inputs. Advances in neural information processing systems, 18:1257, 2006.
[26] Sebastian Thrun. Particle filters in robotics. In Proceedings of the Eighteenth conference
on Uncertainty in artificial intelligence, pages 511–518. Morgan Kaufmann Publishers
Inc., 2002.
[27] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press,
2005.
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
Artificial Intelligence and Statistics, pages 868–875, 2010.
[29] Sethu Vijayakumar, Aaron D’souza, and Stefan Schaal. Incremental online learning in
high dimensions. Neural computation, 17(12):2602–2634, 2005.
[30] Roger Weber, Hans-Jorg Schek, and Stephen Blott. A quantitative analysis and per-
formance study for similarity-search methods in high-dimensional spaces. In VLDB,
volume 98, pages 194–205, 1998.
[31] Eric W Weisstein. Normal distribution. 2002.
[32] Greg Welch and Gary Bishop. An introduction to the kalman filter. 1995.
[33] Stefan Williams, Gamini Dissanayake, and Hugh Durrant-Whyte. Towards terrain-aided
navigation for underwater robotics. Advanced Robotics, 15(5):533–549, 2001.