centroidmotionmodel.java

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

package org.placelab.particlefilter.beacon;

import org.placelab.core.TwoDCoordinate;
import org.placelab.particlefilter.MotionModel;
import org.placelab.particlefilter.Particle;

public class CentroidMotionModel implements MotionModel {
	
	public static TwoDCoordinate curCent=null;
	public static int curCnt=-1;
	public static long totAPObs=0;  // maintains a running average of aps seen per reading 
	public static long totCnt=0;
	
	public static int window[];
	public static int windIdx = 0;
	public static double ratio;
	public static double winAvg;
	
	public CentroidMotionModel() {
		window = new int[5];
		for (int i=0; i<window.length; i++) {
			window[i] = 1;
		}
	}
	
	public void move(Particle p, long elapsedMillis) {
		if (curCent == null) {
			return;
		}
		PositionWithMotionParticle particle = (PositionWithMotionParticle) p;
		TwoDCoordinate curPos = particle.getPosition();
		double la1 = curPos.getLatitude();
		double lo1 = curPos.getLongitude();
		double la2 = curCent.getLatitude();
		double lo2 = curCent.getLongitude();
		double la3 = ratio*la2 + (1-ratio)*la1;
		double lo3 = ratio*lo2 + (1-ratio)*lo1;
		particle.setPosition(new TwoDCoordinate(la3,lo3));
		particle.setVelocity(0);
	}

	/**
	 * @param coordinate
	 * @param cnt
	 */
	public static void setNewCentroid(TwoDCoordinate coordinate, int cnt) {
		if (window == null) {
			return;
		}
		curCent = coordinate;
		curCnt = cnt;
		totAPObs += cnt;
		totCnt++;
		windIdx++;
		if (windIdx == window.length) {
			windIdx = 0;
		}
		window[CentroidMotionModel.windIdx] = cnt;
		// move us proportional to how many APs we saw
		// compute average over last few readings
		winAvg = 0;
		for (int i=0; i<window.length; i++) {
			winAvg += window[i];
		}
		winAvg /= window.length;
//		double ratio = curCnt/((totAPObs/(double)totCnt)+curCnt); // 1/4 for 1 AP, 3/6 for 3...
		ratio = curCnt/(CentroidMotionModel.winAvg+CentroidMotionModel.curCnt); // 1/4 for 1 AP, 3/6 for 3...
//		System.out.println("Ratio is " + ratio + " avg is " + winAvg);
	}
}