kldparticlefilter.java

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

package org.placelab.particlefilter;

import java.util.Vector;

import org.placelab.core.Observable;



/**
 * 
 *
 */
public abstract class KLDParticleFilter extends ParticleFilter {

	private KLDFormula kld=null;

	private double gridSize;
	
	private static final double GRID_SIZE=2.0;

	public KLDParticleFilter() {
		this(PARTICLE_MIN, PARTICLE_MAX, GRID_SIZE);
	}
	public KLDParticleFilter(int minParticles, int maxParticles) {
		this(minParticles, maxParticles, GRID_SIZE);
	}
	public KLDParticleFilter(int minParticles, int maxParticles, double gridSize) {
		super(minParticles, maxParticles);
		this.gridSize = gridSize;
	}
	
	public void reset() {
		super.reset();
	}
	
	protected abstract Vector createParticles(Observable o);
	protected abstract int countSupport(double gridSize, Vector particles);
	
	public void setKLD(KLDFormula kld) { this.kld = kld; }
	
	
	public KLDFormula getKLD() { 
		if (kld==null) {
			kld = new KLDFormula();
		}
		return kld;
	}

	protected Vector runSingleTimeStepImpl(long elapsedTimeMillis, Observable obs) {
		int nx=0;
		CDF cdf = applySensorModel(obs);
		//ParticleList result=new ParticleList(),temp=new ParticleList();
		//Particle clones[]=new Particle[PARTICLE_MIN];
		//Particle example=particleList.get(0);
		//cdf.dumpCompressed();
		Vector result=null;
		int numNeeded = minParticles;
		do {
			Vector clones = cdf.getClonesOfHeavilyWeightedEntries(CDF.RANDOM, particleList, numNeeded);
			applyMotionModel(clones, elapsedTimeMillis);

			if (result==null) {
				result = clones;
			} else {
				result.addAll(clones);
			}
			
			if (result.size() < maxParticles) {
				int support = countSupport(gridSize, result);
				nx = getKLD().compute(support);
				//System.out.println("nx "+ nx+" size "+ result.size());
				if (nx > result.size()) {
					if (nx > maxParticles) {
						nx = maxParticles;
					}
					numNeeded = nx-result.size();
				}
			}
				
		} while ((result.size() < nx) && (result.size() < maxParticles));
		return result;
	}
	
	
	public class TEST_PROBE extends ParticleFilter.TEST_PROBE{
		
		public Vector runSingleTimeStepImpl(long elapsedMillis, Observable o)
			{ return KLDParticleFilter.this.runSingleTimeStepImpl(elapsedMillis, o);}
	}

}