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 {
	
	private static boolean USE_TRIANGULAR = false;
	private static int SONAR_CONE = 14;
	private static double SONAR_SIGMA = 9;
	
	private static double ALPHA1 = 0.01;		// desvio padrao do erro no gito
	private static double ALPHA2 = 0.005;	// desvio padrao do erro qunado gira proporconal ao deslocalmento
	private static double ALPHA3 = 0.1;		// desvio padrao no erro do deslocamento
	private static double ALPHA4 = 0.01;    // desvio padrao no erro do deslocamento baseado em quanto rodou (nao importa para o nosso robo)

	LineMap map;
	public Models(LineMap map) {
		this.map = map;
	}
	
    // return pdf(x, mu, signma) = Gaussian pdf with mean mu and stddev sigma
    private static double pdf(double x, double sigma) {
    		if (USE_TRIANGULAR) {
    			double r = Math.sqrt(sigma*6);
    			double abs = Math.abs(x);
    			if (abs > r) return 0;
    			return (r-abs)/(6*sigma);
    		}
        return Math.exp(-x*x / 2 / sigma) / Math.sqrt(2 * Math.PI * sigma);
    }

	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);
	}
	
	
	/* 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) {
		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);
	}
	
	/* retorna a probabilidade do robo estar em (pa) sendo que ele saiu de (pb) 
	 * sendo que ele rodou ang graus e foi move centimetros para frente.*/
	public double motionModelOdometry (Pose pa, Pose pb, double move, double ang) {
		double drot1 = n(Math.toRadians(ang));
		double pah = Math.toRadians(pa.getHeading());
		double pbh = Math.toRadians(pb.getHeading());
		
		double dtrans = move;
		
		
		double dx = pa.getX()-pb.getX();
		double dy = pa.getY()-pb.getY();
		
		double dcrot1 = n(Math.atan2(dy, dx) - Math.toRadians(90));
		double dctrans = Math.sqrt(dx*dx+dy*dy);
		double dcrot2 = n(pah - dcrot1);
		
		double p1 = probTriangularDistribution(n(pah-pbh-drot1), ALPHA2*dtrans+ALPHA1*drot1);
		double p2 = probTriangularDistribution(dtrans-dctrans, ALPHA3*dtrans+ALPHA4*drot1);
		double p3 = probTriangularDistribution(dcrot2, ALPHA2*dtrans);
		if (dtrans == 0) p3 = 1;
		return p1*p2*p3;
	}
	
	private double n (double x) {
		return Math.atan2(Math.sin(x), Math.cos(x));
	}
	
	/* 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() + 270 + dp.getSensorAngle());
			Pose pose = new Pose(p.getX(), p.getY(), newang);
			
			float mindist = Float.POSITIVE_INFINITY;
			int cone = SONAR_CONE; // 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, SONAR_SIGMA); // desvio padrao
		}
		return prob;
	}
}