beaconandgpssensormodel.java

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

/*
 * Created on Oct 27, 2004
 *
 * TODO To change the template for this generated file go to
 * Window - Preferences - Java - Code Style - Code Templates
 */
package org.placelab.particlefilter.beacon;

import org.placelab.core.BeaconMeasurement;
import org.placelab.core.GPSMeasurement;
import org.placelab.core.Measurement;
import org.placelab.core.Observable;
import org.placelab.mapper.Mapper;
import org.placelab.particlefilter.Particle;
import org.placelab.particlefilter.SensorModel;

/** Spec for BeaconAndGPSSensorModel
 * 
 * @author jhightower
 */
public class BeaconAndGPSSensorModel implements SensorModel {
	protected Mapper mapper;
	protected DefaultSensorModel beaconModel; // for beacons
	protected GPSSensorModel gpsModel; // for GPS
	
    /** A sensor model for using both beacon and GPS spotters at the same time.  Uses a {@link DefaultSensorModel}
     * and {@link GPSSensorModel} internally.
     */
    public BeaconAndGPSSensorModel(Mapper m) {
        beaconModel = new DefaultSensorModel(m);
        gpsModel = new GPSSensorModel();
    }

    public double likelihood(Particle p, Observable obs) {
        Measurement meas = ((MeasurementObservable)obs).getMeasurement();
        if (meas instanceof BeaconMeasurement) {
//        	System.out.println("Whoo!");
            return beaconModel.likelihood(p, obs);
        } else if (meas instanceof GPSMeasurement) {
            return gpsModel.likelihood(p, obs);
        } else {
            throw new IllegalArgumentException("Sensor model only accepts GPS and Beacn measurements.");
        }
    }

}