Reliable Range based Localization and SLAM

1
Reliable Range based Localization and SLAM

• Joseph Djugash
• Masters Student
• Presenting work done by
• Sanjiv Singh, George Kantor, Peter Corke and
  Derek Kurth

2
Motivation
3
Motivation
4
Introduction

• Much research has been done to perform
  localization under normal/ideal conditions
• Classical sensors fail to provide reliable
  results under non-ideal scenarios
• Alternative methods such as range-only sensors
  have not received enough attention in the
  research field

5
Outline

• Introduction
• Range-Only Sensor
• Localization
• SLAM
• Future Work

6
Range Bearing Sensors

• Errors in estimation of robot location and
  landmark locations are represented as ellipses.
• Each landmark ellipse contains the error of both
  the robots current error and the error within
  the sensors.

7
Range-Only Sensors

• We are provided with an annulus instead of an
  ellipse.
• Extending classical approaches to localization
  requires additional considerations.

8
Range-Only Sensors
9
Outline

• Introduction
• Range-Only Sensor
• Localization
• Kalman Filter
• Particle Filter
• Results
• SLAM
• Future Work

10
Predictor ? Corrector

• Iterative Process
• Predict the new state (and its uncertainty) based
  on current state and process model
• Correct state estimate with new measurement

11
Outline

• Introduction
• Range-Only Sensor
• Localization
• Kalman Filter
• Particle Filter
• Results
• SLAM
• Future Work

12
Kalman Filter

• Belief Representation
• Error Function Gaussian
• Mean and Covariance
• Process Model (State qk xr, yr, ?rT)
• qk Aqk-1 Buk-1 wk-1
• Pk APk-1AT BUk-1BT Qk-1
• Measurement Model
• qk qk-1 Kk(zk Hqk-1)
• Kk PkHT(HPkHT Rk)-1
• Pk (I KkH)Pk

13
Kalman Filter

• Belief Representation
• Error Function Gaussian
• Mean and Covariance
• Process Model (State qk xr, yr, ?rT)
• qk Aqk-1 Buk-1 wk-1
• Pk APk-1AT BUk-1BT Qk-1
• Measurement Model
• qk qk-1 Kk(zk Hqk-1)
• Kk PkHT(HPkHT Rk)-1
• Pk (I KkH)Pk

14
Kalman Filter

• Belief Representation
• Error Function Gaussian
• Mean and Covariance
• Process Model (State qk xr, yr, ?rT)
• qk Aqk-1 Buk-1 wk-1
• Pk APk-1AT BUk-1BT Qk-1
• Measurement Model
• qk qk-1 Kk(zk Hqk-1)
• Kk PkHT(HPkHT Rk)-1
• Pk (I KkH)Pk

Estimated range to beacon


15
Kalman Filter

• Advantages
• Computationally Efficient
• Able to handle high dimensionality with limited
  or no extra computational cost
• Handles short periods of sensor silence
• Disadvantages
• Able to represent only Gaussian distributions
• Assumptions are too restrictive

16
Outline

• Introduction
• Range-Only Sensor
• Localization
• Kalman Filter
• Particle Filter
• Results
• SLAM
• Future Work

17
Particle Filter

• Representing belief by sets of samples or
  particles
• Each particle is represented as (xp, yp),
  (orientation is not maintained)
• Updating procedure is a sequential importance
  sampling approach with re-sampling
• Sampling Standard Gaussian Formula
• P(xrm) e( )
• Where rm is the measured range and r is the range
  estimate from the particle to beacon

18
Particle Filter

• Advantages
• Able to represent arbitrary density
• Converging to true posterior even for
  non-Gaussian and nonlinear system
• Efficient in the sense that particles tend to
  focus on regions with high probability
• Disadvantages
• Worst-case complexity grows exponentially in the
  dimensions

19
Outline

• Introduction
• Range-Only Sensor
• Localization
• Kalman Filter
• Particle Filter
• Results
• SLAM
• Future Work

20
The Experiments
21
Dead Reckoning Results
22
Kalman Filter Results
XTE ATE
Mean Abs. Error 0.5539 m 0.3976 m
Max. Error 1.9033 m 2.0447 m
Std (s) 0.4173 m 0.3558 m
23
Particle Filter Results
XTE ATE
Mean Abs. Error 2.8200 m 2.0898 m
Max. Error 8.6526 m 7.7012 m
Std (s) 1.0345 m 1.1943 m
24
Outline

• Introduction
• Range-Only Sensor
• Localization
• SLAM
• Batch Slam
• Kalman Filter
• Results
• Future Work

25
SLAM

• Beacon Locations are unknown
• Measurements are used to predict beacon locations
  
• Due to errors in measurements, not all
  measurements can be weighed equally
• Consistency between inliers help provide a
  reliable estimates

26
Outline

• Introduction
• Range-Only Sensor
• Localization
• SLAM
• Batch Slam
• Kalman Filter
• Results
• Future Work

27
Batch Slam

• Approaches the SLAM problem by solving two
  non-linear optimization problems
• One to generate the initial estimate of the
  beacon locations
• One to simultaneously refine the vehicle and
  beacon estimates
• Estimated Beacon locations are feed to the Kalman
  filter localization algorithm

28
Batch Slam

• Initializing the Beacons
• Assumes robots odometry is perfect
• Using the range measurements predicts the most
  likely beacon estimates
• Estimates are acquired by minimizing the cost
  function
• and,
• Refining estimates
• Assumes error distributions of each measurement
  is independent
• Most likely beacon positions and vehicle relative
  motion can be found by minimizing the cost
  function

29
Batch Slam

• Advantages
• Produces accurate estimates of beacon locations
• Requires very little data to acquire good results
• Disadvantages
• Computationally Expensive
• Requires fairly accurate dead reckoning data to
  acquire its initial beacon estimate

30
Outline

• Introduction
• Range-Only Sensor
• Localization
• SLAM
• Batch Slam
• Kalman Filter
• Results
• Future Work

31
Beacon Initialization

• Find all pair wise intersections of a set of
  range measurements
• Create a histogram grid with the circle
  intersections
• Find the first two peaks on the grid
• When the ratio between the peaks reaches a
  threshold (set to 2), declare the higher of the
  peaks as the beacon location

32
Beacon Initialization
33
Kalman Filter SLAM

• Kalman filter localization algorithm can be
  easily extended for SLAM
• The state vector becomes
• qk xr, yr, ?k, xb1, yb1, , xbn, ybnT
• As new beacons are initialized, expand the state
  vector and covariance matrix to the correct
  dimension
• q 2n3
• P 2n3 square
• where n is the number of initialized beacons

34
Kalman Filter SLAM

• Advantages
• Similar to Kalman Filter Localization
• Settles to locally accurate solution
• Disadvantages
• Wrong Beacon Initialization could lead to
  diverged solution

35
Outline

• Introduction
• Range-Only Sensor
• Localization
• SLAM
• Batch Slam
• Kalman Filter
• Results
• Future Work

36
Kalman Filter SLAM Results
Raw XTE ATE
Mean Abs. Error 8.5544 m 5.1776 m
Max. Error 18.0817 m 19.2575 m
Std (s) 4.8216 m 4.5486 m
Aff. Trans. XTE ATE
Mean Abs. Error 0.7297 m 0.6872 m
Max. Error 2.6787 m 2.7621 m
Std (s) 0.6004 m 0.5745 m
37
Kalman Filter Batch Slam Results(Another
Example)
Batch Slam using only 5 of data set
Batch Slam XTE ATE
Mean Abs. Error 1.5038 m 2.0871 m
Max. Error 4.9149 m 5.8212 m
Std (s) 1.0527 m 1.4968 m
38
Outline

• Introduction
• Range-Only Sensor
• Localization
• SLAM
• Future Work

39
Future Work

• Develop robust algorithms that produce reliable
  results with poor sensor data
• Develop an approach that relies on multiple
  algorithms at various points during the data set
  to produce better results

40
Questions