Reliable Range based Localization and SLAM

Joseph DjugashMasters Student

Presenting work done by:Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth

Motivation

Motivation

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

Outline

Introduction Range-Only Sensor Localization SLAM Future Work

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 robot’s current error and the error within the sensors.

Robot

Landmark

Range-Only Sensors We are provided

with an annulus instead of an ellipse.

Extending classical approaches to localization requires additional considerations.

Robot

Landmark

Range-Only Sensors

r1 r2

Outline

Introduction Range-Only Sensor Localization

Kalman Filter Particle Filter Results

SLAM Future Work

Predictor Corrector Iterative Process

Predict the new state (and its uncertainty) based on current state and process model

Correct state estimate with new measurement

CorrectorPredictor

Outline

Introduction Range-Only Sensor Localization

Kalman Filter Particle Filter Results

SLAM Future Work

Kalman Filter Belief Representation

Error Function – Gaussian Mean and Covariance

Process Model (State qk = [xr, yr, θr]T) qk = A·qk-1 + B·uk-1 + wk-1

Pk= A·Pk-1·AT + B·Uk-1·BT + Qk-1

Measurement Model qk = qk-1 + Kk·(zk – H·qk-1) Kk = Pk·HT·(H·Pk·HT + Rk)-1

Pk = (I – Kk·H)·Pk

^ –

– +

^ +

^ + ^ – ^ –

– –

+ –

Kalman Filter Belief Representation

Error Function – Gaussian Mean and Covariance

Process Model (State qk = [xr, yr, θr]T) qk = A·qk-1 + B·uk-1 + wk-1

Pk= A·Pk-1·AT + B·Uk-1·BT + Qk-1

Measurement Model qk = qk-1 + Kk·(zk – H·qk-1) Kk = Pk·HT·(H·Pk·HT + Rk)-1

Pk = (I – Kk·H)·Pk

^ –

– +

^ +

^ + ^ – ^ –

– –

+ –

Last Relative Measurement. (∆d, ∆θ)

Kalman Filter Belief Representation

Error Function – Gaussian Mean and Covariance

Process Model (State qk = [xr, yr, θr]T) qk = A·qk-1 + B·uk-1 + wk-1

Pk= A·Pk-1·AT + B·Uk-1·BT + Qk-1

Measurement Model qk = qk-1 + Kk·(zk – H·qk-1) Kk = Pk·HT·(H·Pk·HT + Rk)-1

Pk = (I – Kk·H)·Pk

^ –

– +

^ +

^ + ^ – ^ –

– –

+ –

Measurement range to beacon

Estimated range to beaconMeasurement

variance

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

Outline

Introduction Range-Only Sensor Localization

Kalman Filter Particle Filter Results

SLAM Future Work

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(x|rm) = e( )

Where rm is the measured range and r is the range estimate from the particle to beacon

1 .

σ√2π-(r – rm)2

2σ

^

^

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

Outline

Introduction Range-Only Sensor Localization

Kalman Filter Particle Filter Results

SLAM Future Work

The Experiments

Dead Reckoning – Results

Kalman Filter – Results

XTE ATE

Mean Abs. Error

0.5539 m0.3976 m

Max. Error 1.9033 m2.0447 m

Std (σ) 0.4173 m0.3558 m

Particle Filter – Results

XTE ATE

Mean Abs. Error

2.8200 m 2.0898 m

Max. Error 8.6526 m 7.7012 m

Std (σ) 1.0345 m 1.1943 m

Outline

Introduction Range-Only Sensor Localization SLAM

Batch Slam Kalman Filter Results

Future Work

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

Outline

Introduction Range-Only Sensor Localization SLAM

Batch Slam Kalman Filter Results

Future Work

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

Batch Slam Initializing the Beacons

Assumes robot’s 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:

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

Outline

Introduction Range-Only Sensor Localization SLAM

Batch Slam Kalman Filter Results

Future Work

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

Beacon Initialization

Kalman Filter SLAM Kalman filter localization algorithm can be

easily extended for SLAM The state vector becomes:

qk = [xr, yr, θk, xb1, yb1, … , xbn, ybn]T

As new beacons are initialized, expand the state vector and covariance matrix to the correct dimension q ~ 2n+3 P ~ 2n+3 square where n is the number of initialized beacons

Kalman Filter SLAM

Advantages: Similar to Kalman Filter Localization Settles to locally accurate solution

Disadvantages: Wrong Beacon Initialization could lead to

diverged solution

Outline

Introduction Range-Only Sensor Localization SLAM

Batch Slam Kalman Filter Results

Future Work

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 (σ) 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 (σ) 0.6004 m 0.5745 m

Kalman Filter & Batch Slam – Results(Another Example)

Batch Slam XTE ATE

Mean Abs. Error

1.5038 m 2.0871 m

Max. Error 4.9149 m 5.8212 m

Std (σ) 1.0527 m 1.4968 m

Batch Slam using only 5% of data set

Outline

Introduction Range-Only Sensor Localization SLAM Future Work

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

Questions…