lejos.robotics.localization
Class MCLPoseProvider

java.lang.Object
  lejos.robotics.localization.MCLPoseProvider

All Implemented Interfaces:

PoseProvider, MoveListener, Transmittable

public class MCLPoseProvider
extends java.lang.Object
implements PoseProvider, MoveListener, Transmittable

Maintains an estimate of the robot pose using sensor data. It uses Monte Carlo Localization (See section 8.3 of "Probabilistic Robotics" by Thrun et al.
Uses a MCLParticleSet to represent the probability distribution of the estimated pose. It uses a MoveProvider to supply odometry data whenever a movement is completed, from which the Pose of each particle is updated. It then uses a RangeScanner to provide RangeReadings which are used, together with the RangeMap to calculate the probability weight of each MCLParticle .

Author:

Lawrie Griffiths and Roger Glassey

Constructor Summary

MCLPoseProvider(MoveProvider mp, RangeScanner scanner, RangeMap map, int numParticles, int border)
Allocates a new MCLPoseProvider.

MCLPoseProvider(RangeMap map, int numParticles, int border)
Constructor for use on PC

Method Summary

void	dumpObject(java.io.DataOutputStream dos) Dump the serialized estimate of pose to a data output stream
void	estimatePose() Estimate pose from weighted average of the particles Calculate statistics
void	generateParticles() Generate a new particle set, uniformly distributed within the map, and uniformly distributed heading.
java.awt.Rectangle	getErrorRect() Returns the minimum rectangle enclosing all the particles
Pose	getEstimatedPose()
float	getMaxX() Returns the maximum value of X in the particle set
float	getMaxY() Returns the maximum value of Y in the particle set;
float	getMinX() Returns the minimum value of X in the particle set;
float	getMinY() Returns the minimum value of Y in the particle set;
MCLParticleSet	getParticles() Returns the particle set
Pose	getPose() Returns the best best estimate of the current pose;
RangeReadings	getRangeReadings() Returns most recent range readings
RangeReadings	getReadings() Get the current range readings
RangeScanner	getScanner() Returns the range scanner
float	getSigmaHeading() Returns the standard deviation of the heading values in the particle set;
float	getSigmaX() Returns the standard deviation of the X values in the particle set;
float	getSigmaY() Returns the standard deviation of the Y values in the particle set;
float	getXRange() Returns the difference between max X and min X
float	getYRange() Return difference between max Y and min Y
boolean	incompleteRanges() returns range scanner failure status
boolean	isBusy() returns true if particle weights are being updated.
boolean	isLost() returns lost status - all particles have very low probability weights
boolean	isUpdated() Returns update success flag
void	loadObject(java.io.DataInputStream dis) Load serialized estimated pose from a data input stream
void	moveStarted(Move event, MoveProvider mp) Required by MoveListener interface; does nothing
void	moveStopped(Move event, MoveProvider mp) Required by MoveListener interface.
void	setDebug(boolean on) Set debugging on or off
void	setInitialPose(Pose aPose, float radiusNoise, float headingNoise) Generates an initial particle set in a circular normal distribution, centered on aPose.
void	setInitialPose(RangeReadings readings, float sigma) Generates an initial particle set using the range readings.
void	setMap(RangeMap map) Associates a map with the MCLPoseProvider (for example a map send from the PC).
void	setParticles(MCLParticleSet particles) Associate a particle set with the MCLPoseProvider (e.g.
void	setPose(Pose aPose) set the initial pose cloud with radius noise 1 and heading noise 1
boolean	update() Calls range scanner to get range readings, calculates the probabilities of each particle from the range readings and the map and calls resample(()
boolean	update(RangeReadings readings) Calculates particle weights from readings, then resamples the particle set;

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Constructor Detail

MCLPoseProvider

public MCLPoseProvider(MoveProvider mp,
                       RangeScanner scanner,
                       RangeMap map,
                       int numParticles,
                       int border)

Allocates a new MCLPoseProvider.

Parameters:

mp - - the MoveProivder

scanner - - the RangeScanner

map - - the RangeMap

numParticles - number of particles

border - of the map

MCLPoseProvider

public MCLPoseProvider(RangeMap map,
                       int numParticles,
                       int border)

Constructor for use on PC

Parameters:

map - the RangeMap

numParticles - the numbers of particles

border - of the map

Method Detail

setMap

public void setMap(RangeMap map)

Associates a map with the MCLPoseProvider (for example a map send from the PC).

Parameters:

map - the RangeMap

setInitialPose

public void setInitialPose(Pose aPose,
                           float radiusNoise,
                           float headingNoise)

Generates an initial particle set in a circular normal distribution, centered on aPose.

Parameters:

aPose - - center of the cloud

radiusNoise - - standard deviation of the radius of the cloud

headingNoise - - standard deviation of the heading;

setInitialPose

public void setInitialPose(RangeReadings readings,
                           float sigma)

Generates an initial particle set using the range readings. The particles have a significant probability weight given the readings.

Parameters:

readings -

sigma - range reading noise standard deviation.

setDebug

public void setDebug(boolean on)

Set debugging on or off

Parameters:

on - true = on, false = off

setPose

public void setPose(Pose aPose)

set the initial pose cloud with radius noise 1 and heading noise 1

Specified by:

setPose in interface PoseProvider

getParticles

public MCLParticleSet getParticles()

Returns the particle set

Returns:

the particle set

getReadings

public RangeReadings getReadings()

Get the current range readings

Returns:

the RangeReadings object

setParticles

public void setParticles(MCLParticleSet particles)

Associate a particle set with the MCLPoseProvider (e.g. particles sent from the PC)

Parameters:

particles - the particle set

generateParticles

public void generateParticles()

Generate a new particle set, uniformly distributed within the map, and uniformly distributed heading.

moveStarted

public void moveStarted(Move event,
                        MoveProvider mp)

Required by MoveListener interface; does nothing

Specified by:

moveStarted in interface MoveListener

Parameters:

event - the movement

mp - the movement provider

moveStopped

public void moveStopped(Move event,
                        MoveProvider mp)

Required by MoveListener interface. The pose of each particle is updated using the odometry data of the Move object.

Specified by:

moveStopped in interface MoveListener

Parameters:

event - the move just completed

mp - the MoveProvider

update

public boolean update()

Calls range scanner to get range readings, calculates the probabilities of each particle from the range readings and the map and calls resample(()

Returns:

true if update was successful

update

public boolean update(RangeReadings readings)

Calculates particle weights from readings, then resamples the particle set;

Parameters:

readings -

Returns:

true if update was successful.

isUpdated

public boolean isUpdated()

Returns update success flag

Returns:

true if update is successful

isLost

public boolean isLost()

returns lost status - all particles have very low probability weights

Returns:

true if robot is lost

incompleteRanges

public boolean incompleteRanges()

returns range scanner failure status

Returns:

true if range readings are incomplete

getXRange

public float getXRange()

Returns the difference between max X and min X

Returns:

the difference between min and max X

getYRange

public float getYRange()

Return difference between max Y and min Y

Returns:

difference between max and min Y

getPose

public Pose getPose()

Returns the best best estimate of the current pose;

Specified by:

getPose in interface PoseProvider

Returns:

the estimated pose

getEstimatedPose

public Pose getEstimatedPose()

estimatePose

public void estimatePose()

Estimate pose from weighted average of the particles Calculate statistics

getRangeReadings

public RangeReadings getRangeReadings()

Returns most recent range readings

Returns:

the range readings

getErrorRect

public java.awt.Rectangle getErrorRect()

Returns the minimum rectangle enclosing all the particles

Returns:

rectangle : the minimum rectangle enclosing all the particles

getMaxX

public float getMaxX()

Returns the maximum value of X in the particle set

Returns:

max X

getMinX

public float getMinX()

Returns the minimum value of X in the particle set;

Returns:

minimum X

getMaxY

public float getMaxY()

Returns the maximum value of Y in the particle set;

Returns:

max y

getMinY

public float getMinY()

Returns the minimum value of Y in the particle set;

Returns:

minimum Y

getSigmaX

public float getSigmaX()

Returns the standard deviation of the X values in the particle set;

Returns:

sigma X

getSigmaY

public float getSigmaY()

Returns the standard deviation of the Y values in the particle set;

Returns:

sigma Y

getSigmaHeading

public float getSigmaHeading()

Returns the standard deviation of the heading values in the particle set;

Returns:

sigma heading

getScanner

public RangeScanner getScanner()

Returns the range scanner

Returns:

the range scanner

dumpObject

public void dumpObject(java.io.DataOutputStream dos)
                throws java.io.IOException

Dump the serialized estimate of pose to a data output stream

Specified by:

dumpObject in interface Transmittable

Parameters:

dos - the data output stream

Throws:

java.io.IOException

loadObject

public void loadObject(java.io.DataInputStream dis)
                throws java.io.IOException

Load serialized estimated pose from a data input stream

Specified by:

loadObject in interface Transmittable

Parameters:

dis - the data input stream

Throws:

java.io.IOException

isBusy

public boolean isBusy()

returns true if particle weights are being updated. The robot should not move while this is happening otherwise the prediction from odometry data may introduce errors into the updating.

Returns:

true if weight update is in progress.