lejos.robotics.localization
Class MCLParticleSet

java.lang.Object
  extended by lejos.robotics.localization.MCLParticleSet
All Implemented Interfaces:
Transmittable

public class MCLParticleSet
extends java.lang.Object
implements Transmittable

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 within the given map.
MCLParticleSet(RangeMap map, int numParticles, int border, RangeReadings readings, float divisor, float minWeight)
          Generates a set of particles within the map that have a minimum weight as as calculated from the particle pose, the range readings and the map.
MCLParticleSet(RangeMap map, int numParticles, Pose initialPose, float radiusNoise, float headingNoise)
          Generates a circular cloud of particles centered on initialPose with random normal radius and angle, and random normal heading.
 
Method Summary
 void applyMove(Move move)
          Apply a move to each particle
 boolean calculateWeights(RangeReadings rr, RangeMap map)
          Calculate the weight for each particle
 void dumpClosest(RangeReadings rr, java.io.DataOutputStream dos, float x, float y)
          Find the closest particle to specified coordinates and dump its details to a data output stream.
 void dumpObject(java.io.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
 int getIterations()
           
 float getMaxWeight()
          The highest weight of any particle
 MCLParticle getParticle(int i)
          Get a specific particle
 void loadObject(java.io.DataInputStream dis)
          Load serialized particles from a data input stream
 int numParticles()
          Return the number of particles in the set
 boolean resample()
          Resample the set picking those with higher weights.
 void setAngleNoiseFactor(float factor)
          Set the distance angle factor
 void setBorder(int border)
          Set border where no particles should be generated
static 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 within the given map.

Parameters:
map - the map of the enclosed environment

MCLParticleSet

public MCLParticleSet(RangeMap map,
                      int numParticles,
                      int border,
                      RangeReadings readings,
                      float divisor,
                      float minWeight)
Generates a set of particles within the map that have a minimum weight as as calculated from the particle pose, the range readings and the map.

Parameters:
map -
numParticles - - number of particles
border - - within which no particles should be generated
readings - - to use in calculating weight
divisor -
minWeight - - the minimum wight of a particle in the map

MCLParticleSet

public MCLParticleSet(RangeMap map,
                      int numParticles,
                      Pose initialPose,
                      float radiusNoise,
                      float headingNoise)
Generates a circular cloud of particles centered on initialPose with random normal radius and angle, and random normal heading.

Parameters:
map - the map
numParticles - the number of particles
initialPose - the center of the cloud
radiusNoise - standard deviation of the normal of the distance from center
headingNoise - standard deviation of heading
Method Detail

numParticles

public int numParticles()
Return the number of particles in the set

Returns:
the number of particles

setDebug

public static 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 boolean calculateWeights(RangeReadings rr,
                                RangeMap map)
Calculate the weight for each particle

Parameters:
rr - the robot range readings

applyMove

public void applyMove(Move move)
Apply a move to each particle

Parameters:
move - the move to apply

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

dumpObject

public void dumpObject(java.io.DataOutputStream dos)
                throws java.io.IOException
Serialize the particle set to a data output stream

Specified by:
dumpObject in interface Transmittable
Parameters:
dos - the data output stream
Throws:
java.io.IOException

getIterations

public int getIterations()

loadObject

public void loadObject(java.io.DataInputStream dis)
                throws java.io.IOException
Load serialized particles from a data input stream

Specified by:
loadObject in interface Transmittable
Parameters:
dis - the data input stream
Throws:
java.io.IOException

dumpClosest

public void dumpClosest(RangeReadings rr,
                        java.io.DataOutputStream dos,
                        float x,
                        float y)
                 throws java.io.IOException
Find the closest particle to specified coordinates and dump its details to a data output stream.

Parameters:
rr - a dummy set of range readings used to determine the angles
dos - the data output stream
x - the x-coordinate
y - the y-coordinate
Throws:
java.io.IOException