ssrrsensormodel.java

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

package org.placelab.particlefilter.beacon;

import java.util.Hashtable;

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.mapper.Beacon;
import org.placelab.mapper.GSMBeacon;
import org.placelab.mapper.Mapper;
import org.placelab.mapper.WiFiBeacon;
import org.placelab.particlefilter.Particle;

public class SSRRSensorModel  extends CentroidSensorModel {
	public static int DISTANCE_BUCKET_SIZE = 5;
	boolean useSignalStrength, useResponseRate;

	public SSRRSensorModel(Mapper m, boolean useSignalStrength, boolean useResponseRate) {
		super(m,0.1);
		this.useSignalStrength = useSignalStrength;
		this.useResponseRate = useResponseRate;
	}
	
	public double likelihood(Particle p, Observable obs) {
		BeaconMeasurement meas = (BeaconMeasurement)((MeasurementObservable)obs).getMeasurement();
				
		double weightMult=1.0; // give a little bonus to last round's winners
		int count=0;
		boolean sawWifi = false;
		TwoDCoordinate particlePos = ((PositionParticle)p).getPosition();
		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++) {
			//if (! (meas.getReading(i) instanceof WiFiReading)) {
			//	throw new UnsupportedOperationException("DefaultSensorModel only understands WiFiReadings");
			//}

			BeaconReading reading = meas.getReading(i);
//			TwoDBeacon beacon = BeaconTracker.pickBeacon(mapper.findBeacons(reading.getId()), meas, particlePos);
			Beacon beacon = BeaconTracker.pickBeacon(mapper.findBeacons(reading.getId()), meas, particlePos);
//			if (sawWifi && (beacon instanceof GSMBeacon)) { // skip GSM when wifi is available
//				continue;
//			}
			if (beacon == null) {
			continue;
	}
			double weight = likelihood(particlePos, reading, beacon);
			
			if (sawWifi && (beacon instanceof GSMBeacon)) { // Bump likelyhood if low
				if (weight < 0.7) {
					weight = 0.7;
				}
			}
			
			
			weightMult *= weight;
			count++;
		}
		double altApprach = super.likelihood(p,obs);
	//	System.out.println("alt is " + altApprach);
		if ((weightMult > altApprach)) {
//			System.out.println("KEEPING " + weightMult + " over " + altApprach);
			((PositionWithMotionParticle)p).state = HybridMotionModel.USE_DEFAULT;			
		} else {
			// fall back on centroid
//			System.out.println("Falling back " + weightMult + " " + altApprach);
			//weightMult = super.likelihood(p,obs);
			weightMult = altApprach;
			((PositionWithMotionParticle)p).state = HybridMotionModel.USE_CENTROID;
		}
		//System.out.println("returning " + weightMult);
		
		return weightMult;
	}

	protected double likelihood(Coordinate c, BeaconReading r, Beacon beacon) {

		Hashtable kv;
		if (beacon instanceof WiFiBeacon) {
			WiFiBeacon wfb = (WiFiBeacon)beacon;
			kv = wfb.getKeyValues();
		} else if (beacon instanceof GSMBeacon) {
			GSMBeacon gfb = (GSMBeacon)beacon;
			kv = gfb.getKeyValues();
		} else {
		    throw new UnsupportedOperationException("BeaconSensorModel does not understand your Beacon type " + beacon.getType());
		}
		
		int bucketSize = getBucketSize(kv);

		if (bucketSize < 1) {
//	    	System.out.println("I couldn't find bucket size!");
	    	return 0;
		}

		int dist = beacon.getPosition().distanceFromInMeters(c);
		int dBucket = ((int)((dist+bucketSize - 0.0001) / bucketSize))*bucketSize;
		
		String s;
		if (beacon instanceof WiFiBeacon) {
			if (r.getNormalizedSignalStrength() < 10) {
				s = "L";
			} else if (r.getNormalizedSignalStrength() < 30) {
				s = "M";
			} else {
				s = "H";
			}
		} else {
			if (r.getNormalizedSignalStrength() < 40) {
				s = "L";
			} else if (r.getNormalizedSignalStrength() < 75) {
				s = "M";
			} else {
				s = "H";
			}
		}
		String tStr = (String)kv.get(s + dBucket);
		double ss = 0.01;
		if (tStr != null) {
			ss = Double.parseDouble(tStr);
		}
		
		tStr = (String)kv.get("B" + dBucket);
		double rr = 0.01;
		if (tStr != null) {
			rr = Double.parseDouble(tStr);
		}
		
		if (useResponseRate && !useSignalStrength) {
			return rr;
		}
		if (!useResponseRate && useSignalStrength) {
			return ss;
		}
		if (useResponseRate && useSignalStrength) {
			return Math.sqrt(ss*rr);
		}
		return 0.001;
	}

	public int getBucketSize(Hashtable keyValues) {
		double[] rv;
		String bucketSizeStr = (String)keyValues.get("BS");
		try {
			return Integer.parseInt(bucketSizeStr);
		} catch (Exception ex) {
			return -1;
		}
	}
	
}