lejos.robotics.localization
Class MCLParticleSet

java.lang.Object
  lejos.robotics.localization.MCLParticleSet

public class MCLParticleSet
extends Object

Represents a particle set for the particle filtering algorithm.

Author:

Lawrie Griffiths

Field Summary

static int

maxIterations

Constructor Summary

MCLParticleSet(RangeMap map, int numParticles, int border)
Create a set of particles randomly distributed with the given map.

Method Summary

void	applyMove(Movement move) Apply a move to each particle
void	calculateWeights(RangeReadings rr, RangeMap map) Calculate the weight for each particle
void	dumpClosest(RangeReadings rr, RangeMap map, DataOutputStream dos, float x, float y) Find the closest particle to specified coordinates and dump its details to a data output stream.
void	dumpEstimation(DataOutputStream dos) Dump the serialized estimate of pose to a data output stream
void	dumpParticles(DataOutputStream dos) Serialize the particle set to a data output stream
int	findClosest(float x, float y) Find the index of the particle closest to a given co-ordinates.
float	getBorder() Get the border where particles should not be generated
Rectangle	getErrorRect() Return the minimum rectangle enclosing all the particles
Pose	getEstimatedPose() Get the estimated pose of the robot
float	getMaxWeight() The highest weight of any particle
float	getMaxX() Get the maximum X value of the estimated position
float	getMaxY() Get the maximum Y value of the estimated position
float	getMinX() Get the minimum X value of the estimated position
float	getMinY() Get the minimum Y value of the estimated position
MCLParticle	getParticle(int i) Get a specific particle
void	loadEstimation(DataInputStream dis) Load serialized estimated pose from a data input stream
void	loadParticles(DataInputStream dis) Load serialized particles from a data input stream
int	numParticles() Return the number of particles in the set
void	printMaxWeight()
boolean	resample() Resample the set picking those with higher weights.
void	resetEstimate() Reset the estimated position to unknown
void	setAngleNoiseFactor(float factor) Set the distance angle factor
void	setBorder(int border) Set border where no particles should be generated
void	setDebug(boolean debug) Set system out debugging on or off
void	setDistanceNoiseFactor(float factor) Set the distance noise factor
void	setMaxIterations(int max) Set the maximum iterations for the resample algorithm
void	setSigma(float sigma) Set the standard deviation for the sensor probability model

Methods inherited from class java.lang.Object

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

Field Detail

maxIterations

public static int maxIterations

Constructor Detail

MCLParticleSet

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

Create a set of particles randomly distributed with the given map.

Parameters:

map - the map of the enclosed environment

Method Detail

numParticles

public int numParticles()

Return the number of particles in the set

Returns:

the number of particles

setDebug

public void setDebug(boolean debug)

Set system out debugging on or off

Parameters:

debug - true to set debug, false to set it off

getParticle

public MCLParticle getParticle(int i)

Get a specific particle

Parameters:

i - the index of the particle

Returns:

the particle

resample

public boolean resample()

Resample the set picking those with higher weights. Note that the new set has multiple instances of the particles with higher weights.

Returns:

true iff lost

calculateWeights

public void calculateWeights(RangeReadings rr,
                             RangeMap map)

Calculate the weight for each particle

Parameters:

rr - the robot range readings

printMaxWeight

public void printMaxWeight()

applyMove

public void applyMove(Movement move)

Apply a move to each particle

Parameters:

move - the move to apply

getEstimatedPose

public Pose getEstimatedPose()

Get the estimated pose of the robot

Returns:

the estimated pose

getMinX

public float getMinX()

Get the minimum X value of the estimated position

Returns:

the minimum X value

getMaxX

public float getMaxX()

Get the maximum X value of the estimated position

Returns:

the maximum X value

getMinY

public float getMinY()

Get the minimum Y value of the estimated position

Returns:

the minimum Y value

getMaxY

public float getMaxY()

Get the maximum Y value of the estimated position

Returns:

the maximum Y value

resetEstimate

public void resetEstimate()

Reset the estimated position to unknown

getErrorRect

public Rectangle getErrorRect()

Return the minimum rectangle enclosing all the particles

Returns:

the rectangle

getMaxWeight

public float getMaxWeight()

The highest weight of any particle

Returns:

the highest weight

getBorder

public float getBorder()

Get the border where particles should not be generated

Returns:

the border

setBorder

public void setBorder(int border)

Set border where no particles should be generated

Parameters:

border - the border

setSigma

public void setSigma(float sigma)

Set the standard deviation for the sensor probability model

Parameters:

sigma - the standard deviation

setDistanceNoiseFactor

public void setDistanceNoiseFactor(float factor)

Set the distance noise factor

Parameters:

factor - the distance noise factor

setAngleNoiseFactor

public void setAngleNoiseFactor(float factor)

Set the distance angle factor

Parameters:

factor - the distance angle factor

setMaxIterations

public void setMaxIterations(int max)

Set the maximum iterations for the resample algorithm

Parameters:

max - the maximum iterations

findClosest

public int findClosest(float x,
                       float y)

Find the index of the particle closest to a given co-ordinates. This is used for diagnostic purposes.

Parameters:

x - the x-coordinate

y - the y-coordinate

Returns:

the index

dumpParticles

public void dumpParticles(DataOutputStream dos)
                   throws IOException

Serialize the particle set to a data output stream

Parameters:

dos - the data output stream

Throws:

IOException

loadParticles

public void loadParticles(DataInputStream dis)
                   throws IOException

Load serialized particles from a data input stream

Parameters:

dis - the data input stream

Throws:

IOException

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

dumpClosest

public void dumpClosest(RangeReadings rr,
                        RangeMap map,
                        DataOutputStream dos,
                        float x,
                        float y)
                 throws IOException

Find the closest particle to specified coordinates and dump its details to a data output stream.

Parameters:

dos - the data output stream

x - the x-coordinate

y - the y-coordinate

Throws:

IOException