beaconparticlefilter.java

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

/*
 * Created on Jun 18, 2004
 *
 */
package org.placelab.particlefilter.beacon;

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

import org.placelab.client.tracker.BeaconTracker;
import org.placelab.client.tracker.ParticleTwoDEstimate;
import org.placelab.client.tracker.TwoDPositionEstimate;
import org.placelab.core.BeaconMeasurement;
import org.placelab.core.BeaconReading;
import org.placelab.core.Observable;
import org.placelab.core.TwoDCoordinate;
import org.placelab.mapper.Beacon;
import org.placelab.mapper.Mapper;
import org.placelab.particlefilter.KLDParticleFilter;
import org.placelab.particlefilter.MotionModel;
import org.placelab.particlefilter.Particle;
import org.placelab.particlefilter.SensorModel;

/**
 * 
 *
 */
public class BeaconParticleFilter extends KLDParticleFilter {

	protected Mapper mapper;
	protected double maxX, maxY, gridIncrement;
	public static final double MAX_X=1000, MAX_Y=1000, GRID_INCREMENT=50.0;
	
	public BeaconParticleFilter(Mapper m) {
		this(m, MAX_X, MAX_Y, GRID_INCREMENT, PARTICLE_MIN,PARTICLE_MAX);
	}
	public BeaconParticleFilter(Mapper m,int minParticles, int maxParticles) {
		this(m, MAX_X, MAX_Y, GRID_INCREMENT, minParticles,maxParticles);
	}

	public BeaconParticleFilter(Mapper m, double maxX, double maxY, double gridIncr) {
		this(m,maxX,maxY,gridIncr,PARTICLE_MIN,PARTICLE_MAX);
	}
	
	public BeaconParticleFilter(Mapper m, double maxX, double maxY, double gridIncr, 
			int minParticles, int maxParticles) {
		super(minParticles,maxParticles);
		mapper = m;
		this.maxX = maxX;
		this.maxY = maxY;
		this.gridIncrement = gridIncr;
	}
	protected SensorModel createDefaultSensorModel() {
		return new DefaultSensorModel(mapper);
	}
	protected MotionModel createDefaultMotionModel() {
		return new DefaultMotionModel();
	}
	protected Vector createParticles(Observable o) {
		BeaconMeasurement meas = (BeaconMeasurement) ((MeasurementObservable)o).getMeasurement();
		TwoDCoordinate pos = null;
		
		/* find the first reading in the measurement that has a known beacon */
		for (int i=0; i < meas.numberOfReadings(); i++) {
			BeaconReading reading = meas.getReading(i);
			//System.out.println("Mapper="+mapper+",  reading="+reading);
			Beacon beacon = BeaconTracker.pickBeacon(mapper.findBeacons(reading.getId()), meas, null);
			if (beacon != null) {
				pos = (TwoDCoordinate) beacon.getPosition();
				break;
			}
		}
		if (pos==null) return null;
		TwoDCoordinate origin = (TwoDCoordinate)pos.createCloneAndMove(-maxX/2, -maxY/2);
		Vector list = new Vector();
		for (double i=0; i < maxX; i += gridIncrement) {
			for (double j=0; j < maxY;j += gridIncrement) {
				Particle p = new PositionWithMotionParticle(origin.createCloneAndMove(i, j), 0, 0);
				list.addElement(p);
			}
		}
		return list;
	}
	protected int countSupport(double gridSize, Vector particles) {
		Hashtable support=new Hashtable();
		Object UNUSED=new Object();

		if (particles.size() <= 0) return 0;
		TwoDCoordinate origin = ((PositionParticle)particles.elementAt(0)).getPosition();
		for (Enumeration it=particles.elements(); it.hasMoreElements(); ) {
			PositionParticle p = (PositionParticle) it.nextElement();
			TwoDCoordinate pos = p.getPosition();
			double gridCellX = pos.xDistanceFrom(origin)/gridSize;
			double gridCellY = pos.yDistanceFrom(origin)/gridSize;
			double xTrunc = Math.floor(gridCellX);
			double yTrunc = Math.floor(gridCellY);
			String gridCell=""+xTrunc+","+yTrunc;
			support.put(gridCell, UNUSED);
		}
		return support.keySet().size();
	}
	public ParticleTwoDEstimate getEstimate() {
		return ParticleTwoDEstimate.createParticleTwoDEstimate(getLastUpdatedTime(), getParticleList());
	}
	public TwoDPositionEstimate getWeightedEstimate() {
	 Vector particles = getParticleList();
		double totalLat=0.0, totalLon=0.0, totalWeight=0.0;
		
		for (Enumeration it = particles.elements(); it.hasMoreElements(); ) {
			PositionParticle p = (PositionParticle) it.nextElement();
			
			TwoDCoordinate pos = p.getPosition();
			totalLat += pos.getLatitude() * p.getWeight();
			totalLon += pos.getLongitude() * p.getWeight();
			totalWeight += p.getWeight();
		}

		TwoDCoordinate mean = new TwoDCoordinate(totalLat/totalWeight, totalLon/totalWeight);
		double totalDistanceSq = 0.0;
		for (Enumeration it = particles.elements(); it.hasMoreElements(); ) {
			PositionParticle p = (PositionParticle) it.nextElement();
			
			TwoDCoordinate pos = p.getPosition();
			double x = pos.xDistanceFrom(mean), y = pos.yDistanceFrom(mean);
			totalDistanceSq += p.getWeight() * (x*x + y*y);
		}
		double stdDev = Math.sqrt(totalDistanceSq/totalWeight);
		
		return new TwoDPositionEstimate(getLastUpdatedTime(), mean, stdDev);
	}

	public TwoDPositionEstimate getThresholdedWeightedEstimate() {
	 Vector particles = getParticleList();
		double totalLat=0.0, totalLon=0.0, totalWeight=0.0;
		
		for (Enumeration it = particles.elements(); it.hasMoreElements(); ) {
			PositionParticle p = (PositionParticle) it.nextElement();
			
			TwoDCoordinate pos = (TwoDCoordinate)p.getPosition();
			if (p.getWeight() > 0.01) {
				totalLat += pos.getLatitude() *  Math.sqrt(p.getWeight());
				totalLon += pos.getLongitude() * Math.sqrt(p.getWeight());
				totalWeight += Math.sqrt(p.getWeight());
			}
		}
		if (totalWeight == 0) {
			return getEstimate();
		}
		TwoDCoordinate mean = new TwoDCoordinate(totalLat/totalWeight, totalLon/totalWeight);
		double totalDistanceSq = 0.0;
		for (Enumeration it = particles.elements(); it.hasMoreElements(); ) {
			PositionParticle p = (PositionParticle) it.nextElement();
			
			TwoDCoordinate pos = p.getPosition();
			double x = pos.xDistanceFrom(mean), y = pos.yDistanceFrom(mean);
			totalDistanceSq += p.getWeight() * (x*x + y*y);
		}
		double stdDev = Math.sqrt(totalDistanceSq/totalWeight);
		
		return new TwoDPositionEstimate(getLastUpdatedTime(), mean, stdDev);
	}
	
	
	
	
	public class TEST_PROBE extends KLDParticleFilter.TEST_PROBE {
		public int countSupport(double gridSize, Vector particles) {
			return BeaconParticleFilter.this.countSupport(gridSize, particles);
		}
		public void initializeParticles(double startX, double startY, double endX, double endY, double incr) {
			Vector list = new Vector();
			TwoDCoordinate zero = new TwoDCoordinate(0,0);
			for (double i=startX; i < endX; i += incr) {
				for (double j=startY; j < endY;j += incr) {
					Particle p = new PositionWithMotionParticle(zero.createCloneAndMove(i, j), 0, 0);
					list.addElement(p);
				}
			}
			initializeParticleList(list);
		}
		
	}
}