lejos.robotics.localization
Class MCLParticleSet

java.lang.Object
  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