aima.core.robotics.MonteCarloLocalization Maven / Gradle / Ivy
Show all versions of aima-core Show documentation
package aima.core.robotics;
import java.util.Iterator;
import java.util.LinkedHashSet;
import java.util.Set;
import aima.core.probability.bayes.approx.ParticleFiltering;
import aima.core.probability.domain.FiniteIntegerDomain;
import aima.core.probability.util.ProbUtil;
import aima.core.probability.util.RandVar;
import aima.core.robotics.datatypes.IMclMove;
import aima.core.robotics.datatypes.IMclPose;
import aima.core.robotics.datatypes.IMclRangeReading;
import aima.core.robotics.datatypes.IMclVector;
import aima.core.util.Randomizer;
import aima.core.util.Util;
/**
* Artificial Intelligence A Modern Approach (3rd Edition): page 982.
*
*
*
* 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 {@link 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.
*
* The update cycle of the algorithm is executed by the method {@code 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 {@code 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 {@code localize} as this information is already contained in the set itself.
* The method {@code localize} is divided into these two parts implemented each by a single method:
*
* - {@code applyMove} represents the first line of the update cycle. It moves all samples according to the move / motion model.
* - {@code 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.
*
* The WEIGHTED-SAMPLE-WITH-REPLACEMENT is implemented by the method {@code extendedWeightedSampleWithReplacement}. This implementation contains the addition of a cutoff value. All particles having a weight below this cutoff are ignored.
*
* It is possible to reduce the steps needed for the localization by tweaking the sample count and the parameter {@code cutOff}.
*
* @author Arno von Borries
* @author Jan Phillip Kretzschmar
* @author Andreas Walscheid
*
* @param a pose implementing {@link IMclPose}.
* @param an n-1-dimensional vector implementing {@link IMclVector}, where n is the dimensionality of the environment. This vector describes the angle between two rays in the environment.
* @param a movement (or sequence of movements) of the robot implementing {@link IMclMove}.
* @param a range measurement implementing {@link IMclRangeReading}.
*/
public final class MonteCarloLocalization, V extends IMclVector, M extends IMclMove, R extends IMclRangeReading> {
private static final String SAMPLE_INDEXES_NAME = "SAMPLE_INDEXES";
private final IMclMap map;
private final Randomizer randomizer;
private RandVar sampleIndexes;
private double weightCutOff;
/**
* @param map an instance of a class implementing {@link IMclMap}.
* @param randomizer a {@link Randomizer} that is used for re-sampling.
*/
public MonteCarloLocalization(IMclMap map, Randomizer randomizer) {
this.map = map;
this.randomizer = randomizer;
}
/**
* Sets the minimum weight of the particles.
* @param cutOff the minimum weight below which the corresponding particle gets removed during the {@code resample()} step. Set to zero when in doubt.
*/
public void setWeightCutOff(double cutOff) {
this.weightCutOff = cutOff;
}
/**
* Applies a move to the samples, creating a new {@link Set}.
* @param samples the samples the move will be applied to.
* @param move the move to be applied to the samples.
* @return a new set of size N containing the moved samples.
*/
protected Set applyMove(Set
samples, M move) {
Set
newSamples = new LinkedHashSet
();
for(P sample: samples) {
newSamples.add(sample.applyMovement(move.generateNoise()));
}
return newSamples;
}
/**
* Weights the samples by a given vector of range scans.
* @param samples the samples to be weighted.
* @param rangeReadings the vector containing all range scans.
* @return a vector of weights of size N.
*/
protected double[] weightSamples(Set
samples, R[] rangeReadings) {
Iterator
samplesIterator = samples.iterator();
double[] w = new double[samples.size()];
for(int j=0;j extendedWeightedSampleWithReplacement(Set samples, double[] w) {
int i = 0;
for(;i weightCutOff) break;
}
if(i >= samples.size()) return generateCloud(samples.size()); /*If all particleCloud are below weightCutOff, generate a new set of samples, as we are lost.*/
/*WEIGHTED-SAMPLE-WITH-REPLACEMENT:*/
double[] normalizedW = Util.normalize(w);
Set newSamples = new LinkedHashSet
();
Object[] array = samples.toArray(new Object[0]);
for(i=0; i < samples.size(); i++) {
final int selectedSample = (Integer) ProbUtil.sample(randomizer.nextDouble(),sampleIndexes,normalizedW);
newSamples.add(((P) array[selectedSample]).clone());
}
return newSamples;
}
/**
* This method is the initialization phase of the algorithm. It has to be called to generate a set of samples of count N.
* @param N the count of samples.
* @return a set containing N samples.
*/
public Set
generateCloud(int N) {
Set
samples = new LinkedHashSet
();
Integer[] indexes = new Integer[N];
for(int i=0;i localize(Set samples, M move, R[] rangeReadings) {
if(samples == null) return null;/*initialization phase = call generateCloud*/
Set
newSamples = applyMove(samples, move);/*motion model*/
double[] w = weightSamples(newSamples, rangeReadings);/*range sensor noise model*/
newSamples = extendedWeightedSampleWithReplacement(newSamples, w);
return newSamples;
}
}