lejos.robotics.localization
Class MCLParticleSet

java.lang.Object
  extended by 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
clone, equals, finalize, 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