Robust Range Only Beacon Localization

1
Robust Range Only Beacon Localization

• Edwin Olson (eolson)
• John Leonard (jleonard)
• Seth Teller (teller)
• (_at_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 GOATS02

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 dont have a prior
• Newman03 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 isnt 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 vehicles dead-reckoned position and measured
  range to construct a circle

Beacon lies on circle
13
Measurement Consistency

• Consider pair-wise measurement consistency

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

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?

16
Adjacency Matrix

• Create an Adjacency matrix where element
  i,jconsistency of measurements i and j

17
Graph Partitioning

• Let u be an indicator vector
• If ui1, then measurement i is an inlier
• If ui0, 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 its 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

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
• Its an eigenvalue problem!
• Maximize r(u) by setting u to the maximum
  eigenvector

21
Optimal eigenvector

• First eigenvalue of adjacency matrix A

22
Finding the u vector

• We now have the optimal continuous-valued u
  vector.
• Larger values -gt 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 vTu0
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 (GOATS02
data)
26
Result on many blocks
Before
After
Spectral Clustering, block size20, prior50
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 its 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 gtgt votes in 2nd peak, then
  initialize feature
• Ratio 2

30
Initial Solution Estimation
Vote ratio4 to show algorithm working for
longer. Ratio2 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

• GOATS02 data
• Four beacons
• Dead-reckoned path in Red
• Uncalibrated compassDVL
• 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