Robust Range Only Beacon Localization

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

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

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

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

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

13. Inconsistent Consistent (two possible solutions) Measurement Consistency • Consider pair-wise measurement consistency • Limited dead-reckoning accuracy limits comparison of measurements to small window of time

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

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

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

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. cut B 2 6 cut C 1 cut A 4 7 3 8 5 Spectral Clustering • How does our metric perform? • Cut A: 1.6 • Cut B: 0.5 • Cut C: 1.43

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. cut B 2 6 cut C 1 cut A 4 7 3 8 5 Optimal eigenvector • First eigenvalue of adjacency matrix A

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 • 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