F. Arrichiello and G. Antonelli and A.P. Aguiar and A. Pascoal, Observability metrics for the relative localization of AUVs based on range and depth measurements: theory and experiments, 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, San Franscisco, CA, pp. 3166--3171, 2011.

1. Observability metric for the relative
localization of AUVs based on range and
depth measurements: theory and
experiments
Filippo Arrichiello1 , Gianluca Antonelli1
Antonio Pedro Aguiar2 , Antonio Pascoal2
1.University of Cassino, Italy
Robotics Research Group of the DAEIMI
http://webuser.unicas.it/lai/robotica
2.Technical University of Lisbon, Portugal
Lab. of Robotics and Systems in Engineering and Science
http://welcome.isr.ist.utl.pt
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

2. Underwater localization
Localization of an Autonomous Underwater Vehicle (AUV)
◮ GPS not working under the water
◮ Dead-reckoning (IMU, DVL)
◮ drift
◮ External array of acoustic baseline (LBL, SBL, USBL)
◮ expensive
◮ limited coverage area
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

3. Single beacon localization
Analysis of the relative localization using a single beacon
(transducer/transponder couple) and on board sensors
GPS
d η2,z
◮ Acoustic communication: range measurement and sensor data
exchange
◮ On-board sensors information: depth and velocity
◮ We study the observability of the system and define a metrics
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

4. Modeling and objective
ΣI
xv,1
xv,2
Σv,1 x
Σv,2
◮ Model: 
˙
x = v
1 T
y = 2x x
x3

◮ Objective: We want to estimate the relative positioning x of vehicle
2 with respect to vehicle 1 from the output y ∈ IR2 , i.e., from
distance and depth difference
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

5. Observability analysis
Starting from the generic non-linear model
˙
x= f(x, u)
y= h(x)
we discuss the local weak observability of system
Reference:
R. Hermann and A. Krener. Nonlinear controllability and observability.
IEEE Transactions on Automatic Control, 22(5):728–740, 1977
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

6. Local weak observability
By defining the Lie derivatives of the scalar output hj as

L0 hj = hj
 f
L hj = ▽hj · f = ∂hj · f = 3 ∂hj · fi

 1
∂x i =1 ∂xi
 f

L2 hj = ∂∂ L1 hj · f
x f
 f
· · ·



L h = ∂ Ln−1 h · f
 n
f j ∂x f j
▽L0 h1
 
f
 
▽L0 h2  x1 x2 x3
f  0 0 1
▽L1 h1  


f  v1 v2 v3 
1 

O = ▽Lf h2  =  0 rank(O) < 3 ⇔ x1 v2 − x2 v1 = 0
  
 .   0 0
 .  0

 .   0 0
.

▽Ln h1  .
f .
▽Ln h2
f
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

7. Observability and metric for 2D model
Neglecting the vertical components that is given by the depth
measurements, the 2D model is:
˙
x=v
y = 1 xT x
2
We want to study how the 2D relative motions effects the observability
θ
v
x
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

8. Observability and metric for 2D model
It can be easily observed that the terms ▽Lk h1 of O different from zero
f
xT
are only for k = {1, 2}. Thus O = T . Defining x = x , v = v ,
v
x
γ = v , and θ = φ − α, we can reformulate as:
x cos α x sin α γ cos α γ sin α
O= =v
v cos φ v sin φ cos φ sin φ
We define as metric the condition number C ≥ 1 of O :
max{σ1,2 } γ2 + 1 + γ 4 + 2γ 2 cos(2θ) + 1
C= = .
min{σ1,2 } 2γ sin(θ)
Note: C is function of γ and θ
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

9. Observability metrics for 2D model
The condition number C −1 of O as function of γ and θ is plotted:
3
1 2
1
C −1
0.5
θ
0
−1
0
5 −2
6
0 4
2 −3
θ −5 0 γ 0 1 2
γ 3 4
90 5
120 60
1
150 2.5 30
C −1
0.5
180 0
0 210 330
5
5
0 240 300
0 270
−5 −5
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

10. Observability metrics for 2D model
C −1 has a maximum for
γ = 1 (i.e., x = v )
θ = ±π2 (tangential motion)
C −1 is null for θ = 0 (radial motion), γ = 0, and γ → ∞
Assuming a constant relative speed, the observability conditions and
therefore the expected performance of any position observer degrades
when the distance between two AUVs increases.
V
If γ = 1 then C −1 = 1 C −1 = 0 V
X
X
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

11. Simulation results
AUV localization with respect to a fixed buoy using EKF and range-only
measurement
Set of orthogonal segments
10
80 5
0 100 150 200
60 0 50
Estimation error
200
40 100
0 100 150 200
0 50
[m]
20 Distances from transponders
4
0 2
0 100 150 200
0 50
-20 Eigenvalue ekf covarinace
0.2
-40 0.1
-80 -60 -40 -20 0 0
[m] 0 50 100 150 200
1/Observability index
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

12. Simulation results
AUV localization with respect to a fixed buoy using EKF and range-only
measurement
Same circular paths (θ = π/2) at different velocities (changing γ)
20
10
00 50 100 150 200 250
Estimation error
100 200
100
80 00 50 100 150 200 250
Distances from transponders
60 2
[m]
40 1
20 00 50 100 150 200 250
Eigenvalue ekf covarinace
0 0.05
-20 0
-50 0 50 0 50 100 150 200 250
[m] 1/Observability index
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

13. Simulation results
AUV localization with respect to a fixed buoy using EKF and range-only
measurement
Different circular paths with constant γ
20
10
00 50 100 150 200 250
Estimation error
200
300 100
250 00 50 100Distances from transponders
150 200 250
200 2
[m]
150 1
100 00 50 150
100 Eigenvalue ekf covarinace200 250
50
0.01
0 0.005
-200 -100 0 100 200 00 50 100 150 200 250
[m] 1/Observability index
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

14. Relative localization experiments
Inverse localization problem: a surface vehicle has to estimate the
position of an underwater transponder (whose depth is known) using
range measurement from acoustic model.
◮ surface vehicle with GPS
◮ a transponder at 3m depth (as a second vehicle)
◮ acoustic modem exchanging few bytes every 2-3 seconds
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

15. Relative localization experiments
The ASV was commanded to perform different paths: a set of
parallel/orthogonal segments or circular paths
The data were post-processed to test an extended Luenberger observer
estimating the relative positioning between the ASV and the transponder
0 0
−70
−50 −80 −50
T:0
T:248
utmy [m]
utmy [m]
−90
T:330
T:147
−100 T:248
T:98
utmy [m]
T:414
−110 T:83
T:0 T:196
−100 T:83 −100
T:165 T:497
−120
T:413 T:294 T:49
T:496 T:245
T:166
−130 T:331
T:0
−140
−150 −150 −150
−100 −80 −60 −40 −20 0 20 40 60 −80 −60 −40 −20 0 20 40 60 −100 −80 −60 −40 −20 0 20 40 60
utmx [m] utmx [m] utmx [m]
Experiments in Lisbon, Nov. 2010.
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

16. Conclusions and future works
◮ Observability conditions for cooperative underwater localization
◮ Find the relative motions that do not ensure observability
◮ We defined a metric for the observability
◮ We tested different observers for relative positioning estimation
◮ Find elementary behaviors/maneuvers to move the robots ensuring
observability
◮ Extend the observability issues to more than two vehicles
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011

17. CO3 AUVs Project
framework : COoperative COgnitive COntrol for Autonomous
Underwater Vehicles (CO3 AUVs) Project
fundings : FP7 - Cooperation - ICT - Challenge 2
Cognitive Systems, Interaction, Robotics
kind : Collaborative Project (STREP)
acronym : CO3 AUVs
duration : 3 years
start : Feb 2009
effort : 323 pm
budget : ≈ 2.5 Me
http://robotics.jacobs-university.de/projects/Co3-AUVs/
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011