Robust Beacon Localization from Range-Only Data
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
Filtering range data without a prior Outlier rejection with very noisy data
SLAM with estimated beacon locations
Optimal exploration
Problem Statement
Simultaneous Localization and Mapping (SLAM) Range-only measurements
Features only partially observable Use vehicle’s dead reckoning to bootstrap
solution Applications
Covert mine sweeping (beacons not calibrated) Detecting movement of a “stationary” beacon SLAM with uncalibrated sensor networks.
Basic Idea
1. Record range measurements while traveling a relatively short distance.
2. Initialize feature in Kalman filter based on triangulation.
3. Continue updating both robot state and beacon position with EKF.
but…
Feature Initialization
This is the hard step.
Noise is major issue No prior with which to do outlier detection!
The noise is not well behaved…
-10 0 10 20 30 40 50 60 700
0.5
1
1.5
2
2.5
3
-8 -6 -4 -2 0 2 4 6 80
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
Noise is not Gaussian
Easy solution (LSQ) if range error is Gaussian.
It’s not.
Distribution of LBL error (relative to true range). Best Gaussian fit in red. (GOATS’02 data)
These extreme outliers will cause trouble in any linear filter
Noise is not independent or stationary
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.8Nasty, consistent-looking outliers
There’s no signal at all here… but there is dependent noise.
Median Windows (baseline algorithm)
Method: Compute distribution of data z(t) around time t Outlier if z(t)<lowPercentile or
z(t)>highPercentile Pros
Simple, Fast Cons
Can’t distinguish stationary garbage from a real signal Three sensitive parameters to tune Cannot take advantage of multiple observations from
different AUVs.
Median Windows
0 1000 2000 3000 4000 5000 60000
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
0 200 400 600 800 1000 1200 1400 1600 18000
0.2
0.4
0.6
0.8
1
1.2
Hard to tune! Data dependent
Inevitably throwing away good data in order to avoid outliers
50 100 150 200 250 300 350 400 450
0.2
0.25
0.3
0.35
0.4
0.45
0.5
0.55
0.6
Median window misclassifies inliers
Improving Outlier Rejection
Add geometrical constraints
Require measurements to intersect
In AUVs, we don’t get much data
Extract everything we can out of what we have
We can afford to do more processing; not CPU limited.
Measurement Consistency
Consider pair-wise measurement consistency Imposes geometrical constraint on accepted points
Consistent (two possible solutions)
Inconsistent
How do we turn pair-wise constraints into a global classifier?
Spectral Clustering Formulation
Consider Markov process Every measurement is a single state Define transition matrix P
Consistent states have high probability transitions
Find the steady-state state probability vector S. (what state will we be in as t→∞ ?)
t=0: S t=1: PS t=2: P2S t=n: PnS
Best S is eigenvector of P with largest eigenvalue (smaller eigenvalue components get smaller and smaller as
t→∞)
Spectral Clustering
Use singular value decomposition (SVD)
UΣVT=P
First column of U is solution to PS=λS with maximum λ.
Cluster based on thresholding U(:,1) by mean(U(:,1)).
Computation in blocks
Compute SVD for small sets of measurements Manages computational cost: O(n3) Avoids errors in transition matrix by bounding
accumulated DR error Becomes effective at N≈10 for typical LBL data
Performance very good at N≈25
Spectral Clustering
Each circle is a range measurement centered about the AUV’s dead-reckoned position
Blue circles are “inliers” Black circles are
“outliers” Green triangle
represent actual LBL position
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
Spectral Clustering Result
0 1000 2000 3000 4000 5000 6000-200
0
200
400
600
800
1000
1200
1400
1600
2800 3000 3200 3400 3600 3800 4000
0
50
100
150
200
250
300
350
400
0 1000 2000 3000 4000 5000 6000-200
0
200
400
600
800
1000
1200
1400
1600
2800 3000 3200 3400 3600 3800 4000
0
50
100
150
200
250
300
350
400
Median Window (N=21, 20%, 80%)Spectral Clustering, block size=25
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.
(More on this for AUV2004)
Effect of outlier rejection
PDF after outlier rejection…
Can we restore our Gaussian assumptions? Maybe not quite But we’re much
better!
-10 0 10 20 30 40 500
5
10
15
20
25
30
35
-0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.80
0.5
1
1.5
2
2.5
3
3.5
4
4.5
Distribution of LBL error (relative to true range). Outliers rejected via Spectral Clustering. Best Gaussian fit in red. (GOATS’02 data)
Solution Estimation
Given “clean” data, estimate a beacon location Or determine that it’s still ambiguous
K-means clustering of range intersections Typically K=2 We get a measure of cluster variance
(confidence) Least-squares solution within selected cluster
Solution Estimation
Put each intersection into a 2-dimensional accumulator
Extract peaks We get multiple solutions
and the number of votes for each
Initialize feature at mean of points in bucket
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
SLAM Dead-reckoned path in Red 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
200 300 400 500 600 700 800
150
200
250
300
350
400
450
500
550
600
650
7874.1
Path with no priors (this work) Note accuracy up to global
translation/rotation Error accumulated while “locking”
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
How can I make this better/more compelling for the conference?