lejos.robotics.localization
Class MCLPoseProvider

java.lang.Object
  extended by lejos.robotics.localization.MCLPoseProvider
All Implemented Interfaces:
PoseProvider, MoveListener

public class MCLPoseProvider
extends Object
implements PoseProvider, MoveListener

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.
 
Method Summary
 void autoUpdate(boolean yes)
          If yes: after a Travel move, the range scanner is requested to get range readings, which are then used to update the particle weights.
 void dumpEstimation(DataOutputStream dos)
          Dump the serialized estimate of pose to a data output stream
 void generateParticles()
          Generate a new particle set, uniformly distributed within the map, and uniformly distributed heading.
 Rectangle getErrorRect()
          Returns the minimum rectangle enclosing all the particles
 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
 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 loadEstimation(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 setPose(Pose aPose)
           
 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
Method Detail

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)
Specified by:
setPose in interface PoseProvider

getParticles

public MCLParticleSet getParticles()
Returns the particle set

Returns:
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

getRangeReadings

public RangeReadings getRangeReadings()
Returns most recent range readings

Returns:
the range readings

getErrorRect

public 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

dumpEstimation

public void dumpEstimation(DataOutputStream dos)
                    throws IOException
Dump the serialized estimate of pose to a data output stream

Parameters:
dos - the data output stream
Throws:
IOException

loadEstimation

public void loadEstimation(DataInputStream dis)
                    throws IOException
Load serialized estimated pose from a data input stream

Parameters:
dis - the data input stream
Throws:
IOException

autoUpdate

public void autoUpdate(boolean yes)
If yes: after a Travel move, the range scanner is requested to get range readings, which are then used to update the particle weights.

Parameters:
yes -

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.