centroidsensormodel.java

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

package org.placelab.particlefilter.beacon;

import org.placelab.client.tracker.BeaconTracker;
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.GSMBeacon;
import org.placelab.mapper.Mapper;
import org.placelab.mapper.WiFiBeacon;
import org.placelab.particlefilter.Particle;
import org.placelab.particlefilter.SensorModel;


public class CentroidSensorModel implements SensorModel {
	long lastTS = -1;
	TwoDCoordinate curCent = null;
	double fac;
	public Mapper mapper;

	public CentroidSensorModel(Mapper m, double fac) {
		this.fac = fac;
		this.mapper = m;
	}
	
	public double likelihood(Particle p, Observable obs) {
		BeaconMeasurement meas = (BeaconMeasurement)((MeasurementObservable)obs).getMeasurement();
		double totLon = 0.0;
		double totLat = 0.0;
		int cnt=0;
		int weight=0;
		TwoDCoordinate particlePos = ((PositionParticle)p).getPosition();
		boolean sawWifi = false;
		for (int i=0; i < meas.numberOfReadings(); i++) {
			BeaconReading reading = meas.getReading(i);
			Beacon beacon = BeaconTracker.pickBeacon(mapper.findBeacons(reading.getId()), meas, particlePos);
			if (beacon instanceof WiFiBeacon) {
				sawWifi = true;
			}
		}
		for (int i=0; i < meas.numberOfReadings(); i++) {
			BeaconReading reading = meas.getReading(i);
			Beacon beacon = BeaconTracker.pickBeacon(mapper.findBeacons(reading.getId()), meas, particlePos);
			// compute the centroid of these beacons
			if (beacon == null) {
				continue;
			}
			if (sawWifi && (beacon instanceof GSMBeacon)) { // skip GSM when wifi is available
				continue;
			}
			int fact = 1;
			if (beacon instanceof WiFiBeacon) {
				fact *= 10;
			}
			totLat += ((TwoDCoordinate)beacon.getPosition()).getLatitude()*fact;
			totLon += ((TwoDCoordinate)beacon.getPosition()).getLongitude()*fact;
			weight+=fact;
			cnt++;
		}
		if (cnt == 0) {
			return 0.5;
		}
		
		// we want to do this once per measurement, not per particle
		if (obs.getTimestamp() != lastTS) {
			lastTS = obs.getTimestamp();
			curCent = new TwoDCoordinate(totLat/weight,totLon/weight);
			CentroidMotionModel.setNewCentroid(curCent,cnt);
		}
		
		double dist = particlePos.distanceFromInMeters(curCent);
		return Math.pow(fac,cnt); // centriod particles warp to their destination anyway...
	}

}