Package lejos.robotics.localization

Class MCLParticleSet

java.lang.Object

lejos.robotics.localization.MCLParticleSet

public class MCLParticleSet
extends java.lang.Object

Represents a particle set for the particle filtering algorithm.

Field Summary

Fields

Modifier and Type	Field and Description
private int	_iterations
private float	angleNoiseFactor
private static float	BIG_FLOAT
private int	border
private Rectangle	boundingRect
private static boolean	debug
private float	distanceNoiseFactor
private RangeMap	map
static int	maxIterations
private float	maxWeight
private int	numParticles
private MCLParticle[]	particles
private java.util.Random	random
private float	totalWeight
private float	twoSigmaSquared

Constructor Summary

Constructors

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

Modifier and Type	Method and Description
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.
private MCLParticle	generateParticle() Generate a random particle within the mapped area.
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

BIG_FLOAT

private static final float BIG_FLOAT

See Also:

Constant Field Values

maxIterations

public static int maxIterations

twoSigmaSquared

private float twoSigmaSquared

distanceNoiseFactor

private float distanceNoiseFactor

angleNoiseFactor

private float angleNoiseFactor

numParticles

private int numParticles

particles

private MCLParticle[] particles

map

private RangeMap map

maxWeight

private float maxWeight

totalWeight

private float totalWeight

border

private int border

random

private java.util.Random random

boundingRect

private Rectangle boundingRect

debug

private static boolean debug

_iterations

private int _iterations

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

generateParticle

private MCLParticle generateParticle()

Generate a random particle within the mapped area.

Returns:

the particle

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

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

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