1. Robust Range Only Beacon Localization
Edwin Olson (eolson)
John Leonard (jleonard)
Seth Teller (teller)
MIT Computer Science and
Artificial Intelligence Laboratory

2. Outline Our goal: Components Optimal exploration
Navigate with LBL beacons, without knowing the beacon locations
Components
Outlier rejection without a prior
Initial solution estimation
SLAM filter
Optimal exploration

3. Experimental Results
Using real data from GOATS’02

4. Applications Operation in unsurveyed beacon fields
Covert deployment
Aerial deployment
Autonomous deployment
Moving baseline navigation
Vehicles serve as beacons
Detection of beacon movement

5. Basic Idea
Record range measurements while traveling a relatively short distance.
Initialize feature in Kalman filter based on trilateration.
Continue updating both robot state and beacon position with EKF.
but…

6. 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

7. How bad is the noise?
Our data set has extensive interference from SAS payload

8. But is the noise Gaussian?
Extensive outliers; result is not Gaussian
(Multiplicative noise model is similarly poor)

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

10. Outlier Rejection Previous 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

11. Outlier rejection Goal Other considerations
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

12. Measurements
Use vehicle’s dead-reckoned position and measured range to construct a circle:
Beacon lies on circle

13. 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

14. 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! 1 2 4 3 5
Measurements 2 4 5 1 3
Graph

15. 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 C 1 2 3 4 5 6 8 7
cut A
cut B

16. Adjacency Matrix
Create an Adjacency matrix where element {i,j}=consistency of measurements i and j 1 2 3 4 5 6 8 7

17. 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

18. 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
Number of edges connecting inliers (*2)
Total number of inliers

19. Spectral Clustering How does our metric perform? Cut A: 1.6 Cut B: 0.5
Cut C: 1.43
cut C 1 2 3 4 5 6 8 7
cut A
cut B

20. 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

21. Optimal eigenvector First eigenvalue of adjacency matrix A cut B cut A
cut C 1 2 3 4 5 6 8 7
cut A
cut B

22. 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

23. 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

24. 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

25. 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)

26. Result on many blocks Before After
Spectral Clustering, block size=20, prior=50% outliers

27. 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!

28. 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

29. 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

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

31. 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.

32. SLAM GOATS’02 data Dead-reckoned path in Red
Four beacons
Dead-reckoned path in Red
Uncalibrated compass+DVL
EKF path with prior beacon locations in magenta

33. SLAM Movie

34. 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?
Path leads to two possible solutions
Path leads to only one plausible solution

35. Optimal Exploration: Solution
Gradient is easily computed
Absolute value handled by setting A to be the closest of A and B.
Optimal robot motions given possible beacon locations at (-1,0) and (1,0). Arrow size indicates magnitude of ∆r per distance traveled.

36. Future Work
Guess beacon locations earlier and use particle filter to track the multiple hypotheses
Incorporate optimal exploration algorithm into experiment.

37. Questions/Comments