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 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; } }