particletwodestimate.java

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

package org.placelab.client.tracker;

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

import org.placelab.core.TwoDCoordinate;
import org.placelab.particlefilter.beacon.PositionParticle;

public class ParticleTwoDEstimate extends TwoDPositionEstimate {
	Vector particles;

	public static ParticleTwoDEstimate createParticleTwoDEstimate(long timestamp, Vector particles) {
		Vector particleCopies = new Vector(particles == null ? 0 : particles.size());
		double sd;
		TwoDCoordinate mean;
		if ((particles == null) || (particles.size() == 0)) {
			mean = TwoDCoordinate.NULL;
			sd = 0.0;	
		} else {
			double totalLat=0.0, totalLon=0.0;
			int count=0;
			for (Enumeration it = particles.elements(); it.hasMoreElements(); ) {
				PositionParticle p = (PositionParticle) it.nextElement();
				particleCopies.addElement(p);
				TwoDCoordinate pos = (TwoDCoordinate)p.getPosition();
				totalLat += pos.getLatitude();
				totalLon += pos.getLongitude();
				count++;
			}
			mean = new TwoDCoordinate(totalLat/count, totalLon/count);
			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 += x*x + y*y;
			}
			sd = (count > 1 ? Math.sqrt(totalDistanceSq/(count - 1)) : 0.0);
		}
		return new ParticleTwoDEstimate(timestamp,particleCopies, mean,sd);
	}
	
	public Vector getParticles () {
		return particles;
	}
	
	protected ParticleTwoDEstimate(long timestamp, Vector particles, TwoDCoordinate position, double stdDev) {
		super(timestamp,position,stdDev);
		this.particles = particles;
	}
}