particlefiltertracker.java

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

/*
 * Created on Jan 13, 2004
 *
 * Intel research seattle.
 */
package org.placelab.client.tracker;

import java.util.Vector;

import org.placelab.core.BeaconMeasurement;
import org.placelab.core.Measurement;
import org.placelab.core.TwoDCoordinate;
import org.placelab.mapper.Mapper;
import org.placelab.particlefilter.MotionModel;
import org.placelab.particlefilter.VisualizableParticleFilter;
import org.placelab.particlefilter.beacon.BeaconParticleFilter;
import org.placelab.particlefilter.beacon.MeasurementObservable;

/**
 * A Tracker that uses a probabilistic technique to produce Estimates.
 * <p>
 * Sophisticated users could write a motion model and plug it into
 * the BeaconParticleFilter used by this class to produce more accurate
 * results than those produced by the CentroidTracker.
 */
public class ParticleFilterTracker extends BeaconTracker implements VisualizableParticleFilter {
	private BeaconParticleFilter pf;
	private TwoDPositionEstimate estimate;
	
	public ParticleFilterTracker(Mapper m) {
		super(m);
		pf = new BeaconParticleFilter(m);
		estimate = null;
	}
	
	public String toString() {
		return "Place Lab using 802.11";
	}
	
    public Vector getParticleList() {
        return pf.getParticleList();
    }

	public ParticleFilterTracker(Mapper m, double maxX, double maxY, double gridIncr) {
		super(m);
		pf = new BeaconParticleFilter(m,maxX,maxY,gridIncr);
		estimate = null;
	}
	
	protected void resetImpl() {
		pf.reset();
		estimate = null;
	}
	
	public class TEST_PROBE2 {
		public void setMotionModelOfAllParticles(MotionModel m) {
			// XXX: unknown
		}
	}
	
	
	public boolean acceptableMeasurement(Measurement m) {
		return (m instanceof BeaconMeasurement);
	}
	
	protected void updateEstimateImpl(Measurement m) {
		if (!acceptableMeasurement(m)) {
			throw new IllegalArgumentException("Measurement is not acceptable:"+
				m.getClass().getName());
		}
		MeasurementObservable o = new MeasurementObservable(m);
		pf.updateFilter(o);
		recalculateEstimate();
	}

	/** return an estimate based on the last set of measurements we saw * */
	public Estimate getEstimate() {
		if (estimate != null) {
			return estimate;
		} else {
			return new TwoDPositionEstimate(getLastUpdatedTime(), TwoDCoordinate.NULL, 0.0);
		}
	}
	private void recalculateEstimate() {
		estimate = pf.getEstimate();
	}
	
	public BeaconParticleFilter getParticleFilter() { return pf; }
	public void updateWithoutMeasurement(long durationMillis) {
		pf.updateWithoutObservation(durationMillis);
	}
}