MCLPoseProvider

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

All Implemented Interfaces:

PoseProvider, MoveListener
```
public class MCLPoseProvider
extends java.lang.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 lejos.robotics.RangeScanner to provide lejos.robotics.RangeReadings which are used, together with the RangeMap to calculate the probability weight of each MCLParticle .

Nested Class Summary

Nested Classes
Modifier and Type Class and Description

(package private) class MCLPoseProvider.Updater
Predicts particle pose from odometry data.

Nested Classes
Modifier and Type	Class and Description
`(package private) class`	`MCLPoseProvider.Updater` Predicts particle pose from odometry data.

Field Summary

Fields
Modifier and Type	Field and Description
`private float`	`_heading`
`private float`	`_x`
`private float`	`_y`
`private float`	`BIG_FLOAT`
`private int`	`border`
`private boolean`	`busy`
`private boolean`	`debug`
`private boolean`	`incomplete`
`private boolean`	`lost`
`private RangeMap`	`map`
`private float`	`maxX`
`private float`	`maxY`
`private float`	`minX`
`private float`	`minY`
`private int`	`numParticles`
`private MCLParticleSet`	`particles`
`private RangeReadings`	`readings`
`private RangeScanner`	`scanner`
`private boolean`	`updated`
`private MCLPoseProvider.Updater`	`updater`
`private double`	`varH`
`private double`	`varX`
`private double`	`varY`

Constructor Summary

Constructors
Constructor and Description
`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

All Methods Instance Methods Concrete Methods
Modifier and Type	Method and Description
`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.
`Rectangle2D`	`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

- Field Detail
  - particles
```
private MCLParticleSet particles
```
  - scanner
```
private RangeScanner scanner
```
  - map
```
private RangeMap map
```
  - numParticles
```
private int numParticles
```
  - _x
```
private float _x
```
  - _y
```
private float _y
```
  - _heading
```
private float _heading
```
  - minX
```
private float minX
```
  - maxX
```
private float maxX
```
  - minY
```
private float minY
```
  - maxY
```
private float maxY
```
  - varX
```
private double varX
```
  - varY
```
private double varY
```
  - varH
```
private double varH
```
  - updated
```
private boolean updated
```
  - updater
```
private MCLPoseProvider.Updater updater
```
  - border
```
private int border
```
  - debug
```
private boolean debug
```
  - busy
```
private boolean busy
```
  - BIG_FLOAT
```
private float BIG_FLOAT
```
  - readings
```
private RangeReadings readings
```
  - lost
```
private boolean lost
```
  - incomplete
```
private boolean incomplete
```
- 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 Rectangle2D 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
    
    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
    
    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.

Class MCLPoseProvider

Nested Class Summary

Field Summary

Constructor Summary

Method Summary

Methods inherited from class java.lang.Object

Field Detail

particles

scanner

map

numParticles

_x

_y

_heading

minX

maxX

minY

maxY

varX

varY

varH

updated

updater

border

debug

busy

BIG_FLOAT

readings

lost

incomplete

Constructor Detail

MCLPoseProvider

MCLPoseProvider

Method Detail

setMap

setInitialPose

setInitialPose

setDebug

setPose

getParticles

getReadings

setParticles

generateParticles

moveStarted

moveStopped

update

update

isUpdated

isLost

incompleteRanges

getXRange

getYRange

getPose

getEstimatedPose

estimatePose

getRangeReadings

getErrorRect

getMaxX

getMinX

getMaxY

getMinY

getSigmaX

getSigmaY

getSigmaHeading

getScanner

dumpObject

loadObject

isBusy