gpssensormodel.java

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

/*
 * Created on Oct 26, 2004
 *
 * This is a sensor model for for using GPS units during spotting.
 */
package org.placelab.particlefilter.beacon;

import org.placelab.core.Coordinate;
import org.placelab.core.GPSMeasurement;
import org.placelab.core.Measurement;
import org.placelab.core.Observable;
import org.placelab.core.TwoDCoordinate;
import org.placelab.particlefilter.Gaussian;
import org.placelab.particlefilter.Particle;
import org.placelab.particlefilter.SensorModel;

/** Spec for GPSSensorModel
 * 
 * @author jhightower
 */
public class GPSSensorModel implements SensorModel {

    /**
     * @param m
     */
    public GPSSensorModel() {
    }

    /** Compute the likelihood of the GPSMeasurement as a Gausssian with standard
     * deviation equal to 7 meters.
     */
    public double /* [0.0,1.0] */ likelihood(Particle p, Observable obs) {
        double result = 0.5; // default

        Measurement meas = ((MeasurementObservable)obs).getMeasurement();
        if ((p instanceof PositionParticle) && (meas instanceof GPSMeasurement)) {
			TwoDCoordinate particlePos = ((PositionParticle)p).getPosition();
			GPSMeasurement gm = (GPSMeasurement)meas;
			if (!gm.isLikelyAccurate()) {
				return result; //System.out.println("Hey!!! This is maybe bogus!!");
			}
			Coordinate gpsCoord = gm.getPosition();
			double distance = (double)particlePos.distanceFromInMeters(gpsCoord);
			result = Gaussian.univariate(distance, 0.0, 10.0); // used to be 7, Anthony changed it.
			result += 0.001/Math.max(1.0,distance*distance);
//			System.out.println(distance + " " + result);
        } else {
            throw new IllegalArgumentException("GPSSensorModel must be used with PositionParticles and GPSMeasurements.");
        }

    //    if (result < 0.001) result = 0.001;
        return result;
    }

}