beaconandgpsparticlefilter.java

来自「一个基于PlaceLab的室内和室外的智能导航系统」· Java 代码 · 共 81 行

/*
 * Created on Oct 27, 2004
 *
 */
package org.placelab.particlefilter.beacon;

import java.util.Vector;

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

/** Spec for BeaconAndGPSParticleFilter
 * 
 * @author jhightower
 */
public class BeaconAndGPSParticleFilter extends BeaconParticleFilter {

    /**
     * @param m
     */
    public BeaconAndGPSParticleFilter(Mapper m) {
        super(m);
    }

    public BeaconAndGPSParticleFilter(Mapper m, int particleMin, int particleMax) {
        super(m, particleMin, particleMax);
    }

    /**
     * @param m
     * @param maxX
     * @param maxY
     * @param gridIncr
     */
    public BeaconAndGPSParticleFilter(Mapper m, double maxX, double maxY, double gridIncr) {
        super(m, maxX, maxY, gridIncr, PARTICLE_MIN, PARTICLE_MAX);
    }

    /* (non-Javadoc)
     * @see org.placelab.particlefilter.ParticleFilter#createDefaultSensorModel()
     */
    protected SensorModel createDefaultSensorModel() {
        return new BeaconAndGPSSensorModel(mapper);
    }
    /** Construct a new array of particles from either a beacon or GPS measurement.
     */
    protected Vector createParticles(Observable o) {
		Measurement meas = ((MeasurementObservable)o).getMeasurement();

		if (meas instanceof BeaconMeasurement) {
		    return super.createParticles(o);
		} else if (meas instanceof GPSMeasurement) {
		    GPSMeasurement gpsMeas = (GPSMeasurement)meas;

		    /* return null if the GPS does not have a valid lock */
		    if (!gpsMeas.isValid())
		        return null;
		    
			TwoDCoordinate pos = (TwoDCoordinate)gpsMeas.getPosition();
			if (pos==null) return null;
			TwoDCoordinate origin = (TwoDCoordinate)pos.createCloneAndMove(-maxX/2, -maxY/2);
			Vector list = new Vector();
			for (double i=0; i < maxX; i += gridIncrement) {
				for (double j=0; j < maxY;j += gridIncrement) {
					Particle p = new PositionWithMotionParticle(origin.createCloneAndMove(i, j), 0, 0);
					list.addElement(p);
				}
			}
			return list;  
		} else {
		    throw new IllegalArgumentException("Particles can only be created from GPS or Beacon measurements.");
		}
    }
}