FOI!!!!!

HistogramFilter/src/DiscretePoint.java

@@ -31,18 +31,18 @@ public class DiscretePoint {
 	
 	/* menor angulo em graus do intervalo */
 	public double minAng () {
-		double ang = 360.0/m.alphad*z+270;
+		double ang = 360.0/m.alphad*z;
 		return ang;
 	}
 	
 	/* maior angulo em graus do intervalo */
 	public double maxAng () {
-		double ang = 360.0/m.alphad*(z+1)+270;
+		double ang = 360.0/m.alphad*(z+1);
 		return ang;
 	}
 	
 	public double midAng () {
-		return (maxAng()+minAng())/2.0;
+		return 360.0/m.alphad*(z+0.5);
 	}
 	
 	public Pose pose() {

HistogramFilter/src/MainProgram.java

@@ -60,19 +60,24 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 		splitPane.setDividerLocation((int) (frame.getHeight() / 2));
 		frame.add(splitPane);
 
-		matriz = new ProbabilityMatriz(map.getBoundingRect(), 5, 7);
-		
-		for (DiscretePoint p: matriz) {
-			System.out.println(p.z);
-			if (p.z == 3 && p.x == 10 && p.y == 10)
-				p.set(1);
-			else
-				p.set(0);
-		}
+		matriz = new ProbabilityMatriz(map.getBoundingRect(), 5, 10);
+//		
+//		for (DiscretePoint p: matriz) {
+////			System.out.println(n(p.midAng()));
+//			if (p.z == 2 && p.x == 5 && p.y == 5)
+//				p.set(1);
+//			else
+//				p.set(0);
+//		}
 		
 		imap.showMatriz(matriz);
 //		showHelp();
 	}
+	
+	private double n (double x) {
+		x = Math.toRadians(x);
+		return Math.toDegrees(Math.atan2(Math.sin(x), Math.cos(x)));
+	}
 
 	private void showHelp() {
 		String text = "1,2,3 - Change global map view\n";

HistogramFilter/src/MapImage.java

@@ -197,7 +197,7 @@ public class MapImage extends JPanel implements MouseWheelListener, MouseListene
     					(int) (-p.bounds().getMinY()*zoom+h-matriz.ssize*zoom),
     					(int) (matriz.ssize*zoom),
     					(int) (matriz.ssize*zoom),
-    					(int) ((p.minAng())%360), // onde comeca
+    					(int) ((p.minAng()+270)%360), // onde comeca
     					(int) ((p.maxAng()-p.minAng())%360) // tamanho
     			);
     		}

HistogramFilter/src/config/Models.java

@@ -26,17 +26,12 @@ public class Models {
 	/* 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);
 	}
 	
@@ -48,22 +43,29 @@ public class Models {
 	}
 	
 	public double motionModelOdometry (Pose pa, Pose pb, double move, double ang) {
-		double rot = ang+pa.getHeading();
+		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();
-
-		// 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;
+		
+		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
@@ -72,7 +74,7 @@ public class Models {
 		double prob = 1.0;
 		for (DataRead dp: data) {
 			// os 90 é para a correcao do mapa
-			float newang = (float) (p.getHeading() - dp.getSensorAngle() - 90);
+			float newang = (float) (p.getHeading() + 270 - dp.getSensorAngle() - 90);
 			Pose pose = new Pose(p.getX(), p.getY(), newang);
 			
 			float mindist = Float.POSITIVE_INFINITY;
@@ -92,7 +94,7 @@ public class Models {
 				prob = 0;
 				break;
 			}
-			prob *= pdf(dp.getDistance(), teoricaldistance, 40); // desvio padrao
+			prob *= pdf(dp.getDistance(), teoricaldistance, 20); // desvio padrao
 		}
 		return prob;
 	}