Received: 26 March 2018 Revised: 18 December 2018 Accepted: 31 January 2019

DOI: 10.1002/asjc.2107

S P E C I A L I S S U E

A data-driven particle filter for terrain based navigation ofsensor-limited autonomous underwater vehicles

José Melo1 Aníbal Matos1,2

1INESC TEC - INESC Technology andScience, Porto, Portugal2FEUP - Faculty of Engineering,University of Porto, Porto, Portugal

CorrespondenceJosé Melo, INESC TEC - INESCTechnology and Science, Porto, Portugal.Email: jose.melo@fe.up.pt

Abstract

In this article a new Data-Driven formulation of the Particle Filter frameworkis proposed. The new formulation is able to learn an approximate proposaldistribution from previous data. By doing so, the need to explicitly model allthe disturbances that might affect the system is relaxed. Such characteristicsare particularly suited for Terrain Based Navigation for sensor-limited AUVs,where typical scenarios often include non-negligible sources of noise affectingthe system, which are unknown and hard to model. Numerical results are pre-sented that demonstrate the superior accuracy, robustness and efficiency of theproposed Data-Driven approach.

KEYWORDSautonomous underwater vehicles, data-driven learning, particle filter, terrain based navigation,underwater navigation

1 INTRODUCTION

Terrain Based Navigation (TBN) is a term used to refer toa class of algorithms that takes advantage of variations ofthe terrain to obtain navigation position fixes, in a pro-cess that is similar to what happens for instance with theuse of Global Navigation Satellite Systems (GNSS). In fact,information about the terrain, or bottom topography, canbe very powerful not only for the case of TBN, but also,for example, for proximity navigation relative to driftingiceberg, as suggested in [1].

Underwater TBN is a fairly recent topic, with the firstbody of work on the topic dating from the early 1990s,with the initial approaches focused on using dense sen-sors, able to map large areas of terrain within a singlemeasurement acquisition step. The experimental valida-tion of such approaches was also consistently coupledwith the use of high-grade INS. However, recently, thestudy of TBN for sensor-limited Autonomous UnderwaterVehicles (AUVs) has been reported by several authors, for

example [2,3]. Sensor-limited systems refer to a classof vehicles equipped with low-information sonar likeDoppler Velocity Log (DVL) or altimeters, but also lowaccuracy inertial measurement units (IMU).

The use of low grade IMUs motivates a tightly-coupledintegration between all the sensors, but also requires anonline estimation of critical sensor errors 4. Accuratemodelling of such errors will definitely yield better per-formance in terms of navigation accuracy of the system.However this requires a thorough understanding of theunderlying physical properties of the system, but also adetailed modelling of those sources of error, which is notalways easy or even possible to achieve. For a complete andup to date review of state of the art TBN algorithms forAUVs, the reader is referred to [5].

This article presents a novel data-driven approach tounderwater TBN for sensor-limited systems. Data-drivenmethods do not depend on an explicit and detailed modelof the environment. Instead, these methods are based onstatistical models or machine learning techniques, which

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

© 2019 Chinese Automatic Control Society and John Wiley & Sons Australia, Ltd

Asian J Control. 2019;1–12. wileyonlinelibrary.com/journal/asjc 1

2 MELO AND MATOS

try to capture trends from previously collected data. Thelearned trends can then be used to predict future states ofthe system. In this article new data-driven formulationsof the Particle Filter (PF) are proposed, that can be morerobust and efficient than traditional PF formulations whenin presence of strong non-modelled disturbances. More-over, this can be achieved without an explicit modelling ofthe drifting error sources that affect the system. A typicalapplication foreseen for the proposed approach would beof low grade AUV, without a DVL, and navigating underthe influence of currents.

The remainder of this article is organized as follows.Section 2 presents a brief overview of related work, whileSection 3 introduces the Data-Driven Particle Filter. Addi-tionally, Section 4 details on the learning process of theproposed approach. Section 5 gives some numerical resultsattesting the performance of the proposed approach, usingtwo different methods, namely using Least Squares andGaussian Processes, with Section 6 presenting some con-cluding remarks.

2 BACKGROUND AND RELATEDWORK

A state-space model for the underwater TBN problem canbe expressed by the following difference equations:

xk = 𝑓 (xk−1,uk) + wk (1a)

zk = (xk,uk) − dk + vk (1b)

Equation 1a represents the state transition equation. Thestate vector, xk, assumed to be Markovian, consists of thevehicle's two-dimensional horizontal position, referencedto a north-east-down earth-fixed frame. For sensor-limitedsystems, an augmented state vector is sometimes used, inorder to accommodate not only the position, but also thevehicle attitude and angular rates, as well as critical sen-sor errors that might need to be estimated. uk containsthe position updates as calculated from the INS, and wkrepresents the process noise.

The measurement model Equation 1b compares themeasurements from the observation vector, zk, to thebathymetric map function, , with a projection-basedscheme used to project the measured ranges into thethree-dimensional space [4]. Because the vehicle is notnavigating at the surface when acquiring the measure-ments, the depth of the vehicle, dk, is also taken intoaccount. Analogously to the motion model, measurementnoise is described by vk.

TBN is a strong non-linear problem, mostly due tothe strong nonlinearities inherent of the terrain map(xk,uk). Therefore, the interest on using non-parametric

non-linear Bayesian methods, like the Particle Filter (PF)or the Point-Mass Filter (PMF), to address this problem isobvious. While both the PF and the PMF have been suc-cessfully demonstrated to handle the problem of terrainnavigation, there has been a strong preference towards theuse of PFs, which in fact have become the primary choicefor addressing the problem of underwater TBN. Multipleauthors have adopted the Sequential Importance Resam-pling (SIR) PF to address underwater TBN, particularly forsensor-limited systems [2,6–8]. However, it is known thatin some situations the SIR-PF can fail, for example if newmeasurements appear at the tail of the prior distribution,or if the likelihood of the measurements is relatively toopeaked. One of the root causes for this is the assumptionthat the process model can be used as a suitable proposaldistribution of the PF. However, this is not always true, andthe topic of generating better proposal densities has in factreceived significant attention by researchers in the past.

The Hybrid SIR PF [9] first, and later the Unscented Par-ticle Filter (UPF) [10] where among the first algorithmsdesigned to generate better proposal densities. It hasbeen theoretically demonstrated that particle filters witha proposal distribution obtained using the UKF outper-form existing filters, however the additional computationrequirements needed are very significant. An alternativefor generating better proposal distributions, but with moremodest computational requirements, has been proposed[11]. Such an approach uses a Kalman linear smooth-ing estimator for generating the proposal distribution. Themain difference to the previous, is that only a single pro-posal is generated for all the particles, being in that sense amore efficient approach. However, the use of the Kalmansmoother precludes its use when in presence of highlynon-linear models or multimodal distributions.

In the context of TBN, the choice of a suitable pro-posal distribution has also been studied recently. In [7]the Mixture Particle Filter (MPF) and the Prior ParticleFilter (PPF) have been proposed, both based on using anon-informative uniform distribution as proposal density.Simulation results demonstrated the superiority of the PPFin terms of the asymptotic convergence of the filters. Later,a suitable compact support for the uniform distributionswas derived, using the Fisher information matrix of the ter-rain [12]. In a similar approach, in [13] a PF is also used,with subsets of the particles being weighted with differentweighting functions.

3 DATA-DRIVEN PARTICLEFILTERS

The Particle Filter is a numerical approximation to therecursive Bayes Filters that uses a weighted set of particle

MELO AND MATOS 3

to approximate the posterior density, p(xk|zk). PFs borrowideas from Monte Carlo methods, Importance sampling,and Resampling, in order to perform this approximation.While this representation is only approximate, as the num-ber of sampled particles Ns increases it is guaranteed toconverge for the true solution. Moreover, this approxima-tion can represent a much broader space of distributionsthan, for example, the Kalman Filter, that is restrictedto Gaussian distributions only [14]. Notwithstanding, thisrealization of the Bayes Filter suffers for the curse ofdimensionality, as its complexity increases exponentiallywith the dimension of the problem. For a detailed insightof the PF framework the interested reader should refer to[15] and the references therein.

3.1 SIR particle filterThe SIR-PF is, perhaps, the most widely known implemen-tation of the particle filter framework, mostly due to itssimplicity. In the SIR-PF the dynamical model p(xk|xk− 1)is used as the proposal distribution q(∶). By sampling newparticles directly from the process model, the SIR-PF over-comes the need to find an optimal proposal distribution,which is sometimes hard or even impossible to derive. Theother aspect that differentiates the SIR-PF is its adaptivestrategy for resampling. While other variations of the PFmight resample every time step, SIR-PF adopts an adap-tive strategy, using the effective number of particles, Neff,for monitoring the depletion of the particle set.

The performance of the SIR-PF is then dependent of hav-ing a process model that accurately describes the system.When this is not the case, and there are non-modelleddisturbances affecting the system, the process model dis-tribution is, in some sense, distant from the true proposaldistribution. In such situations, the SIR-PF is known tobecome very sensitive to outliers, leading to poor perfor-mance and sometimes even to divergence from the truesolution. In order to alleviate this problem, the use ofRobust Particle Filters has been proposed by some authors,for example [16]

3.2 Data-driven approachesThe main idea behind the Data-Driven PF (DD-PF) hereproposed is then to learn an approximate proposal density,by capturing trends from previously collected data. Suchdata will then be used to find a suitable proposal distribu-tion, that is somehow better than the process model. Theproposed DD-PF can be implemented following the gen-eral PF structure, according to Algorithm 1, with the onlydifference being the learning process, which must be per-formed in the beginning of each iteration, on lines 6 to 11.

The nature of this learning process will be detailed in thefollowing section.

By using previous data, the historic of previous esti-mated states, it is possible to predict a likely next stateof the filter. This information can be valuable in gener-ating a suitable proposal density. The proposal density,q(xk|xk− 1, zk), will then be approximated as

q(xk|xk−1, zk) ≈ (𝜇D,ΣD) (2)

with (𝜇D,ΣD) being the Gaussian distribution with themean and covariance matrix 𝜇D and ΣD, respectively,whose values are learnt from the data using a suitablelearning algorithm. For every iteration of the filter a newproposal density needs to be estimated.

The learning algorithm to be implemented will usethe previous w position estimates of the filter, ={x̃k−w, … , x̃k−1}, together with its respective time instants, = {k − w, … , k − 1}, to make an informed predictionof the new state of the filter at the current time step, x̂k,as well as provide an estimate of the uncertainty of suchprediction. These values, respectively 𝜇D and ΣD, will thenbe used to generate an appropriate proposal density, asindicated by Equation 2.

{𝜇D,ΣD} = ( ,) (3)

Even though the true proposal distribution q(xk|xk− 1, zk)is dependent on the observations, they are not explicitlyused in the learning process. However, the contributionof previous observations is indirectly considered throughthe posterior of the previous time-steps. Two alternativeimplementations of the DD-PF were implemented, oneusing the Least Square algorithm for the learning process(DD-PF-LS), and the other one using Gaussian Processes(DD-PF-GP). This will be detailed in the following section.These formulations were primarily designed for address-ing the problem of underwater TBN for AUVs. Therefore,this constraints the learning step to be performed online,and integrated with remaining steps of the filter.

The convergence of Particle Filters has been widelyaddressed in the literature, for example in [17]. In whatfollows some literature results that provide valuable indi-cations about the convergence of the proposed DD-PFwill be highlighted. The DD-PF differs to the standardSIR-PF only on the proposal distribution, which in thepresent case is approximated by a Gaussian distribution,with parameters learnt from past data.

The convergence properties of PFs with generic proposaldistributions has been addressed in [18], with the authorsnoting that the convergence of PFs with Gaussian proposaldistribution is ensured as long as the ratio of the optimalimportance distribution and its approximation is bounded.Moreover, the covariance matrix should also be boundedfrom below. By following a similar approach, and by noting

4 MELO AND MATOS

the similarity between the KF and the proposed learningprocedures, it should perhaps be possible to ensure theconvergence of the proposed filters DD-PF, under certainterrain conditions. Nevertheless, and following the stan-dard practices within the field of TBN, in the remainingsections we will present a set of Monte Carlo simulationthat will be able to demonstrate the performance of theproposed filters.

4 LEARNING THE PROPOSALDENSITY

For the sake of simplicity, it was chosen to decouple thelearning process in its two components, x and y, respec-tively. Recalling the assumption that AUVs have relativelyslow dynamics, characterized by smooth motions andwithout any sudden changes of velocity, this assumption israther mild. Additionally, such assumption also favours aless computationally demanding learning process, as basi-cally any correlations between the two directions are disre-garded. Therefore, two independent data-driven learningmechanisms will exist. The x will be computed as

{𝜇D,x,ΣD,xx} = ( ,x} (4)

with the y component being computed in an analogousprocess.

To generate an appropriate Gaussian approximation ofthe proposal distribution the mean and covariance matri-ces are build such that 𝜇D =

[𝜇D,x, 𝜇D,𝑦

]T and ΣD =diag

(ΣD,xx,ΣD,𝑦𝑦

). The obtained Gaussian distribution is

going to be highly correlated with the historic data usedin the learning process, particularly when using the LeastSquares, which explicitly models the motion of the vehiclewith constant velocity model. Nevertheless, this correla-tion is in fact expected, due to the assumed straight-linemotion of the vehicles, and should in fact favour a goodperformance of the filter.

4.1 Least squares regressionRecalling the aforementioned low-dynamics and con-stant velocity assumptions made above, a one-dimensionalmodel of vehicle moving with a constant velocity can bemodelled as

xi = 𝛽0 + 𝛽1ti (5)where ti, the input, corresponds to time, while xi, theoutput, corresponds to the position of the vehicle. Theparameters 𝛽0 and 𝛽1 correspond to the initial position ofthe vehicle and its velocity. By collecting a series of inputand output observations, {ti, xi}w

i=1, the parameters B̃ =[𝛽0 𝛽1]T can be determined using the closed-form expres-sion for the Ordinary Least Squares (OLS) algorithm:

B̃ = (ΦTΦ)−1ΦTY (6)

where𝛷 is the w× 2 matrix of time inputs and Y is the w× 1matrix of the output position observations. In this way, itis possible to learn the parameters 𝛽 that better fit previ-ous data. Then, a new position of the vehicle, xn, at timeinstant tn, can then by extrapolated by using the estimatedparameters, 𝛽:

xn =[

1 tn]

B̃ (7)This extrapolated value will the output of the data-drivelearning mechanism.

The OLS algorithm just presented is adequate to esti-mate static parameters. However, when the parametersare time-varying, the use of a recursive version of theOLS algorithm is more appropriate. The Recursive LeastSquares (RLS) algorithm is a recursive formulation of theleast-square problem, where new estimates of the parame-ters are updated with new input and output observations.The use of a forgetting factor 𝜆 can be interpreted as aweighting factor, giving less weight to older data and moreweight to recent data, thus promoting the estimation ofslowly varying parameters [19]. Analogously to the OLSapproach, the RLS algorithm can be implemented with thefollowing set of recursive equations:

Bk = Bk−1 + Lk−1(Yk − ΦTk Bk−1) (8)

MELO AND MATOS 5

whereLk = Pk−1Φk

(𝜆 + ΦT

k Pk−1Φk)T (9)

andPk =

(I − LkΦT

k)

Pk−11𝜆. (10)

Equations 8-10 have a structure similar to most recur-sive estimation schemes, as for example the Kalman Filter,with Equation 8 updating the parameters estimates at eachstep based on the error between the actual output and themodelled one. Pk is the usual covariance matrix, whileLk is the gain matrix. Finally, each element of 𝜇D can beobtained by replacing Bk into to (7). Additionally, the diag-onal elements of ΣD stem directly from matrix Pk of thecorresponding regression.

4.2 Gaussian processesGaussian Processes (GPs) are a non-parametric methodthat can be thought of a generalization of the Gaussianprobability distribution to infinitely many variables. GPsare fully specified by a mean function m(x) and covariancefunction k(x, x′):

𝑓 (x) ∼ (m(x), k(x, x′)) (11)

In most applications there is no prior knowledge aboutthe mean function, m(x). Because GPs are, by definition,a linear combination of random variables with normaldistribution, this is commonly assumed to be zero. Thecovariance function, k(x, x′), can be in general any functionthat takes any two arguments, such that k(x, x′) gener-ates a non-negative definitive covariance matrix K. Thecovariance function implicitly specifies certain aspects ofthe process being modelled, such as smoothness and peri-odicity, among others. One of the frequently covariancefunction is the squared exponential, defined as:

k(x, x′) = 𝜎2𝑓

exp(− 1

2l2 |x − x′|2) . (12)

It can be shown that using the squared exponential asa covariance function is equivalent to regression usinginfinitely many Gaussian shaped basis functions placedeverywhere, and not just the training points [20].

In a learning problem GPs are then used to predict theoutput y∗ given the test inputs x∗. Recalling that a GaussianProcess is a set of random variables which have a consistentGaussian distribution with mean zero, we can representsuch problem as:[

𝑦𝑦∗

]∼

(0,[

K(X ,X) + 𝜎2nI K(X ,X∗)

K(X∗,X) K(X∗,X∗)

]), (13)

where 𝜎n is the variance of independent and identicallydistributed noise affecting the observations, and the dif-ferent K matrix are built using any function k(x, x′) ableto perform as a covariance function. Therefore, and con-sidering the use of w training points, K(X,X) will be an

w × w matrix, K(X,X∗) being an 1 × w matrix and, natu-rally, K(X∗,X) is w × 1 matrix. As the number of trainingpoint w increases, computation of the different covariancematrix can become challenging.

Remembering that y and y∗ are jointly Gaussian randomvectors, then

𝑦∗|𝑦 ∼ (𝑓∗, cov(𝑓∗)) (14)where

𝑓∗ = kT∗ C−

N 1𝑦 (15)and

cov(𝑓∗) = k∗∗ − kT∗ C−1

N k∗ (16)The mean value of the prediction, 𝑓∗ in Equation 15, givesthe best estimate for y∗. Similarly, the variance cov(f∗) is anindication of the prediction's uncertainty. In the Equationsabove k∗ = K(x, x∗), CN = K(X ,X) + 𝜎2

nI and k∗∗ =K(X∗,X∗), with 𝜎n being the variance of the noisy inputs.

The choice of the hyperparameters 𝜃 can play a relevantrole in the prediction process. In the case of a covariancefunction given by (12), 𝜃 = {l, 𝜎f, 𝜎n}. While the values foreach element of 𝜃 could be made empirically, given someknowledge of the system, a more dynamic strategy couldbe adopted by maximizing the log likelihood of the train-ing outputs given the inputs. The interested reader shouldrefer to [20]. For more details on this.

Analogously to what was presented in the previous sub-section, at each iteration of the DD-PF-GP independentGPs will be used for prediction. These individual GPs willbe trained with historic data from the previous w time stepsand yield the elements of 𝜇D, by using (15). AccordinglyΣDwill be a diagonal matrix, with the elements in the diago-nal given by (16). Similarly to the DD-PF-LS, at time k thedifferent matrix K in (13) are generated using for that theoutputs of the filter from time instant k − w to k − 1. Thepredicted mean and covariance matrix of the proposal dis-tribution being estimated with then be the outputs of theGP regression, calculated using (14-16).

5 NUMERICAL RESULTS

In this section the feasibility of the proposed DD-PF is anal-ysed. The case under analysis considers a sensor-limitedAUV under the influence of unmodelled disturbances.Such disturbances can be caused by lack of proper knowl-edge of the system, for example using sensors with unmod-elled error sources, or by external sources, for examplewhen the vehicle is subject to currents that cannot be mea-sured. In what follows, only the horizontal position of thevehicle will be considered.

The simulations compare results obtained between thetwo proposed approaches, namely the DD-PF-LS and theDD-PF-GP, with two state-of-the-art filters being used for

6 MELO AND MATOS

underwater TBN, namely the SIR-PF and the PPF. TheSIR-PF is not only the most widely used implementationof the PF framework, but also it has often been mentionedin the literature concerning TBN for sensor-limited AUVs.At the same time, the PPF has recently emerged as a robustsolution for TBN problems.

The different set of simulations that will be presentedare based on AUV trajectory with a constant surge veloc-ity. For the simulated linear trajectories the heading of thevehicle was then considered to be always constant, whilefor the circular trajectories it was considered the headingto be varying at a constant rate. Such trajectories will thenbe affected by un-modelled disturbances. Simulated sonarmeasurements have been generated, at a rate of one persecond, and consisting on a four beam sonar sensor, in aJanus configuration, similar to a DVL. The ranges returnedfrom each of the beam were them corrupted with Gaussiannoise. For that purpose, a synthetically generated bathy-metric map was used. The different parameters being usedin the simulations are shown in Table 1. For reference, thenumerical results that are going to be presented are basedon simulations performed on Matlab environment, andrunning on a laptop equipped with an Intel Core i3-2350Mprocessor with 2 cores, and 4 giga byte of RAM memory.Additionally, and for the sake of simplicity the GPML Tool-box for Matlab [21] was used for implementing the GPs ofthe DD-PF-GP filter

In order to assess the convergence of the different filtersto the true solution the ensemble Root Mean Square error(RMSE) of independent runs of the filters will be evalu-ated. The time-indexed RMSE metric for a two-dimensionposition, defined as follows, will be used:

RMSEk =

√√√√ 1M

M∑r=1

(x̂k,r − xk)2 + (�̂�k,r − 𝑦k)2 (17)

where M is the number of independent MC runs of the fil-ter. In what follows, M was set to one hundred. The RMSE

TABLE 1 Filter parameters used in the simulations

Filter Settings

Number of Particles (N) 100Process Noise (𝜎w)

√5m

Measurement Noise (𝜎v) 1mResampling Threshold (Nthr) 0.6NInitial Position (𝜎po)

√5m

Sensor SettingsNumber of Beams 4Sensor Noise 0.2mLearning SettingsForgetting Factor (𝜆) 0.9Learning window (w) 10

was chosen for the sake of simplicity, but other metrics tocompare performances of different PFs could have beenused, like for example the Kullback-Leibler divergence[22].

Another feature of interest is the computational com-plexity of the filter. This is particularly relevant forsensor-limited systems, due to the limited computationalpower available on-board the vehicles. In order to com-pare the complexity of both DD-PF and SIR-PF two metricscan be used, namely the number of resampling steps per-formed, and the elapsed time for each run of the filter.These two metrics should provide an indication on therelative efficiency between each of the filters.

The number of required resampling steps is an interest-ing metric to consider, as it is known that the resamplingstage represents an important share of the total computa-tional complexity of PFs. At the same time it is the only stepof the PF that cannot be run in parallel. However, the num-ber of times a resampling algorithm is run does not provideany information on the increase in the global computationtime of the filter. Despite such values being implementa-tion dependent, they can be compared among each other,to assess the relative efficiency.

5.1 Linear trajectoriesInitial simulations were performed to evaluate the perfor-mance of the DD-PF for a vehicle moving on a linear tra-jectory, but subject to external disturbances. Consideringthe vehicle surge velocity to be of 1 m/s−1, the disturbanceswere set up to be of 0.3 m/s−1 in both X and Y directions.The trajectory performed by the vehicle can be seen inFigure 1. Additionally, these trajectories are overlaid on thecontour levels of the topography of the bottom. It can be

FIGURE 1 Simulated Linear Trajectories with the real trajectory(solid line), and the INS only trajectory (dashed) [Color figure canbe viewed at wileyonlinelibrary.com]

MELO AND MATOS 7

seen that while the INS derived trajectory is only of 250meters, the absolute error in position of the vehicle whensubject to disturbances is of roughly 100 meters. This isequivalent to a disturbance affecting the vehicle of around40% the distance travelled (DT), an already quite signif-icant disturbance, and in line with some sensor-limitedAUVs available.

Figure 2 illustrates the performance of each of the filterunder analysis. It is noticeable the similar shape betweenthe output of all the filters, highlighting the different levels

of terrain information throughout the path followed by thevehicle. The SIR-PF, the PPF and the DD-PF-LS have simi-lar performance, with their RMSE ranging below 3 meters.It should be noted that the DD-PF-LS achieved the lowestRMSE, of only 2.2 meters. At the same time, from theseplots it is also possible to infer that the position outputsof the SIR-PF are particularly more noisy and less smooththan all the others. This can be relevant if, for example, theoutput of the TBN filter is going to be used as input to amain navigation filter, as suggested by some authors [23].

FIGURE 2 Comparison of the ensemble RMSE achieved by the filters for linear trajectories [Color figure can be viewed atwileyonlinelibrary.com]

FIGURE 3 Comparison the complexity of the filters for linear trajectories [Color figure can be viewed at wileyonlinelibrary.com]

8 MELO AND MATOS

Figure 3 illustrates the complexity of the filters, by show-ing both the the number of resampling steps for eachMC run, but also their elapsed time. From Figure 3a itis possible to conclude that both the DD-PF-LS and theDD-PF-GP require less resampling steps than the remain-ing. In fact, the DD-PF-LS is the most efficient one, per-forming resampling of its particles 40% less often thanthe standard SIR-PF, and 67% less often than the PPF. Asfor the DD-PF-GP, it also performs better when comparedto the SIR-PF and the PPF. These are good indicators ofthe validity of the proposed approach. In line with theseresults, Figure 3b shows the time of batch processing eachof the MC runs, for each of the filters. From there themain conclusion is that the DD-PF-LS is the filter run-ning faster, requiring on average 17% less processing timethan the SIR-PF. As expected, the PPF performs slightlyslower than the SIR-PF, mostly due the additional numberof resampling steps. However, perhaps not so surprisingly,the DD-PF-GP is the filter that is slower. This is justifiedby the GP learning framework, which requires additionalcomputations.

5.2 Circular trajectoriesA second batch of simulations was performed, similar tothe previous one but this time considering circular trajec-tories. Besides the change of the heading of the vehicle at aconstant rate, all other conditions, including bottom topog-raphy and level of disturbances present, remained thesame. The trajectory performed by the vehicle is illustratedin Figure 4.

Similarly to before, Figure 5 presents the ensembleRMSE of all the filters being considered. In these simula-tions the SIR-PF, with an ensemble RMSE of 2.7 meters,

FIGURE 4 Simulated Circular Trajectories, with the realtrajectory (solid line), and the INS only trajectory (dashed line)[Color figure can be viewed at wileyonlinelibrary.com]

outperformed its competitor. Once again, the DD-PF-LSwas more accurate than the DD-PF-GP, in this case by asmall margin, while the PPF obtained the worst results,with an ensemble RMSE of 3.9 meters. While the SIR-PFperformed better, as it obtained a smaller RMSE, it shouldbe noted that the trajectories obtained by the DD-PF-LSare significantly smoother then all the others, which isa highly desirable characteristic as it suggests a steadiertrajectory of the vehicle.

As before, the complexity of the filters was also eval-uated, with Figure 6a comparing the number of resam-pling steps per run for this batch of simulations. Similarlyto before, the number of resampling steps required byboth the DD-PF-LS and the DD-PF-GP is significantly lessthan for the SIR-PF. By comparing these numbers hereobtained, with the ones obtained for linear trajectories,it can be noticed an increase in the number of resam-pling step for both the SIR-PF and the DD-PF-LS, withthe DD-PF-GP performing resampling of its particle onapproximately the same number of times as in the lineartrajectories. Figure 6b details the elapsed time for each ofthe filters, and it presents some similarities with Figure 3b.Here again, the DD-PF-LS is the filter achieving betterresults, with lower processing time, demonstrating thatincrease of complexity related to the data-driven mecha-nism was more than compensated by requiring a smallernumber of resampling steps.

5.3 Low level of disturbancesThe simulations presented so far address situations onwhich an AUV is following linear or circular trajectories,and subject to the effect of unmodelled disturbances atrelatively high levels, that amount to approximately 40%of the distance travelled. This was in fact the main sce-nario behind the derivation of the two presented DD-PFalgorithms. In what follows, we will study the behaviourof the aforementioned filters when the non-modelled dis-turbances are relatively smaller or even non-existent. Forsuch analysis the simulations for linear trajectories wererepeated, but now with smaller levels of disturbances.The purpose of such simulations is to study if the DD-PFalso performs well when the process model is a moreappropriate approximation of the true proposal density.Two different situations were assessed, one with distur-bances amounting to 15% of the distance travelled, andanother one without any disturbances. It should be notedthat the process noise 𝜎w was maintained in the samelevels as before. A summary of the obtained results can befound on Table 2, which shows the ensemble RMS and theaverage resampling steps of each filter, and for eachsimulated scenario. Additionally, and for an increasedstatistical significance, the standard deviation of these val-ues is also provided.

MELO AND MATOS 9

FIGURE 5 Comparison of the ensemble RMSE obtained by all the filters for circular trajectories [Color figure can be viewed atwileyonlinelibrary.com]

FIGURE 6 Comparison the complexity of the filters for circular trajectories [Color figure can be viewed at wileyonlinelibrary.com]

The obtained results are highlighted in Table 2. Fromthere, it can be concluded that the DD-PF-LS performsrelatively better than the SIR-PF an the PPF for all theforeseen scenarios, closely followed by the DD-PF-GP. Atthe same time, it is also more efficient, as the DD-PF-LSrequires less resampling steps, and is processed in signif-icantly less time. Interestingly, the SIR-PF also presents ahigher standard deviation of its RMSE value, at least twiceas much as the observed for the DD-PF-LS, which is also anindication of it's wobbly trajectory. Recalling from Figure 2,the RMSE plots for the PPF and the DD-PF-LS were muchsmoother than the ones for the SIR-PF, and a similarresult is expected here. A close look to Table 2 reveals

that the RMSE of the SIR-PF remains fairly constant whenthe disturbances are lower or even non-existent, which iscounter-intuitive. However, this reflects the fact that theprocess noise was kept in the same levels for all the sce-narios, which can be unadjusted and excessive when thedisturbances are not so high.

In order to confirm that, the simulations were repeated,but this time adjusting the process noise to values com-patible with the level of disturbances. A summary of theobtained results can be found on Table 3. As before, thetuning process for both the SIR-PF and the PPF consistedon adjusting the process noise to the lowest possible value,but while still being able to converge to the true trajec-

10 MELO AND MATOS

TABLE 2 Summary of the simulations for linear trajectories and different level of disturbances

Dist. (%DT) SIR-PF PPF DD-PF-LS DD-PF-GPRMSE Res. steps RMSE (std) Res. steps RMSE (std) Res. steps RMSE (std) Res. steps(std) (m) (std) (m) (std) (m) (std) (m) (std)

40% 2.95 (2.16) 229 (2.9) 2.39 (1.43) 299 (0.0) 2.09 (1.31) 101 (1.1) 5.16 (3.16) 159 (11.1)15% 2.62 (1.51) 233 (2.9) 1.15 (0.48) 299 (0.0) 1.05 (0.47) 99 (1.1) 1.28 (0.48) 150 (3.1)0% 2.95 (1.92) 247 (3.6) 1.03 (0.31) 299 (0.0) 1.00 (0.35) 102 (1.2) 1.23 (0.34) 147 (3.4)

TABLE 3 Summary of the simulations for linear trajectories and different levels of disturbances with adjusted process noise

Dist. (%DT) SIR-PF PPF DD-PF-LS DD-PF-GPRMSE Res. steps RMSE (std) Res. steps RMSE (std) Res. steps RMSE (std) Res. steps(std) (m) (std) (m) (std) (m) (std) (m) (std)

40% (𝜎w =√

5) 2.95 (2.16) 229 (2.9) 2.39 (1.43) 299 (0.0) 2.09 (1.31) 101 (1.1) 5.16 (3.16) 159 (11.1)15% (𝜎w =

√1) 0.57 (0.31) 25 (0.8) 0.38 (0.10) 150 (0.4) 0.78 (0.18) 296 (0.0) 0.69 (0.22) 296 (0.0)

0% (𝜎w =√

0.3) 0.90 (0.45) 65 (0.9) 4.25 (2.38) 298 (0.5) 2.71 (1.5) 296 (0.0) 2.69 (1.5) 293 (1.6)

TABLE 4 Summary of the simulations for different number of particles

Number Particles SIR-PF PPF DD-PF-LS DD-PF-GPRMSE (std) Res. steps RMSE (std) Res. steps RMSE (std) Res. steps RMSE (std) Res. steps(m) (std) (m) (std) (m) (std) (m) (std)

100 2.95 (2.16) 229 (2.9) 2.39 (1.43) 299 (0.0) 2.09 (1.31) 101 (1.1) 5.16 (3.16) 159 (11.1)200 2.81 (2.19) 233 (2.4) 2.26 (1.42) 299 (0.0) 2.00 (1.28) 104 (1.2) 2.10 (1.21) 176 (5.4)500 2.73 (2.08) 235 (2.0) 2.19 (1.34) 299 (0.0) 1.99 (1.21) 104 (1.0) 2.05 (1.49) 180 (5.5)

tory, for each particular setting of disturbance levels. Theused values are also indicated on Table 3. As for both theDD-PF-LS ans the DD-PF-GP, the value for the variance ofthe process noise remained the same as of the simulationsabove.

As before, Table 3 shows the ensemble RMS and theaverage resampling steps of each filter, and for each sim-ulated scenario, as well as the standard deviations of suchvalues. From there the main conclusions are that when thedisturbances are relatively mild or non-existent, the ben-efits of the proposed approach are less evident. It can beseen on Table 3 that with disturbances of around 15% ofthe distance traveled, and with adjusted process noise, theSIR-PF outperforms by a small margin the DD-PF-LS andthe DD-PF-GP, but is the PPF that achieves a lower RMSE.However, the SIR-PF performs resampling on significantlyless iterations than all the others, which is a good indica-tor of the lower complexity of the filters, as demonstratedin the previous subsection. As for the case when thereare no disturbances present the SIR-PF performs signifi-cantly better than all the others, achieving a lower RMSEwhile at the same time being significantly more efficient.Furthermore, comparing the RMSE of the DD-PF inTables 2 and 3 seems to indicate that the DD-PF is not asrobust to large external disturbances as its counterparts.

5.4 Number of particlesFollowing the simulations presented on the previoussubsections, a final batch of simulations was performedto study the influence on the number for particles on theprevious the presented results. To do so, the simulationsof Section 5.1 were re-done, but now using a differentnumber of particles. All the remaining parameters werenot changed. The obtained results are summarized onTable 4, which compares the results when using 100 par-ticles, presented previously, with the new results using200 and 500 particles. As before, Table 4 presents boththe ensemble RMS and the average number of resamplingsteps for each of the filters, but also the standard deviationof those values.

An increase on the number of particles seems to cause asmall decrease on the observed RMSE for both the SIR-PFand the PPF. It should be noted however that by increasingthe number of particles by a factor of five, only improvedthe RMSE by a meager 0.3 meters for the case of the SIR-PF, and 0.2 meters for the PPF. The increase on the numberof particles seems to have a negligible effect on the RMSEof the DD-PF-LS. Thus, the overall tendency is that anincrease of the number particles seems to cause a slightincrease in this number, but not very significant.

For the case of the DD-PF-GP, increasing the number ofparticles from 100 to 200, did decreased the RMSE quitesignificantly, to less than half. However, further increasingthe number of particles to 500 does not seem to yield

MELO AND MATOS 11

significantly better results. It is interesting to note thatwith an higher number of particles the DD-PF presentsan RMSE which is lower than the SIR-PF, which seems tosuggest that the DD-PF requires more particles, and thusmore computational load, in order to perform better thanthe standard PF.

6 CONCLUSIONS

In this article a new data-driven approach for the problemof underwater TBN was proposed. Differently from thestandard SIR-PF, the DD-PF here presented tries to esti-mate the proposal distribution by learning from the data.This can be particularly advantageous when some of thedisturbances present can't be modelled accurately. Twoalternative learning methods have been proposed, theDD-PF-LS and the DD-PF-GP. Using a series of numeri-cal examples the performance of the proposed filters wasassessed, and compared to both the SIR-PF and the PPF.It has been demonstrated that under the effect of unmod-elled strong disturbances the DD-PF-LS can outperformthe usual SIR-PF and the PPF, achieving lower RMSE.On top of that, the DD-PF-LS has also been demonstratedto be significantly more efficient than both those filters,requiring less resampling steps and being processed faster.Additionally, the DD-PF-LS was also demonstrated to bemore flexible, adapting to difference levels of disturbanceswith only small variatons on its performance. On the otherhand, the simulations presented show that the DD-PF-GPalways takes requires more time, mostly due to the use of acomputationally expensive GP framework. Moreover, theDD-PF-GP was unable to cope with high levels of distur-bance, even though it performed remarkably well for theother analysed scenarios.

While the simulations for the DD-PF focused only on atwo-dimensional position estimation problem, the DD-PFis still applicable for situations requiring a complete nav-igation solution, including estimation of the full pose ofthe vehicle. In such situations, the DD-PF could be used,for example, to generate position updates to a main navi-gation filter, as for example suggested in 23. Future workwould include a further study on the use of GPs as alearning methods, perhaps investigating alternative meanand covariance functions. At the same time, it wouldbe interesting to assess if the use of such nonparametricalgorithm could be more appropriate for a wider range ofapplication scenarios.

ACKNOWLEDGEMENTS

This work is financed by the ERDF - European RegionalDevelopment Fund through the Operational Programmefor Competitiveness and Internationalisation - COMPETE2020 Programme, and by National Funds through the

FCT - Fundação para a Ciência e a Tecnologia (PortugueseFoundation for Science and Technology) within project ≪POCI-01-0145-FEDER-006961 ≫.

ORCID

José Melo https://orcid.org/0000-0001-5190-9730Aníbal Matos https://orcid.org/0000-0002-9771-002X

REFERENCES1. U. Jorgensen and R. Skjetne, Online reconstruction of drifting

underwater ice topography: The 2d case, Asian J. Control 17(2015), no. 5, 1509–1521.

2. C. Morice, S. Veres, and S. McPhail, Terrain referencing forautonomous navigation of underwater vehicles, Proc. of of theMTS/IEEE Oceans'09 Conf., IEEE, Bremen, Germany, 2009, pp.1–7.

3. G. T. Donovan, Position error correction for an autonomousunderwater vehicle inertial navigation system (INS) using a parti-cle filter, IEEE J. Ocean. Eng. 37 (2012), no. 3, 431–445.

4. D. K. Meduna, S. M. Rock, and R. S. McEwen, Low-cost terrainrelative navigation for long-range AUVs, Proc. of of MTS/IEEEOceans'08 Conf., IEEE, Quebec City, Canada, 2008, pp. 1–7.

5. J. Melo and A. Matos, Survey on advances on terrain based nav-igation for autonomous underwater vehicles, Ocean Eng. 139(2017), 250–264.

6. K. B. Anonsen and O. K. Hagen, Recent developments in theHUGIN AUV terrain navigation system, Proc. of of the MTS/IEEEOceans'11 Conf., IEEE, Waikoloa, HI, USA, 2011, pp. 1–7.

7. F. C. Teixeira, A. Pascoal, and P. Maurya, A novel particle fil-ter formulation with application to terrain-aided navigation, 3rdIFAC Workshop on Navigation, Guidance and Control of Under-water Vehicles, Vol. 45, Porto, Portugal, 2012, pp. 132–139.

8. T. Zhou et al., Adaptive particle filter based on kullback-leiblerdistance for underwater terrain aided navigation with multi-beamsonar, IET Radar Sonar Navig. 12 (2018), 433–441(8).

9. J. F. G. de Freitas et al., Sequential Monte Carlo methods totrain neural network models, Neural Comput. 12 (2000), no. 4,955–993.

10. R. van der Merwe et al.: The unscented particle filter.CUED/F-INFENG/TR 380. Cambridge University EngineeringDepartment, 2000.

11. B Ju, Z Zhang, and J Zhu, A novel proposal distribution for par-ticle filter, 3rd Int. Congress on Image and Signal Processing,Vol. 7, 2010, pp. 3120–3124.

12. F. C. Teixeira et al., Robust particle filter formulations withapplication to terrain-aided navigation, Int. J. Adapt Control 31(2017), no. 4, 608–651.

13. M. Lager, E. A. Topp, and J. Malec, Underwater terrain naviga-tion during realistic scenarios, Multisensor Fusion and Integra-tion in the Wake of Big Data, Deep Learning and Cyber Physi-cal System, Springer International Publishing, Cham, 2018, pp.186–209.

14. S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics, TheMIT Press, Cambridge, MA, USA, 2005.

15. M. S. Arulampalam et al., A tutorial on particle filters for onlinenonlinear/non-Gaussian Bayesian tracking, IEEE Trans. SignalProcess. 50 (2002), no. 2, 174–188.

12 MELO AND MATOS

16. A. Murangira et al., Robust regularized particle filter for terrainnavigation, 14th International Conf. on Information Fusion,Chicago, IL, USA, 2011, pp. 1–8.

17. D. Crisan and A. Doucet, A survey of convergence results on parti-cle filtering methods for practitioners, IEEE Trans. Signal Process.50 (2002), no. 3, 736–746.

18. X. Hu, T. B. Schon, and L. Ljung, A general convergence resultfor particle filtering, IEEE Trans. Signal Process. 59 (2011), no. 7,3424–3429.

19. A. Vahidi, A. Stefanopoulou, and H. Peng, Recursive least squareswith forgetting for online estimation of vehicle mass and roadgrade: theory and experiments, Veh. Syst. Dyn. 43 (2005), no. 1,31–55.

20. C. E. Rasmussen and C. K. Williams, Gaussian processes formachine learning, The MIT Press, Cambridge, 2006.

21. C. E. Rasmussen and H. Nickisch, Gaussian processes formachine learning (gpml) toolbox, J. Mach. Learn. Res. 11 (2010),3011–3015.

22. R. Chou et al., Performance evaluation for particle filters, 14thInternational Conf. on Information Fusion, Chicago, IL, USA,2011, pp. 1–7.

23. D. K. Meduna, S. M. Rock, and R. S. McEwen, Closed-loop terrainrelative navigation for AUVs with non-inertial grade navigationsensors, IEEE/OES Autonomous Underwater Vehicles, IEEE,Monterey, CA, USA, 2010, pp. 1–8.

AUTHOR BIOGRAPHIES

José Melo is now a MCM sci-entist at the NATO Centre forMaritime Research and Exper-imentation (CMRE). Before, hewas a researcher at INESCTEC, in Porto, Portugal. In

2016 he received his PhD in Electrical and ComputerEngineering from the Faculty of Engineering, Uni-versity of Porto. In 2008 he received his M.Sc. degreein Electrical and Computer Engineering, with a

major in Automation, from the same University. Inbetween degrees, and until 2010, José has worked asa Software Engineer, at CERN. His current researchinterests are in the area of Navigation and SensorFusion for Autonomous Vehicles.

Aníbal Matos received a PhDin Electrical and ComputerEngineering form Porto Uni-versity in 2001. He is currentlycoordinator of the Centre forRobotics and Autonomous Sys-

tems at INESC TEC and also an assistant professorat the Faculty of Engineering of Porto University.His main research interests are related to percep-tion, sensing, navigation, and control of autonomousmarine robots, being the author or co-author of morethan 100 publications in international journals andconferences. He has participated and lead severalresearch projects on marine robotics and its appli-cation to monitoring, inspection, search and rescue,and defense.

How to cite this article: Melo J, Matos A.A data-driven particle filter for terrain basednavigation of sensor-limited autonomous under-water vehicles. Asian J Control. 2019;1–12.https://doi.org/10.1002/asjc.2107