P - a pose implementing IMclPose.V - an n-1-dimensional vector implementing IMclVector, where n is the dimensionality of the environment. This vector describes the angle between two rays in the environment.M - a movement (or sequence of movements) of the robot implementing IMclMove.R - a range measurement implementing IMclRangeReading.public final class MonteCarloLocalization<P extends IMclPose<P,V,M>,V extends IMclVector,M extends IMclMove<M>,R extends IMclRangeReading<R,V>>
extends java.lang.Object
function MONTE-CARLO-LOCALIZATION(a, z, N, P(X'|X, v, w), P(z|z*), m) returns a set of samples for the next time step
inputs: a, robot velocities v and w
z, range scan z1,..., zM
P(X'|X,v,w), motion model
P(z|z*), range sensor noise model
m, 2D map of the environment
persistent: S, a vector of samples of size N
local variables: W, a vector of weights of size N
S', a temporary vector of particles of size N
Figure 25.9 A Monte-Carlo-Localization algorithm using a range-scan sensor model with independent noise.
The Monte-Carlo-Localization is an extension of a ParticleFiltering as stated on page 982.
This is true for the functionality but this implementation can not extend the implementation of the ParticleFiltering
as both implementations only contain the actual algorithm as a single method.
localize for the given set of samples, move and vector of range readings.
Before calling this method, a set of samples can be generated through the method generateCloud, which represents the initialization phase of the pseudocode, for the given size N.
This removes the need of specifying the size N on every call of localize as this information is already contained in the set itself.
The method localize is divided into these two parts implemented each by a single method:
applyMove represents the first line of the update cycle. It moves all samples according to the move / motion model.weightSamples represents the second to second last line of the update cycle. A vector of weights is created by this method for the given range scans by comparing every range scan to a ray cast with the correspondent sample through the range sensor noise model.extendedWeightedSampleWithReplacement. This implementation contains the addition of a cutoff value. All particles having a weight below this cutoff are ignored.
cutOff.
| Constructor and Description |
|---|
MonteCarloLocalization(IMclMap<P,V,M,R> map,
Randomizer randomizer) |
| Modifier and Type | Method and Description |
|---|---|
protected java.util.Set<P> |
applyMove(java.util.Set<P> samples,
M move)
Applies a move to the samples, creating a new
Set. |
protected java.util.Set<P> |
extendedWeightedSampleWithReplacement(java.util.Set<P> samples,
double[] w)
Taken
weightedSampleWithReplacement out of ParticleFiltering and extended by a minimum weight. |
java.util.Set<P> |
generateCloud(int N)
This method is the initialization phase of the algorithm.
|
java.util.Set<P> |
localize(java.util.Set<P> samples,
M move,
R[] rangeReadings)
Executes the update cycle of the Monte-Carlo-Localization for the given parameters.
|
void |
setWeightCutOff(double cutOff)
Sets the minimum weight of the particles.
|
protected double[] |
weightSamples(java.util.Set<P> samples,
R[] rangeReadings)
Weights the samples by a given vector of range scans.
|
public MonteCarloLocalization(IMclMap<P,V,M,R> map, Randomizer randomizer)
map - an instance of a class implementing IMclMap.randomizer - a Randomizer that is used for re-sampling.public void setWeightCutOff(double cutOff)
cutOff - the minimum weight below which the corresponding particle gets removed during the resample() step. Set to zero when in doubt.protected java.util.Set<P> applyMove(java.util.Set<P> samples, M move)
Set.samples - the samples the move will be applied to.move - the move to be applied to the samples.protected double[] weightSamples(java.util.Set<P> samples, R[] rangeReadings)
samples - the samples to be weighted.rangeReadings - the vector containing all range scans.protected java.util.Set<P> extendedWeightedSampleWithReplacement(java.util.Set<P> samples, double[] w)
weightedSampleWithReplacement out of ParticleFiltering and extended by a minimum weight.samples - the samples to be re-sampled.w - the probability distribution on the samples.public java.util.Set<P> generateCloud(int N)
N - the count of samples.public java.util.Set<P> localize(java.util.Set<P> samples, M move, R[] rangeReadings)
samples - the sample cloud.move - the move to be applied to the cloud.rangeReadings - the range scan that has been performed after the move has ended.null is returned if samples is null.