LocDyn: Robust Distributed Localization for Mobile Underwater Networks
Cláudia Soares João Gomes Beatriz Ferreira João Paulo Costeira

































































































LocDyn: Robust Distributed Localization for Mobile Underwater
  Networks
    Cláudia Soares, Member, IEEE,
        João Gomes, Member, IEEE,
        Beatriz Ferreira, Student Member, IEEE,
        and João Paulo Costeira
This research was partially supported by EU-H2020 WiMUST project (grant agreement No. 645141) and Fundação para a Ciência e Tecnologia (project UID/EEA/50009/2013).
    December 30, 2023
========================================================================================================================================================================================================================================================================================================================






  How to self-localize large teams of underwater nodes using only
  noisy range measurements? How to do it in a distributed way, and
  incorporating dynamics into the problem? How to reject outliers and
  produce trustworthy position estimates? The stringent acoustic
  communication channel and the accuracy needs of our geophysical
  survey application demand faster and more accurate localization
  methods. We approach dynamic localization as a MAP estimation
  problem where the prior encodes dynamics, and we devise a convex
  relaxation method that takes advantage of previous estimates at each
  measurement acquisition step; The algorithm converges at an optimal
  rate for first order methods. LocDyn is distributed: there is no
  fusion center responsible for processing acquired data and the same
  simple computations are performed for each node. LocDyn is accurate:
  experiments attest to a smaller positioning error than a comparable
  Kalman filter. LocDyn is robust: it rejects outlier noise, while the
  comparing methods succumb in terms of positioning error.




Range-based localization, Distributed localization, Autonomous underwater
vehicles, Mobile location estimation, Robust network localization.































§ INTRODUCTION




The development of networked systems of agents that can interact with
the physical world and carry out complex tasks in various contexts is
currently a major driver for research and technological development <cit.>.  This trend is also seen in contemporary ocean
applications and propelled research projects on multi-vehicle systems
like MORPH (Kalwa et al. <cit.>) and, recently, WiMUST
(Al-Khatib et al. <cit.>).

Coordinated operation of vehicles requires a communication network to
share data, most critically, those data related to navigation and
positioning, as further explored in Abreu et
al. <cit.>. Our work concerns
localization of (underwater) vehicles, a key subsystem needed in the
absence of GPS to properly georeference any acquired data and also
used in cooperative control algorithms. This paper presents research
results within the scope of EU H2020 project WiMUST, aiming at
advanced control, communication and signal processing tools to enable
a team of marine robots, either on the surface or submerged, to
jointly conduct geoacoustic surveys.

Today, geophysicists reveal sub-bottom structures using powerful sound
sources and hydrophones. During surveys, a towed source produces
acoustic waves that penetrate the sea bottom, and its layers
are inferred from the pattern of echoes observed at the towed
hydrophones, over a long period of time and a wide geographic
area. Such surveys are routinely carried out to characterize the sea
bottom prior to underwater construction, to monitor pipelines and
submerged structures, and for the operation of offshore oil and gas
fields.

As depicted in Figure <ref>, a single vessel
tows very long arrays of streamers and, thus, operation of a
traditional geophysical survey at sea means we cannot change
trajectories to recheck interesting findings; also, maneuvering
between rectilinear transects while keeping the streamers untangled
is challenging. The vision of WiMUST is to replace the monolithic
setup with a more flexible one where multiple heterogeneous underwater
vehicles tow smaller arrays while retaining a precise spatial
alignment. These are easier to maneuver, and the absence of long
physical ties between the surface ship and the data acquisition
devices enables new capabilities such as operating at variable depths
or adaptively changing the shape of the ensemble of
hydrophones.

Self-localization is a cornerstone for multi-vehicle cooperative
control in general and for WiMUST in particular, as the acoustic
signals must be georeferenced to high precision to enable an accurate
inference of deep sub-bottom layers. Our specific goal in this paper
is to accurately localize a network of moving agents from noisy
inter-vehicle ranges and from the positions of a few anchors or
landmarks.



  
*Related work

The signal processing and control communities studied the network
localization problem in many variants, like static or dynamic network
localization, centralized or distributed computations,
maximum-likelihood methods, approximation algorithms, or outlier
robust methods.  

The control community's mainstream approach to localization
relies on the robust and strong properties of the Kalman
filter to dynamically compensate noise and bias. Recent approaches can
be found in Pinheiro et al. <cit.>
and Rad et al. <cit.>. In the first very
recent paper, position and velocity are estimated from ranges,
accelerometer readings and gyroscope measurements with an Extended
Kalman filter. The authors of the second paper linearize the dynamic
network localization problem, solving it with a linear Kalman filter.
This last method is comparable with our range-only problem, although
the method requires knowledge of the noise's standard deviation.

The signal processing community traditionally studies static network
localization from a centralized perspective, like Keller and
Gur <cit.>, that formulate the problem as a regression
over adaptive bases; but the authors use squared distances, prone to
outlier noise amplification. Shang et
al. <cit.> follow a multidimensional
scaling approach, but multidimensional scaling works well only in
networks with high connectivity — a property not encountered in
practice in large-scale geometric networks. Biswas et
al. <cit.> and more recently Oğuz-Ekim et
al. <cit.> proposed semi-definite and
second order cone relaxations of the maximum likelihood
estimator. Although more precise, these convexified problems get
intractable even for a small number of nodes. Recently, we have
witnessed increasing interest from signal processing in distributed
static network localization. Papers by Shi et
al. <cit.>, Srirangarajan et
al. <cit.>, Chan and So <cit.>,
Khan et al. <cit.>, Simonetto and
Leus <cit.> and recently Soares et
al. <cit.> use different convex approximations to the
nonconvex optimization costs to devise scalable and distributed
algorithms for network localization. But for scenarios where
approximate solutions are not enough, researchers optimized the
maximum likelihood directly, obtaining solutions that depend on the
initialization of the algorithm. The methods in Calafiore et
al. <cit.> and Soares et
al. <cit.> increase the precision of a
relaxation-based solution, but are prone to local minima if wrongly
initialized. Lately, signal processing researchers produced solutions
for dynamic network localization; Schlupkothen et
al. <cit.> incorporated velocity
information from past position estimates to bias the solution of a
static localization problem via a regularization term.



  
*Our approach

In this paper we deal with the network localization problem from an
optimization-based standpoint.  We formalize the network localization
problem under the maximum a posteriori framework considering
white Gaussian noise and we tightly relax the nonconvex estimator to a
convex unconstrained program. We optimize the approximated problem
with a scalable and fast first order method, achieving smooth
trajectories for a small number of distributed iterations. We define
distributed operation as requiring no central or fusion node, and
where all nodes perform the same types of computations. Distributed
operation of vehicles requires the existence of a communication
network to share navigation and positioning data.

We propose a distributed algorithm for network localization of
underwater mobile nodes with the main properties of following a
principled maximum a posteriori approach, distributed iterations at
each agent, robustness to outlier measurements, and fast convergence.
We call our algorithm localization under dynamics, or LocDyn for
short.

While LocDyn supports distributed operation in the classic sense, it
may also be viewed in a more restricted way simply as an efficient
parallel algorithm when run at a central location that collects all
required range measurements through an appropriate forwarding protocol
(discussed, , in Ludovico et
al. <cit.>). Depending on the capacity of
the shared transmission medium the latter solution may be preferable
from a practical standpoint, but it does not impact the derivations
below.

Our approach is most closely related to one-shot network localization
methods in signal processing (i.e., starting anew when repeated over
time), but it adds a temporal dimension that enables filtering to
regularize position estimates and thus improve their
accuracy. Contrary to many Kalman-filtering-based approaches to
localization in the control literature, we assume an extremely simple
dynamic model for mobile nodes and do not rely on navigation
information that could be provided by an inertial measurement unit
(IMU). The rationale for this is that mobile nodes in our scenarios
are not necessarily AUVs; provisions could be made to install some
range-measurement and communication devices in streamers, whose
dynamics are not as well characterized as those of the towing AUVs,
and where the presence of high-quality IMUs seems far-fetched at
present.




 §.§ Contributions


We introduce LocDyn, the first optimization-based dynamic network
localization estimator that is fully distributed and has optimal
convergence rate. LocDyn tightly approximates the MAP estimator, with
the nodes' dynamics as priors, so Bayesian estimation properties
are to be expected. We use a position predictor with information from
previous position estimates via a low-pass velocity approximation.
Our method is more accurate than a Kalman filter implementation by
more than 30cm per trajectory point in all our experiments.

In our companion UCOMMS'16 paper (Ferreira et al. <cit.>) we focused on demonstrating
benefits for collaborative localization using hybrid range/bearing
measurements and time-domain filtering. We explore the same
fundamental idea for time-recursive processing here, but the method
for predicting velocities is now considerably improved, and we propose
an efficient distributed localization algorithm, whereas in
Ferreira et al. the dynamic optimization
problem was solved using a general-purpose (centralized) convex
solver. Also, the algorithm is carefully characterized and benchmarked
using numeric simulations.

The hybrid setup of Ferreira et al.
adopts the FLORIS/CLORIS least-squares framework (from a previous work
by Ferreira et al. <cit.>), which in turn relies
on a so-called disk-based relaxation presented in Soares at
al. <cit.> to attain a high-precision convex
formulation that is amenable to distributed/parallel processing. In
the present paper we consider exclusively range measurements to
streamline the technical content, but we emphasize that accommodating
bearing measurements in a hybrid localization scheme involves only
minor adaptations in the optimization problem and distributed solution
algorithm.



§ STATIC NETWORK LOCALIZATION


The network of range-measurement and communication devices (nodes),
installed on AUVs and conceivably on streamers as well, is represented
as an undirected connected graph
𝒢 = (𝒱,ℰ).  The node set
𝒱 = {1,2, …, n} denotes the agents with unknown
positions. There is an edge i ∼ j ∈ℰ between i
and j if a noisy range measurement between nodes i and j is
available at both, and if i and j can communicate with each other.
The set of landmarks with known positions[These are
  considered constant for convenience, but could also be surface-bound
  mobile devices with permanently known positions obtained through
  GPS.], named anchors, is denoted by
𝒜 = { 1, …, m }. For each i ∈𝒱, we
let 𝒜_i ⊂𝒜 be the subset of anchors (if
any) relative to which node i also possesses a noisy range
measurement.

Let ^p be the space of interest (p=2 for planar networks,
and p=3 in the volumetric scenarios of greater interest here),
x_i ∈^p the position of sensor i, and d_ij the noisy
range measurement between sensors i and j, known by both i and
j. Without loss of generality, we assume d_ij = d_ji. Anchor
positions are denoted by a_k∈^p. Similarly, r_ik
is the noisy range measurement between sensor i and anchor k,
available at sensor i. 

The distributed network localization problem addressed in this work
consists in estimating the sensors' positions x = { x_i  :   i ∈𝒱}, from the available measurements { d_ij  :   i
∼ j }∪{ r_ik  :   i ∈𝒱, k ∈𝒜_i }, through collaborative message passing between neighboring
agents in the communication graph 𝒢.

Under the assumption of zero-mean, independent and
identically-distributed, additive Gaussian measurement noise, the
maximum-likelihood estimator for the nodes' positions is the solution
of the optimization problem

    _x f(x),
 
where

    f(x) = ∑ _i ∼ j1/2(x_i - x_j - d_ij)^2 + ∑_i∑_k ∈𝒜_i1/2(x_i-a_k - r_ik)^2.
 
Problem (<ref>) is nonconvex and difficult
to solve.  Even in the centralized setting (, all measurements are
available at a central node) currently available iterative techniques
don't claim convergence to the global optimum. Also, even with
noiseless measurements, multiple solutions might exist due to
ambiguities in the network topology
itself <cit.>.

We can address this problem by optimizing a convex approximation
to (<ref>), amenable to distributed
implementation, as in Soares et al. <cit.>. The
convex approximation f̂ is tight at each term of f and can be
optimized by a first order method with optimal convergence rate. The
approximated problem is

    _xf̂(x).

The convex surrogate function f̂ is defined as

    f̂(x) = ∑_i ∼ j1/2d^2_B_ij(x_i-x_j) + ∑_i∑_k ∈𝒜_i1/2d^2_Ba_ik(x_i),

where d^2_B_ij and d^2_Ba_ik are the
squared distances to a ball B_ij = {y : y≤ d_ij}, and
a ball Ba_ik = {y : y-a_k≤ r_ik}, respectively. The
convexification strategy underlying (<ref>) is to relax
spheres in the constraint sets of squared distance functions to balls
(disks) B_ij, Ba_ik, hence the name disk-based
  relaxation. In the next section we will use function f̂ and
a modified version of problem (<ref>) to
localize underwater moving nodes.



 §.§ Assumptions




Range-only position estimation needs at least
p+1 anchors, or an equivalent set of physical constraints, to avoid
spatial ambiguities <cit.>.

Consequently, all range-only methods assume that the number
of anchors, or landmarks, is greater than the dimension of the
deployment space — 3 anchors for planar deployment and 4 for a
volumetric one.





§ MOTION-AWARE LOCALIZATION



One naive approach to localize a network of moving agents would be to
estimate the vehicles' positions solving (<ref>)
at each time step. Although it is possible, it does not take advantage
of the knowledge of previously estimated positions, so something
will be lost in processing time or communication bandwidth.

To bring motion into play we invoke the concept of prior
  knowledge in Bayesian statistics and assume a Gaussian prior on the
nodes' positions, now understood as random variables. Each position's
distribution depends on the Gaussian distribution of the noisy range
measurements, its own prior and distributions of neighboring nodes'
positions.
The prior is the predicted position

    x̃_i(k+1) = x̂_i(k) + v(k)_iΔ T,

where x̂_i(k) is the estimated position at time
step k, and v_i(k) is the measured or estimated velocity of i.
As measurements are not taken continuously, we model time in discrete
steps t = k Δ T, where t is continuous time, k is the time
step, and Δ T is the sampling period. Without loss of
generality we consider Δ T fixed.
The distance measurement between vehicles i and j at positions
x_i^⋆ and x_j^⋆ is modeled as

    d_ij = x_i^⋆ - x_j^⋆ + 𝒩(0,σ^2),

and, similarly, the range measurement between vehicle i and
anchor k is

    r_ik = x_i^⋆ - a_k + 𝒩(0,σ^2).

For each node i the prior distribution is also Gaussian, centered
on x̃_i with variance ς^2. Assuming
independency, the posterior distribution of the positions at a given
time step is, up to a normalization constant, p(x|{d}) ∝
p({d}|x)p(x). This evaluates to

    [                                         p(x|{d})                                                ∝                       ∏ _i ∼ j p(x_i-x_j - d_ij);                                                  ∏_i(∏_k ∈𝒜 p(x_i-a_k -
        r_ik) p(x_i - x̃_i)), ]

where all densities on the right-hand side are Gaussian.
We cast network localization as a maximum a posteriori
estimation problem. After applying the logarithm, we get

    _x1/σ^2 f(x) + 1/ς^2∑_ix_i
      - x̃_i^2,

equivalently written as

    _x f(x) + λx - (x̂ + v Δ T)^2,

where we multiplied by σ^2 and
adopted λ = σ^2/ς^2. Thus, the
parameter λ has a physical interpretation: it is the ratio of
the uncertainty in the measurements to the richness of the trajectory.
The concatenated vehicles'
velocities at time k, v(k), can be measured or approximated from
the previous location estimates.
As we have seen, this problem is nonconvex, so we convexify it using
the approach from section <ref> to obtain the problem

    _x g_λ(x) = f̂(x) + λx - (x̂ +
      v Δ T)^2.

We can also interpret (<ref>) as a regularized
network localization problem. The regularization parameter λ
controls how much we want to bias our estimate towards the predicted
position.

For example, if a node moves linearly as in
Figure <ref>, at k=2 our formulation will
use velocity v(1) to predict the position of the vehicle, and bias
the static localization problem towards the predicted solution.

We introduced problem (<ref>) in
Ferreira et al. <cit.> in the context
of hybrid collaborative localization based on range and bearing
measurements.



§ THE LOCDYN ALGORITHM



Problem (<ref>) has properties that allow fast
optimization: it is a sum of convex functions and, thus, convex. It is
actually strongly convex[Recall that a function g is
  m-strongly convex if and only if g(x) - m/2 x^⊤x is
  convex for all x.], meaning that at any point x the function is
lower bounded by a quadratic and thus possesses a unique minimum on
compact sets (c.f. Boyd and
Vandenberghe <cit.>).  Also, our objective
function in (<ref>) is L-smooth, meaning that there
is a quadratic upper bound to g_λ(x) for all x,
so g_λ does not grow too
fast. Appendix <ref> holds proofs and
computation of the L and m constants.
If g_λ is strongly convex and has a Lipschitz continuous
gradient, a first-order minimization algorithm can be maximally
accelerated (c.f. Nesterov <cit.>).

The gradient of g_λ is

    ∇ g_λ = ∇f̂ (x) + 2 λ (x-x̃),

where ∇f̂ is, as defined in (15) of Soares et
al. <cit.>:

    ∇f̂(x) = ℒx -A^⊤P_B(Ax) +
      [ ∑_k ∈𝒜_1x_1 - P_Ba_1k(x_1);                          ⋮; ∑_k ∈𝒜_nx_n - P_Ba_nk(x_n) ],

where A=C ⊗ I_p, C is the arc-node incidence matrix of
𝒢, I_p is the identity matrix of size p, and
B is the Cartesian product of the
balls B_ij = {y : y≤ d_ij} corresponding to
all the edges in ℰ. Similarly,
Ba_ik = {y : y-a_k≤ r_ik}. Also,
ℒ= A^⊤A = L ⊗ I_p, with L being the
Laplacian matrix of graph 𝒢.

  
Algorithm <ref> specifies LocDyn as detailed this far, with
its regularization term. As discussed previously, k indexes time
steps where we have range data and anchor positions acquisition. in
the interval, we run the algorithm, whose steps are indexed
by κ. The procedure inherits and concurs with the
distributed properties of the static method in Soares et al.<cit.>. Step <ref> computes
the extrapolated points w_i in a standard application of Nesterov's
method <cit.>.  Step <ref>
corresponds to the i-th entry of ∇f̂ and an affine term
on x_i dependent only on each node's unknown coordinates,
velocity, and the position estimated in the previous time
step. Constants c_(i ∼ j,i) denote the entry (i ∼ j,i) in
the arc-node incidence matrix C, and δ_i is the degree of
node i. The i-th entry of ℒx can be computed by
node i from its current position estimate and the position estimates
of the neighbors,
as (ℒx)_i = δ_ix_i - ∑_j ∈
  N_ix_j. As further detailed in Soares et
al. <cit.>,

    (A^⊤P_B(Ax))_i = ∑_j ∈ N_ic_(i ∼
      j,i)P_B_ij(x_i-x_j),

as presented in Step <ref>.

Next, we deal with how to approximate the velocity of a node in the
global reference frame only with data collected so far.



 §.§ Velocity estimation


To include vehicle dynamics in network localization, we penalize
discrepancies between the predicted and estimated positions. The
predicted positions are computed based on the previous estimated
position and the vehicle's velocity in world coordinates. This
velocity can be measured by the AUV, or, as we consider next, it can
be estimated from the moving pattern so far. In a previous paper
(Ferreira et al. <cit.>), inspired by
the recent work by Schlupkothen et
al. <cit.>, we estimated the velocity
of each vehicle in the global reference frame by averaging the norm
and the angle over a sliding time window. But prediction by averaging
is accurate only if the averaged quantities are nearly constant
through time — meaning linear constant motion. This is not
necessarily the case in the projected futuristic scenarios for
geoacoustic surveying; in this type of application, richer
trajectories such as the one depicted in Figure <ref>
are meant to densely cover the geographic area under study. In this
paper we estimate each vehicle's velocity v̂_i by taking
Taylor expansions of the derivative of the position.  We start by
approximating velocity by central finite differences. Unlike the
causal backward Euler difference approximation, that converges
linearly, the centered difference approximation converges
quadratically as Δ T → 0 and is defined as:

    v̂_i(k) Δ T = x_i(k+1) - x_i(k-1)/2.

Higher order approximations have even faster convergence rates, and
they are more robust to noise in the position estimates. Nevertheless,
to use them in causal estimators like LocDyn, we have to introduce a
time lag that covers the higher order time shifts. As communication in
the underwater acoustic channel has low bandwidth and slow propagation
speed, networking protocols may entail considerable latency that
invalidates the estimation of velocities with too large time shifts, so there
is a tradeoff between accuracy and opportunity in choosing the order
of approximation. A causal sixth-order approximation is

    [                    v̂_i(k) Δ T                              =    45 (x_i(k-3)-x_i(k-5))/60 +;                                - 9 (x_i(k-2) - x_i(k-6))/60 +;                                         x_i(k-1)-x_i(k-7)/60. ]

However, computing differences amplifies noise. To reduce the impact
of noise in velocity estimation, we take the derivative approximation
as an anti-symmetric FIR filter with a defined accuracy order. The
derivative approximation's transfer function can then be designed to
match the transfer function of the continuous derivative
operator. Holoborodko <cit.> proposed a method to
generate differentiator filters that also cut high frequencies,
rejecting noise both in measurements and in the estimation process,
while preserving the differentiation behavior at low frequencies. This
method was successfully used, for example, by Khong et
al. <cit.> or Hosseini and
Plataniotis <cit.>. We use the smooth low noise
differentiator fitted from a second-degree polynomial with a lag of 7
samples. Although it takes the same estimates
as (<ref>), the low-pass designed coefficients
reject high-frequency noise. The expression for the product between
estimated velocity and sampling is

    [                  v̂_i(k) Δ T                            = 5 (x_i(k-3) - x_i(k-5))/32 +;                                4 (x_i(k-2)-x_i(k-6))/32 +;                                     x_i(k-1)-x_i(k-7)/32. ]





 §.§ Convergence


The accelerated gradient method implemented in Alg. <ref>
for function g_λ with constants m and L specified
in (<ref>) and (<ref>) converges at the optimal rate
O( κ^-2) as proved by Nesterov
<cit.>,<cit.>. Also, the distance to the
unique global optimum g_λ^⋆ at iteration κ is
theoretically bounded by

    [                                      g_λ( x(κ) ) - g_λ^⋆≤; 4/(2+κ√(m/L))^2( g_λ(x(0)) -
      g_λ^⋆ + m/2 x(0) - x^⋆^2). ]






§ NUMERICAL EVALUATION




We evaluate LocDyn in trajectories used in (not necessarily
geoacoustic) surveys: a lap (Figure <ref>), descending
3D spiral (Figure <ref>), and the lawn mower
(Figure <ref>).

In all experiments, we contaminate distance measurements at each time
step k with zero-mean white Gaussian noise with standard deviation
 σ = 1m. This value for the standard deviation of
measurement noise is an upper bound on the real-world noise observed in the
WiMUST vehicles, where ranging errors are less than 1m.
We generated synthetic data according to

    d_ij = |x_i^⋆ -x_j^⋆ + 𝒩(0,σ^2)|,

for inter-sensor distances and

    r_ik = |x_i^⋆ -a_k + 𝒩(0,σ^2)|,

for sensor-anchor distances. We emphasize that measurements are in
mismatch with the data model considered in Sections <ref>
and <ref>, but the discrepancy is not serious, as the
likelihood of d_ij, r_ik being nonpositive in (<ref>) and (<ref>) is typically very small.

We benchmark LocDyn against static localization in Soares et
al. <cit.> and against the linear Kalman filter
solution proposed by Rad et al. <cit.>. We
compare LocDyn with the Kalman filter in the lap and the lawn mower
trajectories and not in the spiral, because the
implementation that the authors kindly made available to us works only
in 2D. We provided the true σ to the Kalman filter, whereas
LocDyn and static localization do not use it. All the other Kalman
filter parameters were not altered.

We ran 100 Monte Carlo trials for each experiment, and measured the
empirical error as

    Error = 1/K∑_k=1^Kx̂(k) -
      x^⋆(k) ,

where K is the total number of steps in the
trajectory, and x^⋆(k) is the concatenation of the true
positions x_i^⋆.
We initialize LocDyn and the Kalman filter in the  position
marked with a magenta cross. Static localization doesn't require
initialization. Anchors are placed to contain the trajectory on their
convex hull. In 2d, we placed 6 anchors for the lawn mower and 12 for
the lap. For 3D, we used 16 anchors.



 §.§ Lap trajectory




Lap trajectories are frequent in ocean surveying. They are also rich
since they combine linear parts with curved ones. We tested LocDyn,
the Kalman filter and static localization in the lap shown in
Figure <ref> and one of the Monte Carlo runs is
depicted in Figure <ref>. We perceive the LocDyn
trajectory as the most natural of the three. There is an intentional
slowdown of the vehicles around (-15,-5) and LocDyn is the least
affected by it. We observed these behaviors in all our visualizations
of the lap estimated trajectories.

We can see in more detail the behavior of the Kalman filter estimates
in Figure <ref>, where we displayed only the center
vehicle. It transitions well from the linear to the circular part, but
it gets lost when entering the next linear one. When the first slowdown
starts it gets lost, and the same happens again, after the second
slowdown.

But what about statistically relevant behavior? To answer this, we
simulated the lap for 100 Monte Carlo trials and computed the
empirical error of the trajectories. Figure <ref>
displays the resulting empirical cumulative distributions (CDF) for
the three algorithms. Not only is LocDyn more accurate, but it also
shows less variance in the error. The Kalman filter lags behind static
localization, although it delivers smoother trajectories. This
increase in the error is due to the bad accuracy near the slowdown
points discussed previously.





 §.§ Descending spiral trajectory



Descending spirals are useful for monitoring the water column. They
are difficult trajectories to follow so, although they are not
associated with geophysical surveying activities, we tested LocDyn in
them.

Figure <ref> shows an example run of the
descending spiral trajectory. LocDyn has the smoothest trajectory,
although both LocDyn and static localization are more deviated towards
the center of the spiral. This is due to the small number of anchors
in this setup, considering the augmented degrees of freedom in passing
from 2D to 3D. Nevertheless, including dynamic information improves
drastically the localization accuracy.

We documented the experimental accuracy increase in
Figure <ref>. Here, we see the CDF of the
empirical error from 100 Monte Carlo simulations. Noticeably, LocDyn
confirms the intuition from the example trajectory of
Figure <ref>: the average accuracy gain of
using LocDyn is of 30cm — about one third of the 1m of measurement
noise standard deviation.



 §.§ Lawn mower trajectory



In this experiment we test the robustness of the algorithms to
outlier noise. Outliers are  due, for example, to
reflection or multi-path of the acoustic wave. At each step k with
probability 1% the noisy range measurement of the vehicle to the anchor
on the SW corner is doubled.

Figure <ref> depicts one example run of the
algorithms. At the outlier-contaminated steps we see that the Kalman
filter looses the lawn mower path. Less frequently, static
localization also increases the localization error.

Figure <ref> displays only LocDyn estimates, and
evidences that the trajectory was not particularly hurt.

The empirical CDF demonstrates the impact of outlier noise in the
positioning error, and confirms that LocDyn is not only the most
accurate by far, but also with much less variance of the error.














































































































§ CONCLUSION

We explored self-localization of a network of underwater vehicles from
no more than noisy range measurements in a principled MAP estimation
framework. We produced a fast, and distributed algorithm, LocDyn,
topping in accuracy a comparable Kalman filter estimator. We showed
the advantage of encoding the dynamic behavior of moving vehicles, by
comparing LocDyn with a static network localization estimator.
Also, we gave physical meaning to LocDyn's only parameter, as a ratio
of variability in measurement noise to variability of trajectories. An
important open end of this work is to devise a way to eliminate the
parameter altogether so to have a parameter-free solution for
motion-aware network localization.































  
  

§ ACKNOWLEDGMENT




The authors would like to thank António Pascoal and Jorge Ribeiro
from DSOR-ISR for the information regarding AUV missions, and Hadi
Jamali Rad, for kindly providing code for his method.






§ L-SMOOTHNESS AND M-STRONG CONVEXITY OF G_Λ



L-smoothness implies that the gradient of g_λ is Lipschitz
continuous with Lipschitz constant L.  
A function g is Lipschitz continuous if
there exists a Lipschitz constant L such that

    g(x) -g(y) ≤ L x-y,

for all x and y. We now prove that the gradient of g_λ is
Lipschitz continuous and a Lipschitz constant L can be identified by
rCl∇g_λ(x) - ∇g_λ(y)    =    ∇f̂(x) - ∇f̂(y) + 2 λ(x-y)
    ≤    ∇f̂(x) - ∇f̂(y) + 2λx-y
 
     ≤    (L_f̂ + 2λ)_L x-y.

The inequality (<ref>) refers to the constant L_f̂
in (16) of Soares et al. <cit.>.
A strong convexity
modulus m for g_λcan be computed from

    g_λ(x) - m/2 x^⊤x  =  f̂(x) +  (
        λ - m/2 ) x^⊤x -2 λx̃^⊤x
      + λx̃^⊤x̃

noticing that the left-hand side is convex if all terms in the
right-hand side are convex in x. This entails that g_λ is
strongly convex with modulus

    m ≤ 2λ.

  As we want
robustness of g_λ to errors in x, we choose m to have
the smallest condition number L/m for
function g_λ; thus, from now on we take the largest
possible

    m = 2λ.

































IEEEtran



