particlefilter.java

一个基于PlaceLab的室内和室外的智能导航系统

package org.placelab.particlefilter;

import java.util.Enumeration;
import java.util.Vector;

import org.placelab.core.Observable;



/**
 * 
 *
 */
public abstract class ParticleFilter {
	
	//////// MEMBER VARIABLES ////////
	private SensorModel sensorModel=null;
	private MotionModel motionModel=null;

	protected int minParticles;
	protected int maxParticles;
	private long lastTimestamp=0L;
	private boolean firstUpdate;
	
	public Vector particleList;
	
	public  static final int INVALID_INDEX=-1;
	protected static final int PARTICLE_MIN=50;
	protected static final int PARTICLE_MAX=250;

	
	/* Constructors
	 * *************
	 * Same old, same old.
	 */
	public ParticleFilter() {
		this(PARTICLE_MIN, PARTICLE_MAX);
	}
	public ParticleFilter(int minParticles, int maxParticles) {
		if ((minParticles < 0 ) || (maxParticles < minParticles)) {
			throw new IllegalArgumentException("Error: Invalid particle bounds minParticles="+minParticles+" maxParticles="+maxParticles);
		}
		particleList=new Vector();
		this.minParticles = minParticles;
		this.maxParticles = maxParticles;
		reset();
	}
	
	/* reset
	 * *****
	 * Resets the particle filter to its post-construction state.
	 */
	public void reset() {
		if (particleList != null) {
			particleList.removeAllElements();
			particleList = null;
		}
		lastTimestamp = 0L;
		firstUpdate = true;
	}
	
	/* createDefaultSensor/MotionModel
	 * *******************************
	 * These are here basically so they can be overridden by filters who want to.
	 */
	protected SensorModel createDefaultSensorModel() { return null; }
	protected MotionModel createDefaultMotionModel() { return null; }
	
	/* setSensorModel / setMotionModel / getSensorModel / SetSensorModel
	 * *****************************************************************
	 * Basic setters/getters for the sensor & motion models.
	 * These require that your own sensor and motion models subclass off fo SensorModel and MotionModel.
	 */
	public void setSensorModel(SensorModel model) { sensorModel = model; }
	public void setMotionModel(MotionModel model) { motionModel = model; }
	public SensorModel getSensorModel() {
		if (sensorModel==null) sensorModel = createDefaultSensorModel();
		return sensorModel;
	}
	public MotionModel getMotionModel() {
		if (motionModel==null) motionModel = createDefaultMotionModel();
		return motionModel;
	}
	

	/* getParticleList / initializeParticleList / getParticleCount
	 * ***********************************************************
	 * Basic getter/setter methods for the particles running in a filter.
	 */
	public Vector getParticleList() {
		return particleList;
	}
	protected void initializeParticleList(Vector particles) {
		particleList = particles;
	}
	public int getParticleCount() {
		return particleList.size();
	}	
	
	/* applySensorModel
	 * ****************
	 * Applies the sensor model (as set by setSensorModel) to each particle in the filter,
	 *  according to the given observable.
	 */
	protected CDF applySensorModel(Observable obs) {
		int count=particleList.size();
		double accumulate[]=new double[count];
		int index[]=new int[count];
		int mostRecentIndex=INVALID_INDEX;
		double sum=0.0,curr;

		int i=0;
		for (Enumeration it=particleList.elements(); it.hasMoreElements(); i++) {
			Particle p = (Particle)it.nextElement();
			curr = getSensorModel().likelihood(p, obs);
			p.updateWeight(curr);
			sum+=curr;
			accumulate[i]=sum;
			if (curr>0.0) {
				mostRecentIndex=i;
				index[i]=i;
			} else {
				index[i]=mostRecentIndex;
			}
		}
		if (mostRecentIndex==INVALID_INDEX) {
			throw new IllegalArgumentException("Thou art boned: Empty CDF Found: No particle has "+
				"any likelihood! " + i);
		}
		return new CDF(accumulate, index, sum);
	}

	/* applyMotionModel
	 * ****************
	 * Applies the motion model (as set by setMotionModel) to each particle in the filter
	 *  (or to each particle in the given list, in the case of the second implementation).
	 */
	protected void applyMotionModel(long elapsedTimeMillis) {
		applyMotionModel(particleList, elapsedTimeMillis);
	}
	protected void applyMotionModel(Vector particles, long elapsedTimeMillis) {
		for (Enumeration it=particles.elements(); it.hasMoreElements(); ) {
			Object o = it.nextElement();
			Particle p = (Particle)o;
			getMotionModel().move(p, elapsedTimeMillis);
			//System.out.println("     "+p+"--"+p.getVoronoiPosition().getEdge().getId()+", "+p.getVoronoiPosition().getDistance()+" vel: "+p.getVelocity());
		}
	}
	
//	public void visitAll(ParticleFilterVisitor walker) {
//		walker.before(this);
//		for (int i=0; i<particleList.size();++i) {
//			walker.visit(this, i, (Particle)particleList.get(i));
//		}
//		walker.after(this);
//	}

	/* gatherSameNumberOfSamples
	 * *************************
	 * Takes the distribution currently represented by the particles, and resamples this distribution
	 * until 'n' new samples are created, where 'n' is the number of particles currently running in
	 * the filter.  Overwrites the old set of particles with this new set.
	 */
	protected void gatherSameNumberOfSamples(Observable obs) {
		if (particleList.size()==0) {
			throw new IllegalStateException("Can't gather same number of particles when you have none");
		}
		int numClones = particleList.size();
		CDF cdf = applySensorModel(obs);
		particleList = cdf.getClonesOfHeavilyWeightedEntries(CDF.RANDOM, particleList, numClones);
	}

	
	/* runSingleTimeStep
	 * *****************
	 * This is the major function called in updateFilter(), which is itself the major funciton
	 * called by the outside world using the filter.  You should have no reason to override this,
	 * but instead will need to implement runSingleTimeStepImpl, which determines the meat of
	 * what actually happens inside the filter.
	 */
	protected void runSingleTimeStep(long elapsedTimeMillis, Observable obs) {
		Vector newList=runSingleTimeStepImpl(elapsedTimeMillis, obs);
		particleList = newList;	
	}
	
	/* getLastUpdatedTime
	 * ******************
	 * Returns the last time at which updateFilter was called.
	 */
	public long getLastUpdatedTime() {
		return lastTimestamp;
	}
	
	/* updateFilter
	 * ************
	 * This is the major method that will be called repeatedly by an outside client in order to
	 * drive the filter.
	 * Each call runs a single timestep.
	 */
	public void updateFilter(Observable o) {
		
		//System.out.println("Updating filter with observable: "+o.getTimestamp());
		
		// how much time has elapsed since the last update?
		long elapsed = (lastTimestamp==0L ? 0 : o.getTimestamp() - lastTimestamp);
		if (elapsed < 0) {
			elapsed = 0;
		} else {
			lastTimestamp = o.getTimestamp();
		}
		
		// if this is the first update of the filter, we require some initialization
		if (firstUpdate) {
			Vector list = createParticles(o);
			if (list==null) return;
			initializeParticleList(list);
			firstUpdate = false;
		}
		
		// run a single time step (single update)
		runSingleTimeStep(elapsed, o);
	}
	
	/* updateWithoutObservation
	 * ************************
	 * This provides a way of updating the filter even when no new observations have been seen.
	 */
	public void updateWithoutObservation(long elapsedMillis) {
		if (particleList == null) return;
		/* there has been no observation for a while, just apply the motion model */
		applyMotionModel(elapsedMillis);
	}
	
	
	/////////////////////////////////////////////
	////////////ABSTRACT METHODS ////////////////
	/////////////////////////////////////////////
	
	/* createParticles
	 * ***************
	 * Returns an ArrayList, each entry of which is a Particle.
	 * This allows subclasses to determine what kind of particles to create, and where to 
	 *  place them.
	 */
	protected abstract Vector createParticles(Observable o);
	
	/* runSingleTimeStepImpl
	 * *********************
	 * In general, the implementation of this will be some version of:
	 *  1.  apply the motion model (ParticleFilter.applyMotionModel)
	 *  2.  apply the sensor model (ParticleFilter.applySensorModel)
	 *  3.  resample
	 * But it's left abstract because different kinds of particle filters want to track different
	 *  things as these three basic steps are being processed.
	 */
	protected abstract Vector runSingleTimeStepImpl(long elapsedTimeMillis, Observable obs);
	
	
	
	// Test Probe
	public class TEST_PROBE {
		public void setParticleList(Vector _l) {particleList=_l;}
		public CDF applySensorModel(Observable o) { 
			return ParticleFilter.this.applySensorModel(o);}
	}

}