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