robust range only beacon localization
DESCRIPTION
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 - PowerPoint PPT PresentationTRANSCRIPT
![Page 1: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/1.jpg)
Robust Range Only Beacon Localization
Edwin Olson (eolson)
John Leonard (jleonard)
Seth Teller (teller)(@csail.mit.edu)
MIT Computer Science and
Artificial Intelligence Laboratory
![Page 2: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/2.jpg)
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
![Page 3: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/3.jpg)
Experimental Results
Using real data from GOATS’02
![Page 4: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/4.jpg)
Applications
Operation in unsurveyed beacon fields Covert deployment Aerial deployment Autonomous deployment
Moving baseline navigation Vehicles serve as beacons
Detection of beacon movement
![Page 5: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/5.jpg)
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…
![Page 6: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/6.jpg)
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
![Page 7: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/7.jpg)
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)
![Page 8: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/8.jpg)
But is the noise Gaussian?
Extensive outliers; result is not Gaussian
(Multiplicative noise model is similarly poor)
![Page 9: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/9.jpg)
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
![Page 10: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/10.jpg)
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
![Page 11: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/11.jpg)
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
![Page 12: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/12.jpg)
Measurements
Use vehicle’s dead-reckoned position and measured range to construct a circle:
Beacon lies on circle
![Page 13: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/13.jpg)
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
![Page 14: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/14.jpg)
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
![Page 15: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/15.jpg)
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
![Page 16: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/16.jpg)
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
![Page 17: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/17.jpg)
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
![Page 18: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/18.jpg)
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
![Page 19: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/19.jpg)
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
)(
![Page 20: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/20.jpg)
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
![Page 21: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/21.jpg)
Optimal eigenvector
First eigenvalue of adjacency matrix A
cut C1
2
3
4
5
6
8
7cut A
cut B
![Page 22: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/22.jpg)
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
![Page 23: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/23.jpg)
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
![Page 24: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/24.jpg)
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
![Page 25: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/25.jpg)
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
![Page 26: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/26.jpg)
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
![Page 27: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/27.jpg)
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!
![Page 28: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/28.jpg)
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
![Page 29: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/29.jpg)
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
![Page 30: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/30.jpg)
Initial Solution Estimation
Vote ratio=4 to show algorithm working for longer. Ratio~=2 more realistic.
![Page 31: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/31.jpg)
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.
![Page 32: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/32.jpg)
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
![Page 33: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/33.jpg)
SLAM Movie
![Page 34: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/34.jpg)
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
![Page 35: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/35.jpg)
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.
![Page 36: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/36.jpg)
Future Work
Guess beacon locations earlier and use particle filter to track the multiple hypotheses
Incorporate optimal exploration algorithm into experiment.
![Page 37: Robust Range Only Beacon Localization](https://reader034.vdocument.in/reader034/viewer/2022050723/56814394550346895db01014/html5/thumbnails/37.jpg)
Questions/Comments