MCLParticle.java

package lejos.robotics.localization;

import java.util.Random;
import lejos.robotics.*;
import lejos.robotics.mapping.RangeMap;
import lejos.robotics.Movement;
import lejos.geom.*;

/*
 * WARNING: THIS CLASS IS SHARED BETWEEN THE classes AND pccomms PROJECTS.
 * DO NOT EDIT THE VERSION IN pccomms AS IT WILL BE OVERWRITTEN WHEN THE PROJECT IS BUILT.
 */

/**
 * Represents a particle for the particle filtering algorithm. The state of the
 * particle is the pose, which represents a possible pose of the robot.
 * 
 * The weight for a particle is set by taking a set of theoretical range readings using a
 * map of the environment, and comparing these ranges with those taken by the
 * robot. The weight represents the relative probability that the robot has this
 * pose. Weights are from 0 to 1.
 * 
 * @author Lawrie Griffiths
 * 
 */
public class MCLParticle {
  private static Random rand = new Random();
 
  // Instance variables (kept to minimum to allow maximum number of particles)
  private Pose pose;
  private float weight = 0;

  /**
   * Create a particle with a specific pose
   * 
   * @param pose the pose
   */
  public MCLParticle(Pose pose) {
    this.pose = pose;
  }

  /**
   * Set the weight for this particle
   * 
   * @param weight the weight of this particle
   */
  public void setWeight(float weight) {
    this.weight = weight;
  }

  /**
   * Return the weight of this particle
   * 
   * @return the weight
   */
  public float getWeight() {
    return weight;
  }

  /**
   * Return the pose of this particle
   * 
   * @return the pose
   */
  public Pose getPose() {
    return pose;
  }

  /**
   * Calculate the weight for this particle by comparing its readings with the
   * robot's readings
   * 
   * @param rr Robot readings
   */
  public void calculateWeight(RangeReadings rr, RangeMap map, float divisor) {
    weight = 1;
    Pose tempPose = new Pose();
    tempPose.setLocation(pose.getLocation());
    for (int i = 0; i < rr.getNumReadings(); i++) {
      float angle = rr.getAngle(i);
      tempPose.setHeading(pose.getHeading() + angle);      
      float robotReading = rr.getRange(i);
      float range = map.range(tempPose);
      if (range < 0) {
    	  weight = 0;
    	  return;
      }
      float diff = robotReading - range;
      weight *= (float) Math.exp(-(diff * diff) / divisor);
    }
  }

  /**
   * Get a specific reading
   * 
   * @param i the index of the reading
   * @return the reading
   */
  public float getReading(int i, RangeReadings rr, RangeMap map) {
    Pose tempPose = new Pose();
    tempPose.setLocation(pose.getLocation());
    tempPose.setHeading(pose.getHeading() + rr.getAngle(i));
    return map.range(tempPose);    
  }

  /**
   * Apply the robot's move to the particle with a bit of random noise.
   * Only works for rotate or travel movements.
   * 
   * @param move the robot's move
   */
  public void applyMove(Movement move, float distanceNoiseFactor, float angleNoiseFactor) {
    float ym = (move.getDistanceTraveled() * ((float) Math.sin(Math.toRadians(pose.getHeading()))));
    float xm = (move.getDistanceTraveled() * ((float) Math.cos(Math.toRadians(pose.getHeading()))));

    pose.setLocation(new Point((float) (pose.getX() + xm + (distanceNoiseFactor * xm * rand.nextGaussian())),
                     (float) (pose.getY() + ym + (distanceNoiseFactor * ym * rand.nextGaussian()))));
    pose.setHeading((float) (pose.getHeading() + move.getAngleTurned()
        + (angleNoiseFactor * move.getAngleTurned() * rand.nextGaussian())));
    pose.setHeading((float) ((int) (pose.getHeading() + 0.5f) % 360));
  }
}