Scholarly article on topic 'Cooperative bathymetry-based localization using low-cost autonomous underwater vehicles'

Cooperative bathymetry-based localization using low-cost autonomous underwater vehicles Academic research paper on "Electrical engineering, electronic engineering, information engineering"

0
0
Share paper
Keywords
{""}

Academic research paper on topic "Cooperative bathymetry-based localization using low-cost autonomous underwater vehicles"

Auton Robot

DOI 10.1007/s10514-015-9508-2

CrossMark

Cooperative bathymetry-based localization using low-cost autonomous underwater vehicles

Yew Teck Tan12 • Mandar Chitre12 • Franz S. Hover3

Received: 7 December 2014 / Accepted: 8 October 2015 © Springer Science+Business Media New York 2015

Abstract We present a cooperative bathymetry-based localization approach for a team of low-cost autonomous underwater vehicles (AUVs), each equipped only with a single-beam altimeter, a depth sensor and an acoustic modem. The localization of the individual AUV is achieved via fully decentralized particle filtering, with the local filter's measurement model driven by the AUV's altimeter measurements and ranging information obtained through inter-vehicle communication. We perform empirical analysis on the factors that affect the filter performance. Simulation studies using randomly generated trajectories as well as trajectories executed by the AUVs during field experiments successfully demonstrate the feasibility of the technique. The proposed cooperative localization technique has the potential to prolong AUV mission time, and thus open the door for long-term autonomy underwater.

Keywords Cooperative localization • Autonomous underwater vehicle • Rao-Blackwellized particle filter • Acoustic ranging

B Yew Teck Tan

tanyt81@gmail.com

Mandar Chitre mandar@arl.nus.edu.sg

Franz S. Hover hover@mit.edu

1 Acoustic Research Laboratory, Tropical Marine Science Institute, National University of Singapore, Singapore,

Singapore

Department of Electrical and Computer Engineering, National University of Singapore, Singapore, Singapore

3 Department of Mechanical Engineering, Massachusetts Institute of Technology, Cambridge, USA

1 Introduction

Bathymetry-based localization and navigation, also known as terrain relative navigation (TRN) (Meduna et al. 2010), terrain based navigation (TBN), terrain-aided navigation (TAN) (Carreno et al. 2010), and bathymetric-aided navigation (BAN) (Kalyan and Chitre 2013) has been used on autonomous underwater vehicles (AUVs) for underwater navigation. Given a bathymetric map, the idea of bathymetry-based localization is essentially to match water depth measurements with the map, in order to estimate the vehicle's position. However, for the localization to perform well, the AUVs are usually equipped with information rich sensors like the side-scan sonar or multi-beam sonar, assisted by high accuracy navigational sensors like the Doppler velocity log (DVL) and/or the inertial navigation system (INS). These hardware configurations make the AUV very expensive and not cost-effective, especially for multi-vehicle operations.

In this work, we focus on cooperative bathymetry-based localization involving a team of low-cost AUVs, equipped only with a single-beam altimeter, a depth sensor and an acoustic modem. An AUV that is capable of measuring only a single altitude measurement at every sampling time step cannot localize itself effectively within a given bathymetry map, due to the multiple occurrences of similar terrain information in the map (Nygren and Jansson 2004). However, a team of these AUVs that is also capable of estimating the inter-vehicle ranges may use this information to impose geometrical constraints on the vehicles' altitude measurements. The set of geometry constrained measurements reduces, if not eliminates, the likelihood of multiple occurrences of similar terrain information in the map and allows each of the vehicles to estimate their individual positions. This is the main idea behind the cooperative bathymetry-based localization. The localization of the individual AUV is based on

Published online: 19 October 2015

Springer

bathymetry information measured along their trajectories, complemented with the ranging information obtained by the inter-vehicle communication among the vehicles in the team. This approach is motivated by the fact that given the vehicles' estimated locations, the relative geometry among the vehicles needs to be consistent with the bathymetry information measured at those locations. In contrast to the statically-deployed underwater positioning systems, the bathymetry map acts as the source of geo-referenced position information, replacing the need for the vehicles to access a beacon signal.

Each of the vehicles in the team runs (locally) a decentralized particle filter to estimate their respective positions. The filter's process model is driven using only the AUV's control inputs and a model that predicts the AUV velocity based on the thruster control input and an onboard compass. The corresponding measurement model is updated by comparing the vehicle's water depth (altitude + depth) measurements against the bathymetry information. At every pre-scheduled period of time, the AUVs broadcast their filters' local sufficient statistics (belief) sequentially, via acoustic communication. Once received by other vehicles in the team, the information and the estimated inter-vehicle ranges are fused into their respective measurement models to influence the filter's particle distribution. Figure 1 further illustrates the concept of the cooperative localization.

The proposed decentralized filter has the potential to allow a team of low-cost AUVs to perform long-term navigation, alleviating the need to surface for a GPS fix. This is important for underwater missions over a large spatial and temporal duration (Smith et al. 2010), where the vehicles need to have a good estimate of their positions throughout the mission. Furthermore, good underwater localization is the fundamental requirement for implementing effective, long-term autonomy since the vehicles' capability for autonomous decision making relies heavily on their knowledge of the surrounding.

In Sect. 2, we discuss the related research and present our rationale in applying the proposed approach for underwater

Fig. 1 Multi-AUV cooperative localization using altimeter measurements and inter-vehicle acoustic communication

localization. In Sect. 3, we present the formulation of the multi-vehicle localization problem using a particle filtering technique, and followed by the extension of the filter's measurement model to incorporate the inter-vehicle ranging and filter information broadcast from other vehicles (Sect. 4). In Sect. 5, we present simulation results based on 3-vehicles localization, using random lawn-mowing paths generated within a bathymetry map that consists of different levels of terrain information. Using the same set of paths, we also investigate the factors that affect the performance of the proposed approach. We report on two field tests and evaluate their results in Sect. 6. In Sect. 7, we discuss the shortcomings of the proposed approach and finally, present our conclusions in Sect. 8.

2 Related work

The proposed cooperative bathymetry-based localization approach reported in this paper is informed by several related research: acoustic navigation and bathymetry-based localization.

2.1 Acoustic navigation

Underwater positioning systems using acoustic beacons have been used for AUV navigation for decades. By measuring the time-of-flight of acoustic signals transmitted from beacons deployed at known locations, an AUV is able to estimate its position with respect to these beacons. The most commonly used systems are long baseline (LBL), short baseline (SBL) and ultra short baseline (USBL) (Vickery 1998; Jakuba et al. 2008). Although these systems act as good navigational aids for AUVs, they are generally expensive and the deployment and retrieval of these positioning systems require considerable operational effort.

Recent advances in the development of AUVs and underwater communications have made inter-vehicle acoustic ranging a viable option for underwater cooperative positioning and localization. The idea of AUV cooperative positioning is to have a vehicle with good quality positioning information (beacon vehicle), to transmit its position and range information acoustically to supported AUVs within its communication range during navigation. The range information between the vehicles can then be fused with the data obtained from proprioceptive sensors to reduce the positioning error during underwater navigation (Rui and Chitre 2010; Bahr et al. 2009a). Since acoustic ranging only contains information in the direction of ranging, the performance of the approach relies on the ability of the beacon vehicle to perform ranging from different directions with respect to the supported vehicles (Song 1999; Gadre and Stilwell 2005). The observability requirement was also studied in a different

context in Arrichiello et al. (2011) where the performance of the range-based multi-vehicle localization depends on the relative velocity and position vectors of the vehicles. Our previous work (Tan and Chitre 2012; Tan et al. 2014) took into account this requirement and explored control strategies for the beacon vehicle that would drive down the uncertainties in the supported vehicles. However, the approach requires the beacon vehicle to be equipped with high accuracy navigational sensors that are able to estimate its position with minimum errors, or to operate at the surface where GPS signal is available for position estimation. Nevertheless, the ability of AUVs to perform acoustic ranging is utilized in the work to impose geometrical constraints on the consistency of the bathymetry information measured by the vehicles at their estimated locations.

2.2 Bathymetry-based localization

Bathymetry-based localization generally employs sequential Bayesian filtering to estimate the probability of a vehicle being at a particular location in the map, using process and measurement models (Meduna et al. 2010; Carreno et al. 2010; Kalyan and Chitre 2013). The measurement model can be updated using two different approaches: batch or recursive. The batch approach is based on matching all the terrain profile measurements periodically with a prior bathymetry map, while in recursive approach, the profile measurements are processed sequentially as they arrive, to estimate the vehicle's position. Typically, the type of sensor used for measuring the terrain profile determines the approach employed: single-beam echo-sounder or altimeter calls for sequential approach, while multi-beam sonar or the DVL which consists of four acoustic beams to measure velocity as well as altitude of the device, can be used in batch approach.

Since there is no closed-form solution for the posterior probability density, due to the highly non-linear bathymetric measurement model, sequential Monte Carlo filtering methods are used as an approximation of the density (Anonsen and Hallingstad 2006; Karlsson and Gustafsson 2003). In Anonsen and Hallingstad (2006), the authors applied both the Point Mass Filter (PMF) and the Particle Filter (PF) for underwater navigation using multi-beam echo-sounder. Offline filtering with field data showed that the PMF slightly outperformed the PF, though it is more computationally expensive. In Karlsson and Gustafsson (2003), the authors adopted the PF for underwater navigation and compared the estimation results to that of those computed by the Cramer Rao Lower Bound (CRLB) along the experimental trajectories to illustrate the efficiency of the filter. Although the CRLB provides a good indicator of the performance of the localization filter, it is not the focus of our paper.

Often a particle filter is designed to estimate and track a large number of system variables which requires a large

number of particles for the Alter to converge. This poses a challenge for the AUVs' limited computational power onboard. In order to alleviate this, a number of researchers have adopted an approach called the Marginalized Particle Filter (MPF), also referred to as Rao-Blackwellization (Doucet et al. 2000; Nordlund and Gustafsson 2001; Schon et al. 2005; Fairfield et al. 2006; Barkby et al. 2011; Teixeira et al. 2012a). The idea behind the MPF is to marginalize the system states that exhibit linear dynamics, and to estimate the marginalized states using a Kalman Filter. The remaining states with reduced dimension can then be estimated by the PF, thus lowering the number of particles required to produce comparable results. The MPF has been employed in Nordlund and Gustafsson (2001), in an integrated navigation system of an aircraft with a state vector of more than 15 dimensions, and simulation results showed good performance with a much lower computational load. In the domain of underwater navigation, the authors in Teixeira et al. (2012a) have shown the feasibility of applying the MPF for an AUV with the number of particles as low as 500 and manage to achieve good localization. The results have encouraged the application of MPF-based localization techniques in low-cost, limited computational-power AUVs. The work presented in this paper adopts the MPF localization technique for its advantages.

In most marine applications, the data for the vehicle's measurement model are provided by on-board multi-beam echo sounders (Nygren and Jansson 2004; Fairfield and Wettergreen 2008; Meduna et al. 2010). This enables multiple simultaneous altimeter measurement at every time step and improves the filter's performance. Furthermore, if the vehicle is fitted with a DVL, like the research reported in Donovan (2012), velocity information is available for more accurate propagation of the process model. In fact, the combination of these information rich and high accuracy navigational sensors also make underwater bathymetry simultaneous localization and mapping (SLAM) possible. For example, the research reported in Roman and Singh (2005), Barkby et al. (2011), Fairfield et al. (2006) made use of multi-beam sonar, DVL, INS and/or IMU to localize the vehicle's position while building 3-D maps along the vehicle's trajectories. However, these techniques are not suitable for a low-cost AUV, which is capable of carrying only low accuracy sensors and possibly perform dead-reckoning using its own thruster model to estimate its position. An example is shown in Nygren and Jansson (2004) where the localization filter may diverge easily due to multiple occurrences of similar terrain information within the bathymetry map, if the vehicle is assumed to have only a single-beam measurement.

In recent years, researchers have complemented bathymetry-based localization with information obtained from other sources of sensor measurements, to better estimate the position of the vehicles. This approach also has

the potential to overcome the problem that arises with bathymetry-based localization when the vehicle is over a terrain that contains insufficient information for the filter to converge. The authors in Fallon et al. (2011) fused both acoustic ranging (obtained from a surface beacon) and position information of underwater targets (obtained by side-scan sonar) to better estimate a vehicle's position and demonstrated the filter's performance via offline filtering with data collected from the field. Another related research is reported in Teixeira et al. (2012b), where the DVL measurements are fused with TAN for position estimates. Again, the reliance on these information rich and high accuracy sensors make these techniques unsuitable for localization of low-cost AUVs.

The research presented in this paper is closely related with Maurya et al. (2012) where range measurements are fused within the bathymetry-based localization filter to estimate a vehicle's position. However, our work does not consider a fixed beacon on the sea floor where an absolute positioning reference can be obtained. Instead, we employ a team of low-cost AUVs where the localization of an individual vehicle is based on the collective filters' information, fused with the range measurements derived from the communicating vehicles. Even though the cooperative localization approach does not depend on a beacon, it requires the individual filter's information to be broadcast via acoustic communication.

3 Problem formulation

In this section, we describe the formulation of the particle filtering technique used for the vehicles' localization. Each vehicle runs a copy of the filter, which adopts the same formulation presented here. Details of how the local estimate is communicated, and ranging information is integrated, are discussed in a later section.

3.1 Process and measurement models

heading and thrust) is the commanded velocity at which the AUV should move in the easting and northing direction for the time step. Zt is the process noise, modeled as an additive zero-mean Gaussian (Zt ~ N(0, a2)).

The corresponding discrete-time measurement model is

yt = h(xt ) + Ç,

where is the measurement noise, modeled as an additive zero-mean Gaussian (ft ~ N(0, a¡2)). yt represents the vehicle's water depth measurement at time t while h(xt) is the non-linear function that relates the bathymetric information at state xt to the measurement.

3.2 Marginalized particle filter

Due to the multiple occurrences of similar elevation typically present in natural terrain, there is no unique mapping from the AUV's measured water depth to its position. Consequently, the vehicle's position estimate produces multi-modal likelihood surfaces. In such a scenario, particle filtering is a popular technique used to track the multi-modal hypotheses while estimating the vehicle's position (Carreno et al. 2010). In this work, we adopt the marginalized particle filter (MPF) described in Teixeira (2007) for the vehicle's position estimation. Let N represent the number of particles used for the particle filter, xlt be the ith particle at time t. The state vector is decomposed into two parts:

xpf „kf

where xpf = [x, y]T represents the position of the vehicle estimated by Particle Filter (PF) and xkf = [cx, cy ]T represents the ocean current estimated by a Kalman Filter (KF). The resulting state-space model becomes:

The vehicle's movement is modeled as a discrete-time, linear, state model with the state dynamics driven by the vehicle's control input and ocean current estimates. Let x, y be the easting and northing position of the vehicle, and cx, cy be the ocean current in the easting and northing direction. Furthermore, let t be the time step and the elapsed time between step t and t + 1be At. The discrete-time process model used for the vehicle is described by:

xt+i = Fxt + Gut + Zt

where x = [x, y, cx, cy]T is the state vector, F and G are the state transition and control-input matrices respectively, and ut = [ux, Uyis the control input that determines the AUV's motion. The control input (derived from commanded

xpf t+i xkf Kt+i

I2x2 Fpf 02x2 Fkf

where Fpf = Gpf =

Gpf 02x2 02x2 02x2

'At 0 and Fkf = "1 0"

0 At .0 i.

. Tracking

the ocean current will improve the accuracy of the vehicle's

position propagation.

Prediction

The decomposed state vectors are propagated from time t to time t + 1 with:

pf,i xt+1

+ Fpfxff'1 + Gpfut + Zj

where Zp = ^(0, FPfPjft-1(Fpf)T + Qpf) with Qpf being the process noise intensity matrix and Pjf — denotes the covariance prediction of the ocean current estimate from time t - 1 to t.

The ocean current is estimated through the following process and measurement equations:

xkf xt+1

x;+, = Fkfxkf + Z't

Zt = Fpfxkf + Gpfut + ZÎ-

Let Zi be the Euclidean distance between the i th particle's position, xf and its immediate successor, xp+1, as predicted in (5). The innovation Zt - (Fpfxf1 + Gpfut) is computed as the difference between the distance measured, Zt and the predicted distance traveled by the vehicle due to ocean current, Fpfxjf'-1 and vehicle's control input, Gpfut. Within a KF's formulation, the ocean current state vector of each particle is propagated with:

kf,i _ xkf,i xt+1 = x t+i|t

_ Fkfxkf,i

= r At it

where the KF's update expression of the ocean current estimate and the corresponding covariance matrices are (Teixeira 2007):

kf, i kf, i

V = V-1 + Kt Vt •

Vt = Zt - (Fpfxkit-1 + Gpfut). Kt = Pt|t-1(Fpf)T [FpfPt|t-1(Fpf)T + Qpf Pt |t = (I - KtFpf)Pt |t-1. Pt+1|t = FkfPt |t (Fkf)T + Qkf.

Update

The update step consists of updating the particle's relative weight (importance) based on its observation. Let wlt be the relative weight associated with ith particle at time t, the weight of a particle is updated according to Teixeira (2007) as:

wt = w

• p(yt | x )

where p(.) is the likelihood function of the observation yt given the particles' predicted states xi and w'0 is initialized to 1/N. With the updated weights, a point estimate (PEst.) of the current state xt can be estimated through:

xfEst- ^ wi xt

while the PF's covariance is approximated by:

f =£ wi (xpfi - xpf№t) ■ (xpfi - xpf№t)T (11)

and the corresponding Kalman part of the state vector has a covariance estimated by:

Pkf Pt |t +

-kf.PEst.

Л f~kî,i -kf.PEstA ) ' (xk|/ - xk )

3.2.1 Sampling importance resampling

One of the problems with particle filters is degeneracy of particles where only a small percentage of the particles contribute to the estimation. This happens due to the fact that as the filter propagates, most of the particles will have small weights as they drift apart. One way to detect the degeneracy problem is to estimate the number of effective samples (Neff) that are currently in the particle set (Nordlund 2002). The Neff indicates how well the current particle set represents the target distribution and is computed with:

Neff =

ZN=1(wj )2

Whenever Neff is lower than the resampling threshold (N th), resampling should be performed to generate a new set of particles. In this work, we adopt the sampling threshold value in Nordlund (2002):

N th =

2 N ~ •

The resampling steps are generally referred to as the sampling importance resampling (SIR) (Karlsson and Gustafsson 2003; Teixeira 2007; Grisetti et al. 2007), and are summarized in Algorithm 1. The type of resampling method used affects the overall computational complexity of a particle filter, and is one of the important considerations because of the limited computational power that a low-cost AUV has onboard. We employed the residual resampling reported in Liu and Chen (1998) for its efficiency in terms of computation time and quality of variance reduction.

However, due to the discrete nature of the particle set, resampling over time leads to another problem called sampling impoverishment (Fearnhead 1998), where the newly generated particle set consists of only the offspring of a

small number of particles and does not reflect the true density. To reduce the effect of this, we add randomly generated Gaussian noise (with variance equal to two times the map resolution) to every sample that is chosen more than once during the resampling step.

4 Measurement model for cooperative localization

The evaluation of the likelihood function in (9) is according to the vehicle's measurement model. For the case of single vehicle localization, the measurement consists of the water depth estimate (AUV altitude measurement + AUV depth measurement) at the location of the AUV. For the case of multi-vehicle localization where acoustic communication is available among the vehicles, the measurement model also incorporates the localization information broadcast by other vehicles for the evaluation of the likelihood function.

4.1 Localization in the case of single vehicle

At every time step, the vehicle performs altitude and depth measurement to obtain the vehicle's water depth information along its trajectory. The vehicle keeps a history of the previous t (where t > 1) time step measurements and the segment of trajectory where these measurements were made. The differences in water depth within this trajectory can then be computed by subtracting each of the measurements from its previous time step's measurement. Figure 2 shows an example of the altitude measurements along a vehicle's trajectory. This approach has the advantage of eliminating the tidal offsets between the time when the bathymetric map was generated and time of mission deployment.

The weights of the particles are updated based on the likelihood function p(.) of the measurement yt given the predicted states xt at every time step. In our case, the segment

Fig. 2 Altitudes measured along the vehicle's trajectory. The differences in water depth can be calculated by subtracting each of the measurements from its previous time step's measurement

of trajectory history kept by the vehicle is appended to each of the particles. The corresponding water depth information of the appended particles is obtained from the bathymetric map, using the same measurement interval. As a result, each of the particles has an array of I measurements. An example is showninFig. 3a. The measurement model of the filter takes into account the variation between the differences in water depth measured at the particles' predicted locations (black diamonds) and the true differences in water depth measured by the vehicle along the trajectory segment (red circle). The smaller the variation, the higher the weight that is assigned to the particular particle. An example of water depth differences (after subtracted from the water depth differences measure by the vehicle) of the particles is shown in Fig. 3b. Thus, the likelihood function of the ith particle is:

P (yt I x^ = p (yt- h (xf J) (15)

where the subscript t - I : t denotes an array of the differences in water depths measured from time t - I to the current time t. In contrast to localization using a single-beam altimeter, this approach provides more than one water depth measurements along the vehicle's trajectory and reduces the probability of multiple occurrences of similar terrain information within the bathymetry map. According to the study in Nygren and Jansson (2004), the higher the number of beams used, the lower the number of likely positions. However, in contrast to the multi-beam sonar where the distances between the locations of depth measurements are known (due to the fixed sonar beams), appending individual depth measurements made along a vehicle's trajectory introduces errors in the estimated distances between measurement locations.

In general, At is influenced by the combination of three different factors: the maximum sampling rate of the sensor, the vehicle's cruising speed and the horizontal resolution of the bathymetry map used. Considering a bathymetry map with a resolution of 1 m, with an AUV cruising at 1 m/s, setting At < 1s (thus the sampling rate <1 s, assuming the echo-sounder is capable of operating at more than 1 Hz) may not be beneficial for the filter, as there is no terrain variation within the cell of 1 x 1 m. On the contrary, setting At to a large value (At > 1s) can be counterproductive as the filter could potentially miss the terrain variation along the vehicle's path.

4.2 Localization in the case of multiple vehicles

The vehicles in the team are assumed to be fitted with an acoustic modem and are capable of measuring the time-of-flight of acoustic signals. Therefore, the vehicles can estimate their range from the broadcasting vehicle using either the one-way-travel-time (OWTT) or the two-way-

Vehicle and Particles' Trajectories

o Vehicle's current position ■■--0- Vehicle's trajectory history $ Particles' currernt positions —Particles' appended trajectories HF™-

-10 -12 -14 -16 -18

y,.-«" « .*-*•

Absolute Water Depth Differences of the Particles

)1.2 1

1st particle 2nd particle 3th particle Q 4th particle

1-1-1-1-r-

th measurement

Fig. 3 a Examples of the vehicle's position (red circle) and its trajectory of length t = 14 (blue cross). The trajectory is appended to all the particles (black diamonds) forming the particles' trajectories (magenta asterisks). b The corresponding absolute water depth differences of the particles (after subtracted from the differences measured by the vehicle). In this example, the 4th particle would have the highest weight (Color figure online)

travel-time (TWTT) of the acoustic signal (Webster et al. 2012), using an assumed constant sound speed profile. In this work, we assume the system clock is synchronized across all the vehicles and adopt the OWTT in estimating the inter-vehicle range. A simple round-robin scheduling is adopted such that each vehicle, termed as peer vehicle (PV), in the team broadcasts its local state information, sequentially using acoustic communication. Round-robin scheduling for ranging among the vehicles has the advantage of eliminating the probability of collision, thus increasing the throughput of the network.

Despite advances in underwater communications, conventional methods of sharing a subset of particles (Rosencrantz et al. 2003) in the implementation of a distributed particle filter simply cannot be applied in the underwater domain due to extremely limited bandwidth and reliability. Various particle distribution aggregations have been developed as alternatives for alleviating communication limits (Jiang and Ravindran 2011; Sheng et al. 2005), but none of them have been applied in the underwater domain. To the authors

knowledge, the approach proposed in this paper is the first attempt in applying the aggregation technique in the underwater domain. Thus, the vehicle's state information broadcast includes its current position point estimate, xpV and its filter's estimated covariance matrix, PpV, as in Eqs. (10) and (11).

When the acoustic signal is received by another vehicle, termed as receiving vehicle (RV), the range, Rt, between the two vehicles can be estimated. Since the range is part of the measurements, we model its measurement error as a zero-

mean Gaussian random variable with variance aR:

where xpV and xRV are PV and RV's positions, respectively.

The range information received is commonly used directly to influence the filter's measurement model for beacon-based underwater navigation (Rui and Chitre 2010; Bahr et al. 2009a; Tan et al. 2014; Maurya et al. 2012; Webster et al. 2013). However, our approach cannot fuse the range information directly because none of the vehicles in the team is equipped with high accuracy navigational sensors, and the PV may have accumulated significant error by the time the information is broadcast. Instead, the PV's information is used to influence the RV's particle distribution and affect the corresponding likelihood computation.

Given Rt, P^ and xf^ we assume that the probability of RV's particle representing the vehicle's true position is directly proportional to the difference between the distance measured from the particle's location to x^ against Rt, taking into account the x^'s uncertainty covariance, The likelihood evaluation for each of the RV particles

(xf, i e 1

.. N) is as follows:

1. Whenever the PV's information is received via acoustic ranging, a set of N particles j = 1 ••• N) are normally distributed around xPV with covariance P^.

2. The Euclidean distances from xpf'1 to all the particles generated in step 1 are computed, resulting in N distances.

3. The likelihood of xtpf,i is evaluated by taking the sum of the differences between the N distances against the estimated range, Rt. The smaller the differences, the higher

the likelihood of xt lowing:

. For simplicity, we adopt the fol-

■ (xPf,i, R,PPV xPV) « x/

xi — m

— Rt I .

The probability computation makes use of the PV's estimated state information as well as ranging information to

(a) x 104 Peer Vehicle Broadcast its State Information 2.218 ■

Est. Error Ellipse A Est. Position

ReceivingVehicle 2 (RV2)

ReceivingVehicle 1 (RV1)

m2.212

Peer Vehicle (PV)

3.012 3.014 3.016 3.018 3.02 3.022 3.024 3.026

PV's Particles Approximated by RV 1 PV's Particles Approximated by RV 2

Fig. 4 a Local sufficient statistic information (both the estimated position and error covariance) is broadcast by the PV via acoustic communication. b N particles (red dots) approximated by RV1 using the received information. c N particles (red dots) approximated by RV2 using the received information (Color figure online)

further influence the RV particle's distribution. An example of state information approximation and sharing among the vehicles is illustrated in Fig. 4. As a result, the RV particles' likelihood evaluation consists of an extra likelihood function, fusing the information received via acoustic communication:

p(yt I xt) = p(yt:t-i-h (xpf-^ Xp(:

PV -PV x t

The detailed implementation of the proposed cooperative bathymetry-based localization is showed in Algorithm 1. The decentralized-MPF (D-MPF for simplicity) formulation allows the approach to be scaled up with the number of vehicles without increasing the computational complexity. Besides, it is also robust against lossy communication link underwater since the state information of the PV is incorporated in the measurement update step only when it is available.

5 Simualtions and results

A series of simulation studies were carried out to assess the feasibility of the proposed D-MPF, and to evaluate its performance by varying different parameters used in the filter. The

Algorithm 1 Marginalized Particle Filtering for cooperative

localization_

1: t 0 (time steps) # 1. Initialization

2: Vm = number of vehicles.

3: Draw N particles: x0'j ~ px0; i = 1 : N and j = 1 : Vm.

^ ukf. ,,i'j

4- Pkf1 pkf. wj 1

4- pni , • p0 . w0 • N .

5: Compute x^'1 according to (5) and xf j'1 according to (7)

# 2. Time update j'1 according to (7

# 3. Measurement update

6: if PV's information received then 7: Evaluate p(yt | xpf'"'1 ) according to (18) 8: else

9: Evaluate p(yt | xpf'"'1 ) according to (15) 10: end if

11: Compute the weights: w't' 1 • w't1 p(yt | xpf' '' 1 ); i = 1 : N

12: Normalize the weights: wt =

i 1 _ wt

13- x.

PEst. , j

ZN i, j i, 1 i=1 wt x

14- Let Neff =

# 4. State estimation

# 5. Resampling

if Neff < Nth then x';1 = Resample({wj} , {xj}); i = 1 : N

W 1 • N ; ' = 1 : N

end if

# 6. Iterate

19- t • t + 1

20- Goto Step # 2.

Table 1 Simulation parameters

Parameter

No. of vehicles No. of particles Filter sampling time Vehicles velocity Ranging period per vehicle Ranging scheduling

Process noise, (a^)

Altimeter measurement noise, (aj) Ranging measurement noise, (aR)

1.5 m/s

Round robin

0.05 0 0 0" 0 0.05 0 0 0 0 0.01 0 0 00 0.01

0.05 m

parameters shown in Table 1 were kept the same throughout the simulation runs, except for studies that involved varying the specified parameters. The process and measurement noises were assumed independent and drawn randomly at every propagation and measurement step, from Gaussian distribution characterized by the noise matrices. We assumed that all the vehicles have a GPS fix before submerging. Thus, each of their local filters was initialized within a circular search area with a radius of 10 m centering at the individual fixes to simulate the GPS's uncertainty.

The bathymetry map was obtained from the water near the St. John Island, Singapore in year 2012 using a Reson 8125 multi-beam echo-sounder. The equipment was operated with 240 beams at 455 kHz with a combined swathe width of 120°. The vertical resolution of the data is 0.01 m while the horizontal resolution is down-sampled to 1 m grid cells. The water depth is from a few meters to around 30 m depth.

The feasibility of bathymetry-based localization depends on the amount of information contained within a bathymetry map. Besides varying different parameters for the filter in the simulation runs, we also investigate, in general, if a given bathymetry map contains a sufficient amount of information for multi-vehicle localization. Thus, 300 different lawn-mowing paths (100 paths for each of the vehicles) were randomly generated within the map. This allows us to conduct 100 different simulated runs using those paths. The results of the simulations are shown in the form of position estimation errors of each vehicle at the end of all the simulated runs. If a high percentage of the simulated runs achieve good localization performance (low position estimation errors), we conclude that the map indeed has sufficient terrain information. Using the results from the simulation, we perform analysis on the best and worst performing cases to further investigate the influence various parameter settings have on the performance of the filter. Figure 5 shows the bathymetry map used in the simulation studies and examples of randomly generated lawn-mowing paths for the simulation studies.

5.1 Influence of communication bandwidth

Due to the limited underwater communication bandwidth, it is impractical to share all the PV's particle information with the RV during underwater cooperative localization. Nevertheless, the simulation can be used as a good benchmark to compare the performance of the decentralized filters, when it is performed offline. In this section, we undertake a simulation study whereby all the vehicles have unlimited communication bandwidth during the filter information broadcast step. As in the decentralized version, each vehicle still runs a local filter. However, instead of approximating the PV's particle distribution, as shown in Fig. 4, it is assumed that all the vehicles have access to all the other vehicles' particle sets. Thus, during the filter's measurement update step, the exact locations of PV's particles are used. This approach makes sure the filters use same process and measurement models, and provides a fair comparison that the only factor which affects the performance of the filter is the amount of information being exchanged among the vehicles in the team.

The first boxplot on the left of Fig. 6 shows the distribution of position estimation errors where the filters information was broadcast assuming unlimited communication bandwidth. By allowing full access to other filter's information during the measurement update step, the decentralized filters achieve the best performance. More importantly, the performance achievable by the D-MPF is comparable (middle boxplot in Fig. 6), even though the filter's information sharing is based on distribution aggregation. The results demonstrate the feasibility of the proposed D-MPF to be used for underwater multi-vehicle cooperative localization, where only the suffi-

Fig. 5 a Bathymetry map of St. John Island, Singapore obtained in year 2012. Terrain variation ranging from a few meters to 30 m depth. b Examples of randomly generated paths within the bathymetry map

Fig. 6 Distribution of position estimation errors for decentralized filters with (middle and right-most), and without (left-most) communca-tion bandwidth limitation (B/Limitation for simplicity)

cient statistic of the particle distribution need to be shared via the bandwidth limited acoustic communication.

The ability of a particle filter in estimating a vehicle's position depends on how accurate the position's probability distribution is represented by a set of particles. Apart from the issue of sample impoverishment mentioned in Sect. 3.2.1, the number of particles used also plays an important role. A higher number of particles may increase the accuracy, but also incurs higher computational cost, while an insufficient number of particles may result in the true density not being encompassed by the sample set. We repeated the simulations and increased the number of particles in the filter from 600 to 2000, while keeping the other parameters mentioned in Table 1. The result showed only a slight improvement (right-most boxplot in Fig. 6) over the case of 600 particles. Since the paths were randomly generated, the result suggests that on average, a set of 600 particles appeared to be sufficient for representing the particle's distribution within the bathymetry map of St. John Island, and is used for subsequent studies.

5.2 Importance of acoustic communication and bathymetry information

As mentioned in Sect. 4.2, multi-vehicle cooperative localization is achieved by incorporating both the vehicle's water depth and inter-vehicle range measurements into the D-MPF's measurement model to estimate the vehicle's position. In this section, we perform simulation studies to investigate the importance of having both pieces of information (terrain & ranging) on the filter's performance, as against having only a single piece of information: either using the terrain information (terrain-only) or the ranging information (ranging-only). The results were compared with the position errors accumulated by the dead-reckoning method, to illustrate the potential benefit, if any, of having terrain and/or ranging information (Fig. 7).

The position estimation errors of the D-MPF with both the terrain and ranging information were the same as the previous case shown in Fig. 6. However, the estimation errors increase significantly in the absence of acoustic communications among the vehicles, as shown by the terrain-only boxplot in Fig. 7. Since the filter's performance in this case depends solely on the terrain information within the area where the paths were generated, the wider spread of position errors showed there was a good mixture of areas, each containing different amount of terrain information, that was randomly selected within the bathymetry map for planning the vehicles' paths. The mixture provides a suitable scenario to illustrate the benefits of incorporating acoustic communication, as can be seen in terrain & ranging boxplot (left-most boxplot in Fig. 7). Nevertheless, the resulting terrain-only filter still outperformed the dead-reckoning method in most

Importance of Terrain Information and Acoustic Ranging

Decentralized Decentralized Decentralized Dead-Terrain & Ranging Terrain-only Ranging-only reckoning

Fig. 7 Distribution of position estimation errors of various D-MPFs against dead-reckoning. The results show the importance of having both the terrain and ranging information in the filter's performance

cases, except some outlier cases where the filter diverged due to insufficient terrain information. On the other hand, cooperative localization with ranging-only performed poorly and in most cases, worse than the dead-reckoning method. This is due to the problem of overconfidence of the filter's estimations mentioned in Webster et al. (2013), Maczka et al. (2007), Bahr et al. (2009b), where the common information (cross-correlation) among the vehicles was ignored when the ranging information is fused into the filter's measurement model. This is made worse especially when none of the vehicles have a geo-referenced position information.

5.2.1 Influence of inter-vehicle acoustic communication

In this section, we turn to the concept of information gain to illustrate the benefits of having acoustic communication in cooperative bathymetry-based localization. As described in Cover and Thomas (2006), the mutual information I(X; Y) is the reduction in the uncertainty of the random variable, X, due to the knowledge of random variable, Y. In the case of cooperative localization, this translates to the reduction of uncertainty in the vehicle's position estimate, due to the information gained from the terrain and/or ranging information. This measure can be treated as an indicator for the effectiveness of the D-MPF in estimating the vehicle's position: if the filter is effective and converges to the correct estimate, large information gain should yield lower position error.

Mutual information is defined as:

I(X; Y) = H(p(X)) - H(p(X) | p(Y)) (19)

where H(X) is the entropy of the prior distribution and H (X | Y ) is the posterior distribution. We adopt the approach

in Lanz (2007) to approximate the entropy of a probability distribution, p, using:

H (p) « ^ wi ln ^ wj K(xf | xpfj (20)

where K(xpf,J | xpf,j) is a Gaussian radial kernel approximated by:

K(xf | xpf^j) « e-2"xpf,i-xpf'j ll>2. (21)

For comparison, the best performing case of the terrain & ranging, and the terrain-only simulation results shown in Fig. 7 were used to investigate the benefits of having inter-vehicle ranging during cooperative localization. The results from the first study is shown in Fig. 8a where the simulation using terrain & ranging (circle-dashed line) was rerun without the ranging information (plus-solid line). The results from the second study are shown in Fig. 8b where the simulation using terrain-only (plus-solid line) was rerun with the addition of inter-vehicle ranging information (circle-dashed line). The large differences in the average position errors, across all levels of mutual information, clearly show that the D-MPF is more effective when the ranging information is incorporated in the measurement model.

5.2.2 Influence of bathymetry information

Without geo-referenced position information broadcast by a beacon, the vehicles rely only on inter-vehicle range measurements to estimate their positions. Over time, this causes the uncertainty of the vehicles' position to increase, due to the accumulation of the process and measurement noises. Furthermore, as shown in Tan et al. (2014), Webster et al. (2013), an acoustic signal broadcast by a PV, at position xPV, only contains ranging information in the radial direction of the ranging circle centered at xPV. Consequently, if the ranging is performed consecutively from about the same absolute bearing between the PV and RV's positions, the position uncertainty of the RV in the tangential direction will grow.

In this section, we analyze one of the cases from the ranging-only simulation results shown in Fig. 7. The simulation is repeated with the addition of bathymetry information in the filter's measurement model to investigate the benefit of fusing the extra bathymetry information for the localization of the vehicle. We compute the Estimated Error Covari-ance (ECC) of the filter's particle distribution to illustrate the difference in the filter performance. The vehicles' paths used for the simulation, as well as the ECC at different way-points are shown in Fig. 9. Ranging from about the same absolute bearings among the vehicles causes the EEC of the ranging-only case to grow at the same tangential direction.

Mutual Information vs. Ave. Position Errors

Mutual Information vs. Ave. Position Errors

Fig. 8 Mutual information against the average position estimation erros of the best performing cases. The D-MPF is more effective whenever both the ranging and bathymetry information are incorporated in the filter's measurements. a Comparison using the best performing case of terrain & ranging (circle-dashed line). The simulation was re-ran without ranging information (plus-solid line). b Comparison using the best performing case of terrain-only (plus-solid line). The simulation was re-ran with the addition of inter-vehicle ranging information (circle-dashed line)

In contrast, complementing the ranging information with bathymetry information collected along the vehicles' paths results in much smaller EEC as shown in the case of bathymetry & ranging.

Figure 10a shows the aspect ratio of the error ellipsoid described by the EEC for all the three vehicles (V1,V2 and V3), while Fig. 10b shows the trace (sum of diagonal elements) of the ECC matrix throughout the mission time. The aspect ratio denotes the skewness of the error uncertainty, while the trace denotes the magnitude of the error uncertainty. An effective filter typically has an aspect ratio as close to 1 as

Estimated Error Covariances (EEC) of X104 Ranging-only vs. Bathymetry with Ranging

V1 Start

3.005 3.01 3.015 3.02 3.025 3.03 3.035 3.04 3.045

Fig. 9 The estimated error covariance (EEC) at different way-points (blue triangles) along the vehicles' paths (Color figure online)

possible, and keeps the trace small. The results in Fig. 10a, b clearly show that by incorporating bathymetry and ranging information in the measurement model, the filter is able to achieve lower trace and keep the aspect ratio closer to 1 throughout mission time.

The position estimation errors in some ranging-only cases can be worse than the dead-reckoning method (see Fig. 7). The issue of overconfidence may results in filter divergence in the vehicles' filters, exacerbated by the wrong estimate feedback within the vehicle network, causing error reinforcement. However, when cooperative localization is aided by bathymetry information, this issue can be alleviated, if not avoided, as the individual vehicles' positions and error covariances are estimated solely from their own bathymetry measurements between acoustic communication.

5.3 Influence of simulated ocean current

Without an exteroceptive sensor, like the DVL, to measure the vehicle's ground speed, it is crucial for the filter to track the existence of an ocean current for more accurate propagation of the process model. In this section, we repeat the simulations with a southward simulated ocean current to investigate its influence on the performance of the filter. We also compare the results with the dead-reckoning method to further illustrate the benefits of employing the proposed D-MPF. The parameters shown in Table. 1 remain the same for this simulation.

Figure 11 shows the results from the simulations with different magnitudes of simulated ocean current. With ocean current being one of the states tracked by the filter, the

со DC

Ratio between the Major and Minor axes of the Estimated Error Covariances (EEC)

EEC Ranging-only -EEC Bathymetry & Ranging

200 400 600

Mission Time (second)

(b) Trace of the Estimated Error Covariances (EEC)

50 —«—«—«—«—I—«—«—«—«—I—«—«—«—«—I—«—«—«—

40 30 20 10 50 40 30 20 10 50 40 30 20 10

EEC Ranging-only -EEC Bathymetry & Ranging

200 400 600

Mission Time (second)

Fig. 10 a The ratio between the major and minor axes of the EEC throughout the mission time. b The trace of the EEC throughout the mission time. By incorporating both the bathymetry and ranging information in the measurement model, the vehicle's individual filter was able to achieve lower trace and keep the aspect ratio closer to 1 throughout mission time

Position Errors of Decentralized (De) vs Dead-reckoning (Dr) with Simulated Current

No Current 0.25 0.50 No Current 0.25 0.50 Ocean Current Magnitude (meter/second)

Fig. 11 Position estimation errors of the decentralized (De) MPF and dead-reckoning (Dr) under the influence of simulated ocean currents with different magnitude

Position Errors with Compass Bias (1 deg) and Thrust Bias (0.1 meters)

No Bias With Bias With Bias With Bias

No Current No Current 0.25 0.50

Ocean Current Magnitude (meter/second)

Fig. 12 Position estimation errors of the D-MPFs under the influence of compass bias of 1° and thrust bias of 0.1 m/s. The filter shows some level of robustness against the biases and simulated ocean current

D-MPF is able to retain its performance under the influence of a southward ocean current with a magnitude of 0.25 m/s, and degrades slightly when the magnitude was increased to 0.50 m/s. On the other hand, the position estimation errors which resulted from dead-reckoning were much higher even without the presence of ocean current, and grew substantially as the magnitude of the simulated ocean current increased. Although it is expected that dead-reckoning would perform poorly under the influence of an ocean current, the results suggest that given sufficient terrain information, in addition to the geometrical constraints imposed by the inter-vehicle acoustic ranging, the D-MPF is robust against ocean current offset.

5.4 Influence of compass and thruster biases

Often a vehicle's sensors or actuators produce readings with a constant offset (also known as bias) if left uncalibrated. Although the biases can be modeled as part of the system state and tracked by the D-MPF, it increases the dimensionality of the search space, thus requiring a higher number of particles for the filter to converge. Since we do not model any bias in the system state, it is worthwhile to investigate the robustness of the filter when there are indeed biases that exist in either the sensors or actuators of the vehicle.

In this simulation study, we repeated the simulation runs with a compass bias of 0.1°, and a thruster model bias of 0.1 m/s. The results of the simulation runs are shown in Fig. 12. As can be seen, the position estimation errors increased significantly due to the existence of the biases. Since the biases were not taken into account in the process model, they were treated as the ocean current by the filters and misled the propagation process. However, depending on

the geometry of the planned paths, if the biases affect the resultant trajectories in a way that mimics the existence of ocean current, the cooperative filter could still achieve a good tracking results. This can be observed from the cases of low position estimation errors at the lower whisker of the box-plots. The filters exhibited a position error of approximately 50 % of the cases below 26 m even under a simulated ocean current of 0.25 m/s.

6 Field experiments

In this section, we report the results obtained from field experiments using bathymetric maps from two areas with distinct terrains. The first map is from waters Charles River Basin, Boston (Fig. 13) where the terrain is flat and patchy in places. The second map is from the water near St. John Island, Singapore, as shown in Fig. 5a. In the following experiments, we collected necessary data using surface and underwater vehicles, and ran the D-MPF in post-processing to evaluate the localization performance. The parameters shown in Table 1 were used throughout all the runs, unless stated otherwise.

6.1 Charles river basin, Boston

The first set of tests were performed at the Charles River Basin, with a bathymetry map generated using depth data collected by an autonomous surface vehicle (ASV). The ASV was fitted with a Tritech-PA500 single-beam altimeter (providing one-millimeter resolution when it operates in digital mode) and instructed to perform a lawn-mower mission covering an area of about 500 x 300 m2 in the middle of the river basin. The collected depth data were post-processed to

Fig. 13 Bathymetry map of Charles River, paths executed (solidline) by the autonomous surface vehicle (inset) and the trajectories tracked (dotted-line) by the D-MPF. The vehicle was fitted with a single-beam altimeter

generate the bathymetry map (rectangular water depth map showed in Fig.13).

For the cooperative localization mission, a total of three lawnmowing-like paths were planned within the area where the bathymetry information is available. The ASV was commanded to follow these three paths using high-precision RTK GPS as a ground truth, while collecting depth data. The resultant paths are shown in Fig. 13. The oscillating patterns on the trajectories were due to the surface waves, which were not modeled in the filter. Since the control input to the filter's process model is derived from the planned paths, the depth data collected along the resultant oscillating paths allow us to test the effectiveness of the D-MPF in localizing the vehicles.

We carried out cooperative localization using the depth data as if it has been obtained by three separate vehicles. Acoustic communication was simulated between the vehicles with a ranging period of 15 s per vehicle. The range between the vehicles was computed from the vehicle's GPS ground truth during acoustic communication, corrupted with measurement noise in (16). Figure 13 shows the trajectories tracked (dotted-lines) by the D-MPF of each vehicle, while Fig. 14 shows the corresponding position errors accumulated by the filters. Throughout the mission execution, the D-MPF maintained the position errors within 20 m (under 10 m for most of the time). In comparison, the position errors are higher when there is no filter and ranging information sharing among the vehicles, and gets even higher when the vehicles depend solely on dead-reckoning for navigation.

Due to the layout of the vehicles' path, individual vehicles receives the ranging broadcasts from other vehicles at about the same absolute bearings (or 180° successively from each of the vehicles in the case of V3) throughout the mission, as shown in Fig. 15b. Such inter-vehicle ranging does not contain information in the tangential direction (with respect

Fig. 14 The average position estimation errors for all three vehicles (V1...V3) over 10 localization runs using the same paths. The position errors of the vehicles are lower when both the ranging and bathymetry information are incooperated in the D-MPF for cooperative localization

to the direction of ranging) and causes the estimated error covariances (EEC) to grow unbounded in that direction. This can be clearly seen in Fig. 15a where the EEC of the individual vehicle evolved from circular shape at beginning of the mission, t = 1 s, to elongated ellipses with their major axes almost parallel to each other, at time t = 200 s. Such cases of inter-vehicle ranging, coupled with the patchy terrain (lack of terrain information) around the mission area, resulted in poor localization performance especially around 200 s mark into the mission for V1 and 260 s mark for V2 (see Fig. 14).

6.2 St. John Island, Singapore

In a separate experiment, we performed offline cooperative localization with field data collected using a team autonomous marine vehicles that consisted of a STARFISH AUV (Koay et al. 2011) and a SCOUT ASV (Curcio et al. 2005) (Fig. 16a). The AUV was equipped with a Tritec Micron single-beam altimeter, providing 1 mm digital resolution, to measure the bathymetry information along its pre-planned path. The ASV was equipped with a BlueView P900-45 multi-beam sonar with a range resolution of 2.54 cm and a total of 256 beams. For the purpose of this experiment, only the range measurement of a single beam (128th beam) was used as the input to the localization filter.

Both the vehicles were also equipped with an acoustic modem, capable of performing OWTT measurement for acoustic ranging, and broadcast the filter information at a pre-scheduled period. The schedule was defined by a fixed, 16-s time-division multiple access (TDMA) scheduling, consisting of 8 s per acoustic broadcast, per vehicle. Whenever

Estimated Error Covariance (EEC) of the Vehicles

- Vehicle Paths A WayPoints

- - EEC at time = 1 s

- - EEC at time = 200 s

4850 4900 4950 5000 5050 5100 5150 5200 5250 meter

ro 0 ^400

Absolute Bearings between Vehicles during Ranging

a) V1 ' 1

From V2 From V3

' tp ' * ' * > te tï

From V1 From V3

From V1 From V2

It It It It It It It. It It It It It It It It It It. It It It

100 150 200

Mission Time (second)

Fig. 15 The inter-vehicle ranging at about the same absolute bearing between the vehicles cause the estimated error covariance (EEC) of the vehicles to grow at tangential direction with respect to the direction of ranging from mission time t = 1to t = 200 s. a The EEC for each of the vehicles (V1.. .V3) estimated at different waypoints (triangles) along the vehicle paths. b Absolute bearings between the vehicles (global frame) during inter-vehicle rangings

an acoustic transmission is successful, the RV incorporates the received ranging and filter information into its D-MPF's measurement update. Otherwise, the filter is updated solely by the vehicle's water depth measurements (see line 6-10 of Algorithm 1).

The experiment was conducted at St. John Island, Singapore, where we had access to the detailed bathymetry map (see Fig. 5a). During the experiment, the vehicles were deployed from a jetty and programmed to follow two lawn-moving paths, oriented approximately 90° to each other, as shown in Fig. 16b. All the tests were conducted as surface missions so that the GPS logs could be used as ground truth. We compare the performance of the filters in post-processing, so that the acoustic ranging could be selectively ignored for the case where the localization of the vehicles solely rely

Fig. 16 a The ASV and AUV used for the field experiment. Both the vehicles are equipped with an acoustic modem for acoustic ranging and filter information broadcast. The Kayak uses only a single beam of the multi-beam sonar as its altitude measurement. b The trajectories excuted by the AUV (V1) and the ASV (V2) within the bathymetry map of the St. John Island, Singapore

on the bathymetry information collected along their planned paths.

The blue-vertical arrows below each of the subfigures in Fig. 17 indicate when the vehicles successfully received the acoustic transmissions from another vehicle. Due to the close proximity of the acoustic modems to the vehicles' thruster, the acoustic transmissions were corrupted by the thruster noise whenever either of the thrusters was in between the line-of-sight of the modems. This can be seen from both the 200-300 s mark and 500-600 s mark when the ASV was heading away (north) from the AUV (Fig. 18). Furthermore, the surface mission also made the acoustic transmission unreliable towards the end of the mission when the distances between the vehicles increased. Nevertheless, we evaluate the performance of the filters using the available ranging information and their corresponding filter information broadcast.

Apart from acoustic communication, Fig. 17 also shows the position errors of the D-MPF in each of the vehicles,

^ 20 E

-20 J 60

'crt o

100 200 300 400 500 600 700 800 time (second)

Fig. 17 The estimated position errors of the vehicles throughout the mission time. The ranging time (blue vertical arrows) indicates when the vehicle sucessfully received the acoustic transmission from another vehicle (Color figure online)

Modeled velocity

10225 c 100

i00* * '

3.01 3.015

3.025 X104

Fig. 18 Timed-stamps and succeessful acoustic transmissions along the vehicles' trajectories. Inset shows the modeled velocity (computed with thruster model and compass readings) of the vehicles at the beginning of the mission

with and without incorporating the ranging information in the filters' measurement update. As expected, the position errors grow unbounded when dead-reckoning is used for the vehicles' navigation. The D-MPF that relies solely on the terrain information reduces the position error of each individual vehicle only when they navigate through regions with sufficient terrain information. However, when both the acoustic ranging and the PV's filter information are available and incorporated into the D-MPF for localization, the position errors of the vehicles are significantly lower. This can be clearly seen in Fig. 17 at around 400-500 and 600-700 s marks for V1 and at around 100-200 s mark for V2. Again, the results demonstrated the benefit of sharing the vehicles'

filter information as well as utilizing the ranging information for cooperative localization.

Even though the acoustic transmissions between the vehicles were stable at the beginning of the mission (0-100 s mark in Fig. 17), the position errors of both the vehicles still increases at about the same rate as the dead-reckoning. This is due to the prevailing ocean current in the north-west direction in that region, as shown in the insert of Fig. 18 where the modeled velocities along the vehicles's trajectories (computed using both the thruster model and the compass readings) are pointing in the south-east direction in order for the vehicles to follow the planned trajectories. The results from applying the D-MPF without ranging (blue-square line in Fig. 17) show that the bathymetry information alone around that region is insufficient for both of the vehicles to localize themselves. In a situation where there are only 2 vehicles involved in cooperative localization, the individual vehicles would either depend on their own collected terrain information for localization, or ranging information from a PV that has good position information. Since both the vehicles exhibited poor localization based on the terrain information, the acoustic ranging provided little benefit in reducing their position errors.

To further illustrate the benefit of increasing the number of vehicles to the proposed D-MPF, we reran the offline localization with an additional trajectory that was executed by another vehicle in a separate experiment, to simulate a three vehicles cooperative localization mission. The same TDMA scheduling mentioned in the previous two-vehicle case was adopted. The resultant trajectories of the vehicles are showed in Fig. 19a. The results in Fig. 19b show the estimated position errors of both the V1 and V2 are relatively low as compared to the two-vehicle case (Fig. 17). With the additional constraint provided by the ranging information of V3, the vehicles were able to achieve better performance.

It is worth pointing out that, even with the relatively featureless terrain off the St. John Island, while using only a single beam echo-sounder and inter-vehicle acoustic ranging, the D-MPF managed to keep position error at around 20~30 m for the case of two vehicles, and lower to within 20 m when the number of vehicles was increased to three.

7 Discussion

As mentioned in Sect. 5.2, when the vehicles' state information are shared among the team members, their state estimates become correlated. The cross-correlation, if not taken into account, can cause the issue of overconfidence on the position estimation and result in filter divergence. Although the proposed D-MPF does not take into account the cross covari-ance of the estimation, the issue of overconfidence is not serious enough to cause the filter to diverge, as shown in

16351336

x10 meter

3.005 3.01 3.015 3.02 3.025

o Start I

-2.215

With Ranging -Without Ranging - Dead-reckoning

100 200 300 400 500 600 700 800 time (second)

Fig. 19 With extra ranging and bahtymetry information provided by the third vehicle, the D-MPF performed better compared to the two-vehicle case showed in Fig. 17. a The trajectory of the third vehicle (V3) overlaying on top of the existing two trajectories. b The estimated position errors of the vehicles for the case of 3 vehicles

Fig. 6 where the position errors for the D-MPF remained low for most of the simulation runs. Besides, since the individual vehicles' position estimations are solely based on their own bathymetry measurements in between the times of acoustic communication, the effect of correlation has also been alleviated, even if the filters' belief broadcasts within the vehicle network are cyclic. Tracking the correlation may further improve the performance of the D-MPF. However, doing so would also increase the requirement of communication bandwidth for the vehicles to share the correlation information.

During the filter information broadcast, some information of the vehicle's particle distribution is lost because only a single mode is estimated (via Eqs. (10) and (11)) and shared with other vehicles. Even though it is impractical to transmit all the particles' location information

via the bandwidth-limited underwater communication link, it may be beneficial to estimate and transmit multiple modes of the distribution, if they exist. This will allow the RV to better approximate the PV's true particle distribution during the measurement update step, and potentially improve the filter's performance. However, the approach comes with a cost of longer packet size for the information broadcast, and may potentially decrease the transmission success rate and cause the channel throughput to be lower.

The simulation and field trial results suggest that both the ranging and bathymetry information are equally important for the proposed D-MPF to perform well. Given the bathymetry map of a mission area, together with the requirement of the inter-vehicle range observation, a multi-vehicle control strategy can be designed to take these into account so that the trajectories planned would allow all the vehicles to navigate through information-rich regions and maintain the range observability throughout a mission. The combination of the control strategy and the proposed D-MPF has the potential to be used for long-term underwater cooperative localization, without relying on an underwater positioning system.

8 Conclusion

We showed that it is feasible for a team of low-cost AUVs to perform cooperative localization. In particular, we employed the marginalized particle filtering technique in a distributed manner in each of the vehicles and extended the filter's measurement model to incorporate the information broadcast by other vehicles in the team. The decentralized formulation is scalable to as many vehicles as can operate within the communication range, without increasing the computational complexity.

We showed that both the bathymetry information and inter-vehicle acoustic communication among the vehicles are crucial for the proposed cooperative localization approach to perform well. Empirical studies using simulated data demonstrated the benefits of the decentralized filter against dead-reckoning navigation, as well as showcased its ability in estimating the vehicles' position under the influence of ocean current and sensor biases. Finally, offline localization using data from field experiments conducted in areas with different terrain variabilities also validated the effectiveness of the cooperative localization algorithm.

Acknowledgments This work was supported by Singapore-MIT Alliance for Research and Technology (SMART) graduate fellowship. The authors wish to thank the Hovergroup, WAVES lab and STARFISH team for obtaining the experimental data, and Dr. Bharath Kaylan for providing bathymetric maps.

References

Anonsen, K., & Hallingstad, O. (2006). Terrain aided underwater navigation using point mass and particle filters. In Position, location, and navigation symposium, 2006 IEEE/ION (pp. 1027-1035). doi: 10.1109/PLANS.2006.1650705.

Arrichiello, F., Antonelli, G., Aguiar, A., & Pascoal, A. (2011). Observability metric for the relative localization of auvs based on range and depth measurements: Theory and experiments. In 2011 IEEE/RSJ international conference on intelligent robots and systems (IROS) (pp. 3166-3171). doi:10.1109/IR0S.2011.6094466.

Bahr, A., Leonard, J. J., & Fallon, M. F. (2009a). Cooperative localization for autonomous underwater vehicles. The International Journal of Robotics Research, 28(6), 714-728. doi:10.1177/ 0278364908100561.

Bahr, A., Walter, M., & Leonard, J. (2009b). Consistent cooperative localization. In IEEE international conference on robotics and automation (ICRA), Kobe, Japan. doi:10.1109/R0B0T.2009. 5152859.

Barkby, S., Williams, S. B., Pizarro, O., & Jakuba, M. V. (2011). A featureless approach to efficient bathymetric slam using distributed particle mapping. Journal of Field Robotics, 28(1), 19-39.

Carreno, S., Wilson, P., Ridao, P., & Petillot, Y. (2010). A survey on terrain based navigation for AUVs. In OCEANS 2010 (pp. 1-7). doi:10.1109/0CEANS.2010.5664372.

Cover, T. M., & Thomas, J. A. (2006). Wiley series in telecommunications and signal processing. Elements of information theory (2nd ed.). New York: Wiley.

Curcio, J., Leonard, J., & Patrikalakis, A. (2005). Scout: a low cost autonomous surface platform for research in cooperative autonomy. In OCEANS, 2005. Proceedings of MTS/IEEE (Vol. 1, pp. 725-729). doi:10.1109/0CEANS.2005.1639838.

Donovan, G. (2012). Position error correction for an autonomous underwater vehicle inertial navigation system (INS) using a particle filter. IEEE Journal of Oceanic Engineering, 37(3), 431-445. doi:10.1109/J0E.2012.2190810.

Doucet, A., Freitas, Nd., Murphy, K. P., & Russell, S. J. (2000). Rao-blackwellised particle filtering for dynamic bayesian networks. In Proceedings of the 16th conference on uncertainty in artificial intelligence, UAI'00 (pp. 176-183). San Francisco, CA: Morgan Kaufmann Publishers Inc.

Fairfield, N., & Wettergreen, D. (2008). Active localization on the ocean floor with multibeam sonar. In OCEANS 2008 (pp. 1-10). doi:10. 1109/0CEANS.2008.5151853.

Fairfield, N., Kantor, G. A., & Wettergreen, D. (2006). Towards particle filter SLAM with three dimensional evidence grids in a flooded subterranean environment. In Proceedings of ICRA 2006 (pp. 3575-3580).

Fallon, M. F., Kaess, M., Johannsson, H., & Leonard J. J. (2011). Efficient AUV navigation fusing acoustic ranging and side-scan sonar. In IEEE international conference on robotics and automation (ICRA), Shanghai.

Fearnhead, P. (1998). Sequential monte carlo methods in filter theory, University of 0xford. PhD thesis.

Gadre, A., & Stilwell, D. (2005). A complete solution to underwater navigation in the presence of unknown currents based on range measurements from a single location. In IEEE/RSJ International Conference on Intelligent Robots and Systems, 2005. (IROS 2005) (pp. 1420-1425). doi:10.1109/IR0S.2005.1545230.

Grisetti, G., Stachniss, C., & Burgard, W. (2007). Improved techniques for grid mapping with Rao-Blackwellized particle filters. IEEE Transactions on Robotics, 23(1), 34-46. doi:10.1109/TR0.2006. 889486.

Jakuba, M. V., Roman, C. N., Singh, H., Murphy, C., Kunz, C., Willis, C., et al. (2008). Long-baseline acoustic navigation for under-

ice autonomous underwater vehicle operations. Journal of Field Robotics, 25(11-12), 861-879. doi:10.1002/rob.20250.

Jiang, B., & Ravindran, B. (2011). Completely distributed particle filters for target tracking in sensor networks. In Parallel Distributed Processing Symposium (IPDPS), 2011 IEEE International (pp. 334-344). doi:10.1109/IPDPS.2011.40.

Kalyan, B., & Chitre, M. (2013). A feasibility analysis on using bathymetry for navigation of autonomous underwater vehicles. In Proceedings of the 28th Annual ACM Symposium on Applied Computing, ACM, New York, NY, SAC'13 (pp. 229-231). doi:10.1145/ 2480362.2480411.

Karlsson, R., & Gustafsson, F. (2003). Particle filter for underwater terrain navigation. In IEEE Workshop on Statistical Signal Processing (pp. 526-529). doi:10.1109/SSP.2003.1289507.

Koay, T. B., Tan, Y. T., Eng, Y. H., Gao, R., Chitre, M., Chew, J. L., et al. (2011). STARFISH-a small team of autonomous robotic fish. Indian Journal of Geo-Marine Sciences, 20(2), 157-167.

Lanz, 0. (2007). An information theoretic rule for sample size adaptation in particle filtering. In International conference on image analysis and processing (pp. 317-322).

Liu, J. S., & Chen, R. (1998). Sequential Monte Carlo methods for dynamic systems. Journal ofthe American Statistical Association, 93, 1032-1044.

Maczka, D., Gadre, A., & Stilwell, D. (2007). Implementation of a cooperative navigation algorithm on a platoon of autonomous underwater vehicles. In OCEANS 2007 (pp. 1-6). doi:10.1109/ 0CEANS.2007.4449404.

Maurya, P., Teixeira, F. C., & Pascoal, A. (2012). Complementary terrain/single beacon-based AUV navigation. In Proceedings of the IFAC workshop on navigation, guidance and control of underwater vehicles (NGCUV'2012), Porto (pp. 10-12).

Meduna, D., Rock, S., & McEwen, R. (2010). Closed-loop terrain relative navigation for AUVs with non-inertial grade navigation sensors. In Autonomous underwater vehicles (AUV), 2010 IEEE/OES (pp. 1-8). doi: 10.1109/AUV.2010.5779659.

Nordlund, P. J. (2002). Sequential Monte Carlo filters and integrated navigation, Linkopings universitet. PhD thesis.

Nordlund, P. J., & Gustafsson, F. (2001). Sequential Monte Carlo filtering techniques applied to integrated navigation systems. In Proceedings of the American Control Conference (Vol. 6, pp. 4375-4380). doi: 10.1109/ACC.2001.945666.

Nygren, I., & Jansson, M. (2004). Terrain navigation for underwater vehicles using the correlator method. IEEE Journal of Oceanic Engineering, 29(3), 906-915. doi:10.1109/J0E.2004.833222.

Roman, C., & Singh, H. (2005). Improved vehicle based multibeam bathymetry using sub-maps and SLAM. In IEEE/RSJ International conference on intelligent robots and systems, (IR0S 2005) (pp. 3662-3669).

Rosencrantz, M., Gordon, G., & Thrun, S. (2003). Decentralized sensor fusion with distributed particle filters. In Proceedings of the nineteenth conference on uncertainty in artificial intelligence, UAI'03 (pp. 493-500) San Francisco, CA: Morgan Kaufmann Publishers Inc.

Rui, G., & Chitre, M. (2010). Cooperative positioning using range-only measurements between two AUVs. In OCEANS 2010 IEEE-Sydney (pp. 1-6). doi:10.1109/0CEANSSYD.2010.5603615.

Schon, T., Gustafsson, F., & Nordlund, P. J. (2005). Marginalized particle filters for mixed linear/nonlinear state-space models. IEEE Transactions on Signal Processing, 53(7), 2279-2289. doi:10. 1109/TSP.2005.849151.

Sheng, X., Hu, Y. H., & Ramanathan, P. (2005). Distributed particle filter with GMM approximation for multiple targets localization and tracking in wireless sensor network. In Fourth international symposium on information processing in sensor networks, 2005. IPSN2005 (pp. 181-188). doi:10.1109/IPSN.2005.1440923.

Smith, R. N., Chao, Y., Li, P. P., Caron, D. A., Jones, B. H., & Sukhatme, G. S. (2010). Planning and implementing trajectories for autonomous underwater vehicles to track evolving ocean processes based on predictions from a regional ocean model. The International Journal of Robotics Research, 29(12), 1475-1497. doi: 10.1177/0278364910377243.

Song, T. L. (1999). Observability of target tracking with range-only measurements. IEEE Journal of Oceanic Engineering, 24(3), 383387. doi:10.1109/48.775299.

Tan, YT., Chitre, M. (2012). Direct policy search with variable-length genetic algorithm for single beacon cooperative path planning. In International symposium on distributed autonomous robotic systems (DARS). Baltimore.

Tan, Y. T., Gao, R., & Chitre, M. (2014). Cooperative path planning for range-only localization using a single moving beacon. IEEE Journal of Oceanic Engineering, 39(2), 371-385. doi:10.1109/ JOE.2013.2296361.

Teixeira, F. C. (2007). Terrain-aided navigation and geophysical navigation of autonomous underwater vehicles. PhD thesis, Dynamical systems and ocean robotics lab, Lisbon.

Teixeira, F. C., Pascoal, A., & Maurya, P. (2012a). A novel particle filter formulation with application to terrain-aided navigation. In Proceedings of IFAC workshop on navigation, guidance and control of underwater vehicles (NGCUV'2012) (pp 10-12). Porto.

Teixeira, F. C., Quintas, J., & Pascoal, A. (2012b). AUV terrain-aided doppler navigation using complementary filtering. In Proceedings of IFAC conference on manoeuvring and control of marine craft (MCMC'2012). Arenzano.

Vickery, K. (1998). Acoustic positioning systems. a practical overview of current systems. In Proceedings of the 1998 workshop on autonomous underwater vehicles, 1998. AUV'98 (pp. 5-17). doi: 10.1109/AUV. 1998.744434.

Webster, S., Walls, J., Whitcomb, L., & Eustice, R. (2013). Decentralized extended information filter for single-beacon cooperative acoustic navigation: Theory and experiments. IEEE Transactions on Robotics, 29(4), 957-974. doi:10.1109/TRO.2013.2252857.

Webster, S. E., Eustice, R. M., Singh, H., & Whitcomb, L. L. (2012). Advances in single-beacon one-way-travel-time acoustic navigation for underwater vehicles. The International Journal of Robotics Research, 31(8), 935-950. doi:10.1177/0278364912446166.

YewTeckTan received the B.S. degree in computer science from the University Tunku Abdul Rahman (UTAR), Perak, Malaysia, developing path planner for mobile robotic systems, and the M.Eng. degree from the National University of Singapore (NUS), Singapore, where he is currently working toward the Ph.D. degree under the Singapore/MIT Alliance for Research and Technology (SMART) fellowship. Currently, he is involved in developing a command and control system and cooperative algorithms for a team of autonomous underwater vehicles at NUS. His research interests include intelligent and autonomous systems, robotics, multiagent multirobot systems, and biologically inspired computing.

Mandar Chitre received B.Eng. and M.Eng. degrees in electrical engineering from the National University of Singapore (NUS), Singapore, a M.Sc. degree in bioinformatics from the Nanyang Technological University (NTU), Singapore, and a Ph.D. degree from NUS. From 1997 to 1998, he worked with the ARL, NUS in Singapore. From 1998 to 2002, he headed the technology division of a regional telecommunications solutions company. In 2003, he rejoined ARL, initially as the Deputy Head (Research) and is now the Head of the laboratory. Dr. Chitre also holds a joint appointment with the Department of Electrical & Computer Engineering at NUS as an Assistant Professor. His current research interests are underwater communications, autonomous underwater vehicles, model-based signal processing and modeling of complex dynamic systems. Dr. Chitre has served on the technical program committees of the IEEE OCEANS, WUWNet, DTA and OTC conferences and has served as reviewer for numerous international journals. He was the chairman of the student poster committee for IEEE OCEANS'06 in Singapore, and the chairman for the IEEE Singapore AUV Challenge 2013. He is currently the IEEE Ocean Engineering Society technology committee co-chair of underwater communication, navigation & positioning.

Franz S. Hover (M'09) received the B.S.M.E. degree from Ohio Northern University and the M.S. and Sc.D. degrees from the Joint Program in Applied Ocean Physics and Engineering at the Woods Hole Oceano-graphic Institution and the Massachusetts Institute of Technology (MIT), Cambridge. He was a consultant to industry and then a member of the research staff at MIT, where he worked in fluid mechanics, biomimetics, and marine robotics. He is currently Finmeccanica Associate Professor at the MIT Department of Mechanical Engineering, with research focusing on design methodologies and complex marine systems.