Robust Beacon Localization from Range-Only Data

1
Robust Beacon Localization from Range-Only Data

• 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
• Filtering range data without a prior
• Outlier rejection with very noisy data
• SLAM with estimated beacon locations
• Optimal exploration

3
Problem Statement

• Simultaneous Localization and Mapping (SLAM)
• Range-only measurements
• Features only partially observable
• Use vehicles dead reckoning to bootstrap
  solution
• Applications
• Covert mine sweeping (beacons not calibrated)
• Detecting movement of a stationary beacon
• SLAM with uncalibrated sensor networks.

4
Basic Idea

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

5
Feature Initialization

• This is the hard step.
• Noise is major issue
• No prior with which to do outlier detection!
• The noise is not well behaved

6
Noise is not Gaussian

• Easy solution (LSQ) if range error is Gaussian.
• Its not.

These extreme outliers will cause trouble in any
linear filter
Distribution of LBL error (relative to true
range). Best Gaussian fit in red. (GOATS02 data)
7
Noise is not independent or stationary
Nasty, consistent-looking outliers
Theres no signal at all here but there is
dependent noise.
8
Median Windows (baseline algorithm)

• Method
• Compute distribution of data z(t) around time t
• Outlier if z(t)ltlowPercentile or
  z(t)gthighPercentile
• Pros
• Simple, Fast
• Cons
• Cant distinguish stationary garbage from a real
  signal
• Three sensitive parameters to tune
• Cannot take advantage of multiple observations
  from different AUVs.

9
Median Windows
Median window misclassifies inliers

• Hard to tune!
• Data dependent
• Inevitably throwing away good data in order to
  avoid outliers

10
Improving Outlier Rejection

• Add geometrical constraints
• Require measurements to intersect
• In AUVs, we dont get much data
• Extract everything we can out of what we have
• We can afford to do more processing not CPU
  limited.

11
Measurement Consistency

• Consider pair-wise measurement consistency
• Imposes geometrical constraint on accepted points

• How do we turn pair-wise constraints into a
  global classifier?

12
Spectral Clustering Formulation

• Consider Markov process
• Every measurement is a single state
• Define transition matrix P
• Consistent states have high probability
  transitions
• Find the steady-state state probability vector S.
• (what state will we be in as t?8 ?)
• t0 S t1 PS t2 P2S tn PnS
• Best S is eigenvector of P with largest
  eigenvalue
• (smaller eigenvalue components get smaller and
  smaller as t?8)

13
Spectral Clustering

• Use singular value decomposition (SVD)
• USVTP
• First column of U is solution to PS?S with
  maximum ?.
• Cluster based on thresholding U(,1) by
  mean(U(,1)).

14
Computation in blocks

• Compute SVD for small sets of measurements
• Manages computational cost O(n3)
• Avoids errors in transition matrix by bounding
  accumulated DR error
• Becomes effective at N10 for typical LBL data
• Performance very good at N25

15
Spectral Clustering

• Each circle is a range measurement centered about
  the AUVs dead-reckoned position
• Blue circles are inliers
• Black circles are outliers
• Green triangle represent actual LBL position

Spectral clustering of 25 measurements (GOATS02
data)
16
Spectral Clustering Result
Median Window (N21, 20, 80)
Spectral Clustering, block size25
17
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.
• (More on this for AUV2004)

18
Effect of outlier rejection

• PDF after outlier rejection
• Can we restore our Gaussian assumptions?
• Maybe not quite
• But were much better!

Distribution of LBL error (relative to true
range). Outliers rejected via Spectral
Clustering. Best Gaussian fit in red. (GOATS02
data)
19
Solution Estimation

• Given clean data, estimate a beacon location
• Or determine that its still ambiguous
• K-means clustering of range intersections
• Typically K2
• We get a measure of cluster variance (confidence)
• Least-squares solution within selected cluster

20
Solution Estimation

• Put each intersection into a 2-dimensional
  accumulator
• Extract peaks
• We get multiple solutions and the number of votes
  for each
• Initialize feature at mean of points in bucket

21
SLAM

• Path with no priors (this work)
• Note accuracy up to global translation/rotation
• Error accumulated while locking

• Dead-reckoned path in Red
• EKF path with prior beacon locations in magenta

22
SLAM Movie
23
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
24
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.
25
Future Work

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

26
Questions/Comments

• How can I make this better/more compelling for
  the conference?