defaultsensormodel.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.Coordinate;
import org.placelab.core.Observable;
import org.placelab.core.TwoDCoordinate;
import org.placelab.core.WiFiReading;
import org.placelab.mapper.Beacon;
import org.placelab.mapper.Mapper;
import org.placelab.mapper.WiFiBeacon;
import org.placelab.particlefilter.Particle;
import org.placelab.particlefilter.SensorModel;


public class DefaultSensorModel implements SensorModel {
	private static final double SMALL_CHANCE=0.02;
	private static final double REALLY_LIKELY=0.99;
	private static final double LIKELY=0.65;
	private static final double SOMEWHAT_LIKELY=0.35;
	private static final double UNLIKELY=0.2;
	public static final double REALLY_UNLIKELY=0.02;
	public Mapper mapper;

	public DefaultSensorModel(Mapper m) {
		this.mapper = m;
	}
	
	protected double likelihood(Coordinate c, BeaconReading r, Beacon beacon) {
		// this sensor model only applies to WiFi beacons
		if (! (beacon instanceof WiFiBeacon)) {
			System.out.println("I see " + beacon.getClass().getName());
		    // XXX could put some stuff beacon class so that we can generalise this, if possible...
		    // hard to do this without introducing floats... sigh -- JWS
		    throw new UnsupportedOperationException("DefaultSensorModel does not understand your Beacon type " + beacon.getType());
		}
		double range = ((TwoDCoordinate)beacon.getPosition()).distanceFrom((TwoDCoordinate)c);
		int signalStrength = ((WiFiReading)r).getRssi();
		if (range < 20) {
			if (signalStrength > -75) {
				return REALLY_LIKELY;
			}
			if (signalStrength > -90) {
				return SOMEWHAT_LIKELY;
			}
			return UNLIKELY;
		}
		if (range < 35) {
			if (signalStrength > -60) {
				return SOMEWHAT_LIKELY;
			}
			return LIKELY;
		}
		
		if (range < 65) {
			if (signalStrength > -70) {
				return UNLIKELY;
			}
			return LIKELY;
		}
		if (range < 100) {
			if (signalStrength > -85) {
				return UNLIKELY;
			}
			return SOMEWHAT_LIKELY;
		}
		return SMALL_CHANCE + 10.0/range /* pull in really distant particles */;
	}

	public double likelihood(Particle p, Observable obs) {
			BeaconMeasurement meas = (BeaconMeasurement)((MeasurementObservable)obs).getMeasurement();
			double weightSum=0.0;
			int count=0;
			for (int i=0; i < meas.numberOfReadings(); i++) {
				//if (! (meas.getReading(i) instanceof WiFiReading)) {
				//	throw new UnsupportedOperationException("DefaultSensorModel only understands WiFiReadings");
				//}
	
				BeaconReading reading = meas.getReading(i);
				TwoDCoordinate particlePos = ((PositionParticle)p).getPosition();
	//			TwoDBeacon beacon = BeaconTracker.pickBeacon(mapper.findBeacons(reading.getId()), meas, particlePos);
				Beacon beacon = BeaconTracker.pickBeacon(mapper.findBeacons(reading.getId()), meas, particlePos);
				if (beacon == null) continue;
	
				double weight = likelihood(particlePos, reading, beacon);
				
				weightSum += weight; //###JL Is this the right way to combine weights?
				count++;
			}
			if (count == 0) {
				return 0.5; // Not seeing anything seems reasonable
			} else {
				return weightSum/count;
			}
		}
}