MAC0318/HistogramFilter/src/config/Models.java

package config;

import java.util.ArrayList;

import lejos.geom.Line;
import lejos.robotics.mapping.LineMap;
import lejos.robotics.navigation.Pose;
import robots.DataRead;

public class Models {
	LineMap map;
	public Models(LineMap map) {
		this.map = map;
	}
	
    // return pdf(x) = standard Gaussian pdf
    private static double pdf(double x) {
        return Math.exp(-x*x / 2) / Math.sqrt(2 * Math.PI);
    }

    // return pdf(x, mu, signma) = Gaussian pdf with mean mu and stddev sigma
    private static double pdf(double x, double mu, double sigma) {
        return pdf((x - mu) / sigma) / sigma;
    }
	
	/* retorna a probabilidade do robo estar em (ap) sendo que ele saiu de (bp) 
	 * com um movimento de rotacao de ang graus.*/
	public double rotateProbability (Pose pa, Pose pb, double ang) {
//		if (pa.getX() != pb.getX() || pa.getY() != pb.getY()) return 0;
//		double delta = Math.abs(pb.getHeading()+ang-pa.getHeading());
//		if (delta > 180) delta -= 360;
//		return pdf(delta, 0, Math.sqrt(Math.abs(ang*0.1)));
		return motionModelOdometry(pa, pb, 0, ang);
	}
	
	/* retorna a probabilidade do robo estar em (pa) sendo que ele saiu de (pb) 
	 * com sendo que ele foi move centimetros para frente.*/
	public double moveProbability (Pose pa, Pose pb, double move) {

		return motionModelOdometry(pa, pb, move, 0);
	}
	
	private double probTriangularDistribution (double a, double b) {
		double r = Math.sqrt(b*6);
		double abs = Math.abs(a);
		if (abs > r) return 0;
		return (r-abs)/(6*b);
	}
	
	public double motionModelOdometry (Pose pa, Pose pb, double move, double ang) {
		double rot = ang+pa.getHeading();
		
		double dx = pa.getX()-pb.getX();
		double dy = pa.getY()-pb.getY();

		// teorico
		// atan2 retorna entre -180 e 180
		double rott = Math.toDegrees(Math.atan2(dy,dx))+pb.getHeading();
		double transt = dx*dx;
		transt += dy*dy;
		transt = Math.sqrt(transt);
//		System.out.println(rott);

		double p1 =  probTriangularDistribution(rott-rot, rot+0.0001*move);
		double p2 =  probTriangularDistribution(transt-move, 0.01*ang+move);
		return p1*p2;
	}
	
	/* retorna a probabilidade dele estar em (p) sendo que realizou a leitura do
	 *  conjunto data de medidas - pdf */
	public double readProbability (Pose p, ArrayList<DataRead> data) {
		double prob = 1.0;
		for (DataRead dp: data) {
			// os 90 é para a correcao do mapa
			float newang = (float) (p.getHeading() - dp.getSensorAngle() - 90);
			Pose pose = new Pose(p.getX(), p.getY(), newang);
			
			float mindist = Float.POSITIVE_INFINITY;
			int cone = 1; // CONE CONE
			for (int angulo = -cone / 2; angulo <= cone / 2; angulo++) {
				pose.setHeading((float) (newang - angulo));
				float dist = map.range(pose);
				if (dist > 0 && dist < mindist)
					mindist = dist;
			}
			double teoricaldistance = mindist;
			
			if (dp.getDistance() < 4 || dp.getDistance() > 240)
				continue;
			
			if (teoricaldistance == -1) {
				prob = 0;
				break;
			}
			prob *= pdf(dp.getDistance(), teoricaldistance, 40); // desvio padrao
		}
		return prob;
	}
}