Robust adaptive optics for localization microscopy deep in ...
Robust Range Only Beacon Localization
description
Transcript of Robust Range Only Beacon Localization
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