Robust Range Only Beacon Localization

Edwin Olson (eolson)

John Leonard (jleonard)

Seth Teller (teller)(@csail.mit.edu)

MIT Computer Science and

Artificial Intelligence Laboratory

Outline

Our goal: Navigate with LBL beacons, without knowing the

beacon locations

Components Outlier rejection without a prior Initial solution estimation SLAM filter

Optimal exploration

Experimental Results

Using real data from GOATS’02

Applications

Operation in unsurveyed beacon fields Covert deployment Aerial deployment Autonomous deployment

Moving baseline navigation Vehicles serve as beacons

Detection of beacon movement

Basic Idea

1. Record range measurements while traveling a relatively short distance.

2. Initialize feature in Kalman filter based on trilateration.

3. Continue updating both robot state and beacon position with EKF.

but…

Feature Initialization

Noise is a major issue Interference from sensors/other robots

Outlier rejection Necessary due to non Gaussian error

(if Gaussian noise, Kalman filter is optimal) No prior with which to do outlier detection

How bad is the noise?

Our data set has extensive interference from SAS payload

5000 5500 6000 6500 7000 7500 80000

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

Time (seconds)

LBL

time

(2*R

TT

+T

AT

) (s

econ

ds)

But is the noise Gaussian?

Extensive outliers; result is not Gaussian

(Multiplicative noise model is similarly poor)

Noise Characterization

Noise is non-stationary Particular errors can occur consistently Examples: Multi-path, periodic interference

500 1000 1500 2000 2500 3000 3500 4000 4500 5000

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2000 2050 2100 2150 2200 2250 2300 23500

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

Outlier RejectionPrevious Work Prior-based outlier rejection (“gating”)

But we don’t have a prior…

Newman’03 searches for “low-noise” regions, uses them to extrapolate a constraint over higher noise regions Many parameters to tune, ad-hoc

Outlier rejection

Goal Well-principled method (few tunable parameters) Good performance, even in extreme noise

Other considerations CPU time isn’t really a factor

Data arrives so slowly (~4Hz)… More important to make good use of data

Try to make use of what we do know E.g., dead-reckoned vehicle position

Measurements

Use vehicle’s dead-reckoned position and measured range to construct a circle:

Beacon lies on circle

Measurement Consistency

Consider pair-wise measurement consistency

Consistent (two possible solutions)

Inconsistent

Limited dead-reckoning accuracy limits comparison of measurements to small window of time

Spectral Clustering Formulation

Consider many pair-wise compatibility tests Construct a graph: vertices are measurements, edges connect

consistent measurements Inliers will tend to be more connected than outliers!

2

4

5

1

3

1

2

4

3

5

Measurements Graph

Outlier Rejection

Find a cut that separates the inliers from outliers

Cut A: Good! Cut B: Awful Cut C: Mediocre

How do we formalize this?

cut C1

2

3

4

5

6

8

7cut A

cut B

Adjacency Matrix

00010000

00100000

01000010

10001101

00010111

00011010

00101101

00011010

A

Create an Adjacency matrix where element {i,j}=consistency of measurements i and j

1

2

3

4

5

6

8

7

Graph Partitioning

Let u be an indicator vector If ui=1, then measurement i is an inlier

If ui=0, then measurement i is an outlier

What makes a value of u good? Highly consistent measurements are classified as

inliers Less consistent measurements are classified as

outliers

Average Connectivity

Use average inlier connectivity as our metric:

Intuition: Given a set of inliers, when should a measurement be added? Answer: when it’s at least as consistent with the

inliers as the inliers are with themselves

uu

uAuur

T

T

)(Number of edges connecting inliers (*2)

Total number of inliers

Spectral Clustering

How does our metric perform?

Cut A: 1.6 Cut B: 0.5 Cut C: 1.43

cut C1

2

3

4

5

6

8

7cut A

cut B

uu

uAuur

T

T

)(

Finding the best u vector

For discrete-valued u, this is hard! For continuous-valued u, exact solution is known

Differentiating r(u) with respect to u, setting to zero:

It’s an eigenvalue problem! Maximize r(u) by setting u to the maximum eigenvector

ruAu

Optimal eigenvector

First eigenvalue of adjacency matrix A

cut C1

2

3

4

5

6

8

7cut A

cut B

Finding the u vector

We now have the optimal continuous-valued u vector. Larger values -> inliers

We need the discrete version Threshold u by scalar t Brute force search for t

Only O(n); try each value of u as threshold Incorporate prior, if known, of % of outliers

Computation in blocks

Measurements use dead-reckoned position Error in Adjacency matrix grows with dead-

reckoning error Must limit this by performing outlier rejection in

blocks Computation in blocks also bounds work

needed to compute eigenvector

Computational Optimization

Helpful observation: Our solution is the largest eigenvector, use the

power method to find it!

Power method Behavior of Anv is dominated by the largest

eigenvector of A (call it u). For all1 v, Anv u as n infinity Anv is good enough in a few iterations (~3)

1Except vTu=0

Result on one block

Results from our algorithm: Black: outlier Blue: inlier

Highly consistent set of measurements are classified as inliers

Spectral clustering of 25 measurements (GOATS’02 data)

-600 -400 -200 0 200 400 600 800 1000 1200 1400-400

-200

0

200

400

600

800

1000

1200

1400

16000.435581

Result on many blocks

5000 5500 6000 6500 7000 7500 8000200

300

400

500

600

Spectral Clustering, block size=20, prior=50% outliers

5000 5500 6000 6500 7000 7500 8000200

300

400

500

600

After

Before

Multiple vehicles

If vehicles positions are known in the same coordinate frame, just add the data and use the same algorithm.

No need to do outlier rejection independently on each AUV. In fact, better not to!

Initial Solution Estimation

Given “clean” data, estimate a beacon location Or determine that it’s still ambiguous!

Compute intersections of consistent measurements

Find an area with many votes

Solution Estimation

Put each intersection into a 2-dimensional accumulator

Extract peaks We get multiple solutions and the

number of votes for each

If #votes in 1st peak >> #votes in 2nd peak, then initialize feature

Ratio ~=2

250 300 350 400 450 500 550 600 650 700 750

150

200

250

300

350

400

450

500

550

2 4 6 8 10 12 14 16

2

4

6

8

10

12

14

16

18

10

20

30

40

50

60

2 4 6 8 10 12 14 16

2

4

6

8

10

12

14

16

18

10

20

30

40

50

60

2 4 6 8 10 12 14 16

2

4

6

8

10

12

14

16

18

10

20

30

40

50

60

Initial Solution Estimation

Vote ratio=4 to show algorithm working for longer. Ratio~=2 more realistic.

Put it all together

We can now filter range measurements

We can estimate where beacons are When we find a beacon, add feature to SLAM filter

Beacons define coordinate system for robot Differ from global frame by rigid translation and rotation Difference is related to dead-reckoning error before

acquiring beacons.

SLAM

GOATS’02 data Four beacons

Dead-reckoned path in Red Uncalibrated compass+DVL

EKF path with prior beacon locations in magenta

200 300 400 500 600 700 800150

200

250

300

350

400

450

500

550

600

650

SLAM Movie

Optimal Exploration

Robot at x, beacon is at either A or B.

Disambiguate by maximizing the difference in range depending on actual location

i.e., maximize:

What should robot do now?150 200 250 300 350 400 450 500 550 600

150

200

250

300

350

400

450

18.037225 (0.433380)

100 200 300 400 500 600 700250

300

350

400

450

500

550

600

650

700

750

22.523955 (0.567436)

2

1222

122 )()()()( yBxByAxAr yxyx

Path leads to two possible solutions

Path leads to only one plausible solution

Optimal Exploration: Solution

Gradient is easily computed

Absolute value handled by setting A to be the closest of A and B.

yrA

rA

rB

rB

xrA

rA

rB

rBr

yyyy

xxxx

ˆ

ˆ

-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5-2.5

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

Optimal robot motions given possible beacon locations at (-1,0) and (1,0). Arrow size indicates magnitude of ∆r per distance traveled.

Future Work

Guess beacon locations earlier and use particle filter to track the multiple hypotheses

Incorporate optimal exploration algorithm into experiment.

Questions/Comments