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) {
		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 drot1 = n(Math.toRadians(ang));
		double pah = Math.toRadians(pa.getHeading() + Math.toRadians(90));
		double pbh = Math.toRadians(pb.getHeading() + Math.toRadians(90));
		
		double dtrans = move;
		
		
		double dx = pa.getX()-pb.getX();
		double dy = pa.getY()-pb.getY();
		
		double dcrot1 = n(Math.atan2(dy, dx) - pbh - Math.toRadians(90));
		double dctrans = Math.sqrt(dx*dx+dy*dy);
		double dcrot2 = n(pah - pbh - dcrot1);
		
		double p1 = probTriangularDistribution(n(pah-pbh-drot1), 0.001*dtrans+0.1*drot1);
		double p2 = probTriangularDistribution(dtrans-dctrans, dtrans+0.001*drot1);
		double p3 = probTriangularDistribution(dcrot2, 0.001*dtrans);
		if (dtrans == 0) p3 = 1.0;
		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() - 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, 20); // desvio padrao
		}
		return prob;
	}
}