Nao sei....

HistogramFilter/.classpath

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<classpath>
+	<classpathentry kind="con" path="org.lejos.nxt.ldt.LEJOS_LIBRARY_CONTAINER/pc"/>
+	<classpathentry kind="src" path="src"/>
+	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.launching.macosx.MacOSXType/1.8"/>
+	<classpathentry kind="output" path="bin"/>
+</classpath>

HistogramFilter/.project

@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>Sonar</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.jdt.core.javabuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.jdt.core.javanature</nature>
+	</natures>
+</projectDescription>

HistogramFilter/.settings/org.eclipse.jdt.core.prefs

@@ -0,0 +1,111 @@
+eclipse.preferences.version=1
+org.eclipse.jdt.core.compiler.annotation.inheritNullAnnotations=disabled
+org.eclipse.jdt.core.compiler.annotation.missingNonNullByDefaultAnnotation=ignore
+org.eclipse.jdt.core.compiler.annotation.nonnull=org.eclipse.jdt.annotation.NonNull
+org.eclipse.jdt.core.compiler.annotation.nonnull.secondary=
+org.eclipse.jdt.core.compiler.annotation.nonnullbydefault=org.eclipse.jdt.annotation.NonNullByDefault
+org.eclipse.jdt.core.compiler.annotation.nonnullbydefault.secondary=
+org.eclipse.jdt.core.compiler.annotation.nullable=org.eclipse.jdt.annotation.Nullable
+org.eclipse.jdt.core.compiler.annotation.nullable.secondary=
+org.eclipse.jdt.core.compiler.annotation.nullanalysis=disabled
+org.eclipse.jdt.core.compiler.codegen.inlineJsrBytecode=enabled
+org.eclipse.jdt.core.compiler.codegen.methodParameters=do not generate
+org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.8
+org.eclipse.jdt.core.compiler.codegen.unusedLocal=preserve
+org.eclipse.jdt.core.compiler.compliance=1.8
+org.eclipse.jdt.core.compiler.debug.lineNumber=generate
+org.eclipse.jdt.core.compiler.debug.localVariable=generate
+org.eclipse.jdt.core.compiler.debug.sourceFile=generate
+org.eclipse.jdt.core.compiler.problem.APILeak=warning
+org.eclipse.jdt.core.compiler.problem.annotationSuperInterface=warning
+org.eclipse.jdt.core.compiler.problem.assertIdentifier=error
+org.eclipse.jdt.core.compiler.problem.autoboxing=ignore
+org.eclipse.jdt.core.compiler.problem.comparingIdentical=warning
+org.eclipse.jdt.core.compiler.problem.deadCode=warning
+org.eclipse.jdt.core.compiler.problem.deprecation=warning
+org.eclipse.jdt.core.compiler.problem.deprecationInDeprecatedCode=disabled
+org.eclipse.jdt.core.compiler.problem.deprecationWhenOverridingDeprecatedMethod=disabled
+org.eclipse.jdt.core.compiler.problem.discouragedReference=warning
+org.eclipse.jdt.core.compiler.problem.emptyStatement=ignore
+org.eclipse.jdt.core.compiler.problem.enumIdentifier=error
+org.eclipse.jdt.core.compiler.problem.explicitlyClosedAutoCloseable=ignore
+org.eclipse.jdt.core.compiler.problem.fallthroughCase=ignore
+org.eclipse.jdt.core.compiler.problem.fatalOptionalError=disabled
+org.eclipse.jdt.core.compiler.problem.fieldHiding=ignore
+org.eclipse.jdt.core.compiler.problem.finalParameterBound=warning
+org.eclipse.jdt.core.compiler.problem.finallyBlockNotCompletingNormally=warning
+org.eclipse.jdt.core.compiler.problem.forbiddenReference=error
+org.eclipse.jdt.core.compiler.problem.hiddenCatchBlock=warning
+org.eclipse.jdt.core.compiler.problem.includeNullInfoFromAsserts=disabled
+org.eclipse.jdt.core.compiler.problem.incompatibleNonInheritedInterfaceMethod=warning
+org.eclipse.jdt.core.compiler.problem.incompleteEnumSwitch=warning
+org.eclipse.jdt.core.compiler.problem.indirectStaticAccess=ignore
+org.eclipse.jdt.core.compiler.problem.localVariableHiding=ignore
+org.eclipse.jdt.core.compiler.problem.methodWithConstructorName=warning
+org.eclipse.jdt.core.compiler.problem.missingDefaultCase=ignore
+org.eclipse.jdt.core.compiler.problem.missingDeprecatedAnnotation=ignore
+org.eclipse.jdt.core.compiler.problem.missingEnumCaseDespiteDefault=disabled
+org.eclipse.jdt.core.compiler.problem.missingHashCodeMethod=ignore
+org.eclipse.jdt.core.compiler.problem.missingOverrideAnnotation=ignore
+org.eclipse.jdt.core.compiler.problem.missingOverrideAnnotationForInterfaceMethodImplementation=enabled
+org.eclipse.jdt.core.compiler.problem.missingSerialVersion=ignore
+org.eclipse.jdt.core.compiler.problem.missingSynchronizedOnInheritedMethod=ignore
+org.eclipse.jdt.core.compiler.problem.noEffectAssignment=warning
+org.eclipse.jdt.core.compiler.problem.noImplicitStringConversion=warning
+org.eclipse.jdt.core.compiler.problem.nonExternalizedStringLiteral=ignore
+org.eclipse.jdt.core.compiler.problem.nonnullParameterAnnotationDropped=warning
+org.eclipse.jdt.core.compiler.problem.nonnullTypeVariableFromLegacyInvocation=warning
+org.eclipse.jdt.core.compiler.problem.nullAnnotationInferenceConflict=error
+org.eclipse.jdt.core.compiler.problem.nullReference=warning
+org.eclipse.jdt.core.compiler.problem.nullSpecViolation=error
+org.eclipse.jdt.core.compiler.problem.nullUncheckedConversion=warning
+org.eclipse.jdt.core.compiler.problem.overridingPackageDefaultMethod=warning
+org.eclipse.jdt.core.compiler.problem.parameterAssignment=ignore
+org.eclipse.jdt.core.compiler.problem.pessimisticNullAnalysisForFreeTypeVariables=warning
+org.eclipse.jdt.core.compiler.problem.possibleAccidentalBooleanAssignment=ignore
+org.eclipse.jdt.core.compiler.problem.potentialNullReference=ignore
+org.eclipse.jdt.core.compiler.problem.potentiallyUnclosedCloseable=ignore
+org.eclipse.jdt.core.compiler.problem.rawTypeReference=warning
+org.eclipse.jdt.core.compiler.problem.redundantNullAnnotation=warning
+org.eclipse.jdt.core.compiler.problem.redundantNullCheck=ignore
+org.eclipse.jdt.core.compiler.problem.redundantSpecificationOfTypeArguments=ignore
+org.eclipse.jdt.core.compiler.problem.redundantSuperinterface=ignore
+org.eclipse.jdt.core.compiler.problem.reportMethodCanBePotentiallyStatic=ignore
+org.eclipse.jdt.core.compiler.problem.reportMethodCanBeStatic=ignore
+org.eclipse.jdt.core.compiler.problem.specialParameterHidingField=disabled
+org.eclipse.jdt.core.compiler.problem.staticAccessReceiver=warning
+org.eclipse.jdt.core.compiler.problem.suppressOptionalErrors=disabled
+org.eclipse.jdt.core.compiler.problem.suppressWarnings=enabled
+org.eclipse.jdt.core.compiler.problem.syntacticNullAnalysisForFields=disabled
+org.eclipse.jdt.core.compiler.problem.syntheticAccessEmulation=ignore
+org.eclipse.jdt.core.compiler.problem.terminalDeprecation=warning
+org.eclipse.jdt.core.compiler.problem.typeParameterHiding=warning
+org.eclipse.jdt.core.compiler.problem.unavoidableGenericTypeProblems=enabled
+org.eclipse.jdt.core.compiler.problem.uncheckedTypeOperation=warning
+org.eclipse.jdt.core.compiler.problem.unclosedCloseable=warning
+org.eclipse.jdt.core.compiler.problem.undocumentedEmptyBlock=ignore
+org.eclipse.jdt.core.compiler.problem.unhandledWarningToken=warning
+org.eclipse.jdt.core.compiler.problem.unlikelyCollectionMethodArgumentType=warning
+org.eclipse.jdt.core.compiler.problem.unlikelyCollectionMethodArgumentTypeStrict=disabled
+org.eclipse.jdt.core.compiler.problem.unlikelyEqualsArgumentType=info
+org.eclipse.jdt.core.compiler.problem.unnecessaryElse=ignore
+org.eclipse.jdt.core.compiler.problem.unnecessaryTypeCheck=ignore
+org.eclipse.jdt.core.compiler.problem.unqualifiedFieldAccess=ignore
+org.eclipse.jdt.core.compiler.problem.unusedDeclaredThrownException=ignore
+org.eclipse.jdt.core.compiler.problem.unusedDeclaredThrownExceptionExemptExceptionAndThrowable=enabled
+org.eclipse.jdt.core.compiler.problem.unusedDeclaredThrownExceptionIncludeDocCommentReference=enabled
+org.eclipse.jdt.core.compiler.problem.unusedDeclaredThrownExceptionWhenOverriding=disabled
+org.eclipse.jdt.core.compiler.problem.unusedExceptionParameter=ignore
+org.eclipse.jdt.core.compiler.problem.unusedImport=warning
+org.eclipse.jdt.core.compiler.problem.unusedLabel=warning
+org.eclipse.jdt.core.compiler.problem.unusedLocal=warning
+org.eclipse.jdt.core.compiler.problem.unusedObjectAllocation=ignore
+org.eclipse.jdt.core.compiler.problem.unusedParameter=ignore
+org.eclipse.jdt.core.compiler.problem.unusedParameterIncludeDocCommentReference=enabled
+org.eclipse.jdt.core.compiler.problem.unusedParameterWhenImplementingAbstract=disabled
+org.eclipse.jdt.core.compiler.problem.unusedParameterWhenOverridingConcrete=disabled
+org.eclipse.jdt.core.compiler.problem.unusedPrivateMember=warning
+org.eclipse.jdt.core.compiler.problem.unusedTypeParameter=ignore
+org.eclipse.jdt.core.compiler.problem.unusedWarningToken=warning
+org.eclipse.jdt.core.compiler.problem.varargsArgumentNeedCast=warning
+org.eclipse.jdt.core.compiler.source=1.8

HistogramFilter/bin/.gitignore

@@ -0,0 +1,2 @@
+/config/
+/robots/

HistogramFilter/src/DiscretePoint.java

@@ -0,0 +1,56 @@
+import lejos.geom.Rectangle;
+import lejos.robotics.navigation.Pose;
+
+/*
+ * Representa um ponto do quadrado da matriz.
+ * Criado somente qunado necessário.
+ */
+public class DiscretePoint {
+	ProbabilityMatriz m;
+	int x, y, z;
+	public DiscretePoint(ProbabilityMatriz m, int x, int y, int z) {
+		this.m = m;
+		this.x =x;
+		this.y = y;
+		this.z = z;
+	}
+	
+	public void set (double v) {
+		m.set(v, x, y, z);
+	} 
+	
+	public double get () {
+		return m.get(x, y, z);
+	}
+	
+	public Rectangle bounds () {
+		float a = (float) (m.bounds.getMinX()+x*m.ssize);
+		float b = (float) (m.bounds.getMinY()+y*m.ssize);
+		return new Rectangle(a, b, (float)m.ssize, (float)m.ssize);
+	}
+	
+	/* menor angulo em graus do intervalo */
+	public double minAng () {
+		double ang = 360.0/m.alphad*z+270;
+		return ang;
+	}
+	
+	/* maior angulo em graus do intervalo */
+	public double maxAng () {
+		double ang = 360.0/m.alphad*(z+1)+270;
+		return ang;
+	}
+	
+	public double midAng () {
+		return (maxAng()+minAng())/2.0;
+	}
+	
+	public Pose pose() {
+		return new Pose(
+				(float) bounds().getCenterX(),
+				(float) bounds().getCenterY(),
+				(float) midAng()
+		);
+	}
+
+}

HistogramFilter/src/MainProgram.java

@@ -0,0 +1,345 @@
+import java.awt.BorderLayout;
+import java.awt.event.KeyEvent;
+import java.awt.event.KeyListener;
+import java.awt.event.WindowEvent;
+import java.awt.event.WindowListener;
+import java.util.ArrayList;
+
+import javax.swing.JFrame;
+import javax.swing.JOptionPane;
+import javax.swing.JPanel;
+import javax.swing.JSplitPane;
+import javax.swing.SwingUtilities;
+
+import config.Map;
+import config.Models;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import robots.BluetoothRobot;
+import robots.DataRead;
+import robots.LCPRobot;
+import robots.Robot;
+import robots.RobotReturn;
+import robots.VirtualRobot;
+
+public class MainProgram extends JPanel implements KeyListener, WindowListener, RobotReturn {
+
+	private MapImage imap;
+	private Robot robot;
+	private ScannerImage scanner;
+	private Models model;
+	private ProbabilityMatriz matriz;
+
+	public static final byte FORWARD = 0;
+	public static final byte STOP = 1;
+	public static final byte EXIT = 2;
+	public static final byte LEFT = 3;
+	public static final byte RIGHT = 4;
+	public static final byte BACKWARD = 5;
+
+	public MainProgram(LineMap map, Robot robot) {
+		this.robot = robot;
+		JFrame frame = new JFrame("Mapa MAC0318");
+
+		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
+		frame.setLayout(new BorderLayout());
+		imap = new MapImage(map);
+		scanner = new ScannerImage();
+		model = new Models(map);
+		// frame.add(this.map);
+		frame.setSize(800, 800);
+		frame.setVisible(true);
+
+		frame.setFocusable(true);
+		frame.requestFocusInWindow();
+		frame.addKeyListener(this);
+		frame.addWindowListener(this);
+
+		JSplitPane splitPane = new JSplitPane(JSplitPane.HORIZONTAL_SPLIT, scanner, imap);
+		splitPane.setOneTouchExpandable(true);
+		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);
+		}
+		
+		imap.showMatriz(matriz);
+//		showHelp();
+	}
+
+	private void showHelp() {
+		String text = "1,2,3 - Change global map view\n";
+		text += "l - Show global map trace.\n";
+		text += "c - Clean maps.\n";
+		text += "m - Enter robot movement.\n";
+		text += "r - Enter robo rotation.\n";
+		text += "a - Colect sonar data.\n";
+		text += "z - Make sonar continuous scanner.\n";
+		text += "i - Stop continuous scanner.\n";
+		text += "g - Save global map image.\n";
+		text += "f - Save scanner image.\n";
+		text += "<arrows> - Move robot.\n";
+		text += "h - help.\n";
+		JOptionPane.showMessageDialog(null, text, "HELP", JOptionPane.PLAIN_MESSAGE);
+	}
+
+	public void addPoint(Pose p) {
+		imap.addPoint(p);
+	}
+
+	@Override
+	public void keyPressed(KeyEvent e) {
+
+		char input = e.getKeyChar();
+		switch (input) {
+		case '1':
+			imap.setVisual(0);
+			break;
+		case '2':
+			imap.setVisual(1);
+			break;
+		case '3':
+			imap.setVisual(2);
+			break;
+		case 'l':
+			imap.showLine();
+			break;
+		case 'g':
+			imap.save();
+			break;
+		case 'f':
+			scanner.save();
+			break;
+		case 'c':
+			imap.clean();
+			scanner.clean();
+			break;
+		case 'm':
+			moveRobot();
+			break;
+		case 'r':
+			rotateRobot();
+			break;
+		case 'a':
+			colectSonar();
+			break;
+		case 'h':
+			showHelp();
+			break;
+		case 's':
+			// reseta a contagem
+			System.out.println("resetou");
+			matriz.setAll(1);
+			matriz.normalize();
+			imap.repaint();
+			break;
+		default:
+			break;
+		}
+
+		if (robot == null)
+			return;
+
+		switch (e.getKeyCode()) {
+		case KeyEvent.VK_UP:
+			robot.moveForward();
+			scanner.clean();
+			break;
+		case KeyEvent.VK_DOWN:
+			robot.moveBackward();
+			scanner.clean();
+			break;
+		case KeyEvent.VK_LEFT:
+			robot.moveLeft();
+			scanner.clean();
+			break;
+		case KeyEvent.VK_RIGHT:
+			robot.moveRight();
+			scanner.clean();
+			break;
+		}
+	}
+
+	private void colectSonar() {
+		int interval;
+		try {
+			String rs = JOptionPane.showInputDialog("Interval (degress):");
+			interval = Integer.parseInt(rs);
+		} catch (Exception e) {
+			return;
+		}
+		ArrayList<DataRead> data = robot.scann(-90, 90, interval);
+		if (data == null) return;
+
+		/* PROBABILIDADES DEPOIS LEITURA */
+		for (DiscretePoint p: matriz) {
+			Pose pose = p.pose();
+			double prob = model.readProbability(pose, data);
+			p.set(prob*p.get());
+		}
+		matriz.normalize();
+		imap.setMatriz(matriz);
+		
+		for (DataRead d : data) {
+			robotData(d);
+		}
+		
+		imap.repaint();
+	}
+
+	private void rotateRobot() {
+		try {
+			String rs = JOptionPane.showInputDialog("Enter rotation (degress-clockwise):");
+			double r = Double.parseDouble(rs);
+			robot.rotate(r);
+			
+			/* PROBABILIDADES DEPOIS MOVIMENTO */
+			ProbabilityMatriz newmatriz = new ProbabilityMatriz(matriz.bounds, matriz.ssize, matriz.alphad);
+			System.out.print("Iniciando...");
+			for (DiscretePoint newp: newmatriz) {
+				double sum = 0;
+				for (DiscretePoint p: matriz) {
+					Pose pa = p.pose();
+					Pose pb = newp.pose();
+					sum += model.rotateProbability(pa, pb, r)*p.get();
+				}
+				newp.set(sum);
+			}
+			System.out.println("Finalizado");
+			matriz = newmatriz;
+			imap.setMatriz(matriz);
+			matriz.normalize();
+
+			scanner.clean();
+			imap.repaint();
+		} catch (Exception e) {
+		}
+	}
+
+	private void moveRobot() {
+		try {
+			String rs = JOptionPane.showInputDialog("Enter distance (cm):");
+			double r = Double.parseDouble(rs);
+			robot.move(r);
+			
+			/* PROBABILIDADES DEPOIS MOVIMENTO */
+			ProbabilityMatriz newmatriz = new ProbabilityMatriz(matriz.bounds, matriz.ssize, matriz.alphad);
+			System.out.print("Iniciando...");
+			for (DiscretePoint newp: newmatriz) {
+				double sum = 0;
+				for (DiscretePoint p: matriz) {
+					Pose pa = p.pose();
+					Pose pb = newp.pose();
+					sum += model.moveProbability(pa, pb, r)*p.get();
+				}
+				newp.set(sum);
+			}
+			System.out.println("Finalizado");
+			matriz = newmatriz;
+			matriz.normalize();
+			imap.setMatriz(matriz);
+	
+			scanner.clean();
+			imap.repaint();
+		} catch (Exception e) {
+		}
+	}
+
+	@Override
+	public void keyReleased(KeyEvent arg0) {
+		if (robot == null)
+			return;
+		robot.stop();
+	}
+
+	@Override
+	public void keyTyped(KeyEvent arg0) {
+	}
+
+	@Override
+	public void windowOpened(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowClosing(WindowEvent e) {
+		System.err.println("Fechando...");
+		if (robot == null)
+			return;
+		robot.disconnect();
+
+	}
+
+	@Override
+	public void windowClosed(WindowEvent e) {
+	}
+
+	@Override
+	public void windowIconified(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowDeiconified(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowActivated(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowDeactivated(WindowEvent e) {
+	}
+
+	@Override
+	public void robotData(DataRead data) {
+		// posicao do robo
+		System.out.print("   sang:"+data.getSensorAngle());
+		System.out.println("   distance:"+data.getDistance());
+		scanner.addRead(data.getDistance(), data.getSensorAngle()-90);
+	}
+
+	public static void main(String[] args) {
+
+		LineMap map = Map.makeMap();
+		Robot robotv = new VirtualRobot(map);
+		Robot robotbt = new BluetoothRobot(null);
+		String s = "LCPRobot";
+
+		Object[] possibleValues = { robotv, robotbt, s };
+		Object robottmp = JOptionPane.showInputDialog(null, "Choose one", "Input", JOptionPane.PLAIN_MESSAGE, null,
+				possibleValues, possibleValues[0]);
+		Robot robot;
+		
+		
+		boolean result = false;
+		if (robottmp == s) {
+			robot = new LCPRobot();
+		} else {
+			robot = (Robot) robottmp;
+		}
+
+		if (robot != null)
+			result = ((Robot) robot).connect();
+
+		if (result == false) {
+			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
+			System.exit(ERROR);
+		}
+
+		SwingUtilities.invokeLater(new Runnable() {
+			public void run() {
+				new MainProgram(map, (Robot) robot);
+			}
+		});
+	}
+}

HistogramFilter/src/MapImage.java

@@ -0,0 +1,337 @@
+import java.awt.Color;
+import java.awt.Graphics;
+import java.awt.Point;
+import java.awt.event.MouseEvent;
+import java.awt.event.MouseListener;
+import java.awt.event.MouseMotionListener;
+import java.awt.event.MouseWheelEvent;
+import java.awt.event.MouseWheelListener;
+import java.awt.image.BufferedImage;
+import java.io.File;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.concurrent.Semaphore;
+
+import javax.imageio.ImageIO;
+import javax.swing.JOptionPane;
+import javax.swing.JPanel;
+
+import lejos.geom.Line;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class MapImage extends JPanel implements MouseWheelListener, MouseListener, MouseMotionListener  {
+    private double zoom = 2.0; // pixel per cm
+    private double grid = 10.0; // cm
+    private double centerx = 0.0;
+    private double centery = 0.0; // cm
+    private Point mousePt;
+    private ArrayList<Pose> lista_pontos;
+    private ArrayList<Pose> lista_ultra;
+    private int visual_method = 0;
+    private boolean line = false;
+    
+    private Semaphore semaphore;
+    
+    private LineMap map;
+	private ProbabilityMatriz matriz;
+    
+	public MapImage() {
+		super();
+		semaphore = new Semaphore(1);
+		lista_pontos = new ArrayList<Pose>();
+		lista_ultra = new ArrayList<Pose>();
+		setBackground(Color.BLACK);
+		addMouseWheelListener(this);
+		addMouseListener(this);
+		addMouseMotionListener(this);
+	}
+	
+	public MapImage(LineMap map) {
+		this();
+		this.map = map;
+	}
+	
+    private void drawModel (Graphics g) {
+        int width = (int) (getWidth()+2*centerx);
+        int height = (int) (getHeight()+2*centery);
+        int count = 0;
+        int x_tmp = 0, y_tmp = 0;
+        
+	    	for (Pose p : lista_pontos) {
+			double hading = Math.toRadians(p.getHeading());
+				
+	    		int x = width/2+(int)(p.getX()*zoom);
+	    		int y = height/2+(int)(p.getY()*zoom)*-1;
+	    		
+			if (visual_method == 0) {
+				g.setColor(Color.getHSBColor((float) (hading/(2.0*Math.PI)), 1, 1));
+				g.fillOval(
+						x-(int)(zoom/2.0*1.5),
+						y-(int)(zoom/2.0*1.5),
+						(int)(zoom*1.5),
+						(int)(zoom*1.5)
+				);
+			} else if (visual_method == 1) {
+	            g.setColor(Color.WHITE);
+	            g.drawLine(
+	                width/2+(int)(p.getX()*zoom),
+	                height/2-(int)(p.getY()*zoom), 
+	                width/2+(int)(p.getX()*zoom+Math.sin(hading)*zoom),
+	                height/2-(int)(p.getY()*zoom-Math.cos(hading)*zoom)
+	            );
+	
+	           g.drawLine(
+	                width/2+(int)(p.getX()*zoom+zoom*Math.sin(hading)),
+	                height/2-(int)(p.getY()*zoom-zoom*Math.cos(hading)),
+	                width/2+(int)(p.getX()*zoom+0.6*zoom*Math.sin(Math.PI/8+hading)),
+	                height/2-(int)(p.getY()*zoom-0.6*zoom*Math.cos(Math.PI/8+hading))
+	            );
+			} else if (visual_method == 2) {
+				g.setColor(Color.WHITE);
+				g.fillOval(
+						x-(int)(zoom/2.0*5),
+						y-(int)(zoom/2.0*5),
+						(int)(zoom*5),
+						(int)(zoom*5)
+				);
+	            g.setColor(Color.BLACK);
+	            g.drawLine(
+	                width/2+(int)(p.getX()*zoom),
+	                height/2-(int)(p.getY()*zoom), 
+	                width/2+(int)(p.getX()*zoom+Math.sin(hading)*zoom*2.5),
+	                height/2-(int)(p.getY()*zoom-Math.cos(hading)*zoom*2.5)
+	            );
+			}
+	
+		    	if (line && count != 0) {
+		    		g.setColor(Color.LIGHT_GRAY);
+		    		g.drawLine(x_tmp, y_tmp, x, y);
+		    	}
+	
+		    	x_tmp = x;
+		    	y_tmp = y;
+		    	count++;
+		}
+	    	
+	    	g.setColor(Color.RED);
+	    	for (Pose p : lista_ultra) {
+	    		int x = width/2+(int)(p.getX()*zoom);
+	    		int y = height/2+(int)(p.getY()*zoom)*-1;
+	    		g.fillRect(
+					x-(int)(zoom/2.0*1.0),
+					y-(int)(zoom/2.0*1.0),
+					(int)(zoom*1.0),
+					(int)(zoom*1.0)
+			);
+	    	}
+	    	
+	    	if (map != null) {
+	    		Line[] lines = map.getLines();
+	    		for (int i = 0; i < lines.length; i++) {
+	    			Line l = lines[i];
+	            g.drawLine(
+		                width/2+(int)(l.x1*zoom),
+		                height/2-(int)(l.y1*zoom), 
+		                width/2+(int)(l.x2*zoom),
+		                height/2-(int)(l.y2*zoom)
+		        );
+	    		}
+	    	}
+    }
+    
+    @Override
+    protected void paintComponent(Graphics g) {
+        int width = (int) (getWidth());
+        int height = (int) (getHeight());
+        int width2 = (int) (getWidth()+2*centerx);
+        int height2 = (int) (getHeight()+2*centery);
+        super.paintComponent(g);
+    
+        g.setColor(new Color(20, 20, 20));
+        
+        int initial_x = height2/2;
+        while (initial_x < width) {
+        	initial_x += grid*zoom;
+        	g.drawLine(0, initial_x, width, initial_x); 
+        }
+        initial_x = height2/2;
+        while (initial_x > 0) {
+        	initial_x -= grid*zoom;
+        	g.drawLine(0, initial_x, width, initial_x); 
+        }
+        int initial_y = width2/2;
+        while (initial_y < width) {
+        	initial_y += grid*zoom;
+            g.drawLine(initial_y, 0, initial_y, height);
+        }
+        initial_y = width2/2;
+        while (initial_y > 0) {
+        	initial_y -= grid*zoom;
+            g.drawLine(initial_y, 0, initial_y, height);
+        }
+
+        g.setColor(Color.ORANGE);
+        g.drawLine(width2/2, 0, width2/2, height);
+        g.drawLine(0, height2/2, width, height2/2);
+        
+        drawMatriz(g);
+
+        if (semaphore.tryAcquire()) {
+	        drawMatriz(g);
+	        drawModel(g);
+	        semaphore.release();
+		}
+    }
+    
+    private void drawMatriz(Graphics g) {
+    		if (matriz == null) return;
+        double w = getWidth()/2.0+centerx;
+        double h = getHeight()/2.0+centery;
+        double max = matriz.max();
+    		for (DiscretePoint p: matriz) {
+    			
+        		g.setColor(new Color(0.31f, 0.58f, 0.80f, (float)(p.get()/max)));
+    			g.fillArc(
+    					(int) (p.bounds().getMinX()*zoom+w),
+    					(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.maxAng()-p.minAng())%360) // tamanho
+    			);
+    		}
+	}
+
+	/**
+     * Adiciona um ponto ao mapa
+     * @param p ponto
+     */
+    public void addPoint(Pose p) {
+    		if (semaphore.tryAcquire()) {
+    			lista_pontos.clear();
+    			lista_pontos.add(p);
+	        semaphore.release();
+		}
+    		repaint();
+	}
+
+    public void addPoint(float x, float y, float z) {
+		if (semaphore.tryAcquire()) {
+			lista_pontos.add(new Pose(x, y, z));
+	        semaphore.release();
+		}
+    		repaint();
+	}
+    
+
+    public void addRead(float x, float y) {
+		if (semaphore.tryAcquire()) {
+			lista_ultra.add(new Pose(x, y, 0));
+	        semaphore.release();
+		}
+    		repaint();
+	}
+    
+
+    public void addPoint(double x, double y, double z) {
+    		addPoint((float)x, (float)y, (float)z);
+	}
+    
+
+    public void addRead(double x, double y) {
+    		addRead((float)x, (float)y);
+	}
+    
+    
+    public void showLine () {
+    		line = !line;
+    		repaint();
+    }
+
+    public void setVisual (int method) {
+    		visual_method = method;
+    		repaint();
+    }
+    
+    public void save () {
+	    	Integer name = new Integer((int) (Math.random()*1000000));
+	    	BufferedImage imagebuf = new BufferedImage(getWidth(), getHeight(), BufferedImage.TYPE_INT_RGB);
+	    	Graphics g = imagebuf.createGraphics();
+	    	g.fillRect(0, 0, imagebuf.getWidth(), imagebuf.getHeight());
+	    	print(g);
+	    	try {
+			ImageIO.write(imagebuf, "png",  new File(name.toString()+".png"));
+			JOptionPane.showMessageDialog(null, "Image saved.");
+		} catch (IOException e) {
+			e.printStackTrace();
+			JOptionPane.showMessageDialog(null, "Image not saved.");
+		}
+    }
+    
+	public void clean() {
+		if (semaphore.tryAcquire()) {
+			lista_pontos.clear();
+			lista_ultra.clear();
+	        semaphore.release();
+		}
+		repaint();
+	}
+	@Override
+	public void mouseDragged(MouseEvent e) {
+		centerx += e.getX() - mousePt.x;
+		centery += e.getY() - mousePt.y;
+		mousePt = e.getPoint();
+		repaint();
+		
+	}
+	@Override
+	public void mouseMoved(MouseEvent e) {		
+	}
+	
+	@Override
+	public void mouseClicked(MouseEvent e) {		
+	}
+	
+	@Override
+	public void mousePressed(MouseEvent e) {
+		mousePt = e.getPoint();
+		repaint();
+		
+	}
+	@Override
+	public void mouseReleased(MouseEvent e) {		
+	}
+	
+	@Override
+	public void mouseEntered(MouseEvent e) {
+	}
+	
+	@Override
+	public void mouseExited(MouseEvent e) {	
+	}
+	
+	@Override
+	public void mouseWheelMoved(MouseWheelEvent e) {
+		if(e.getWheelRotation()<0){
+			if (zoom < 15.0)
+				zoom *= 1.1;
+			repaint();
+		}
+		//Zoom out
+		if(e.getWheelRotation()>0){
+			if (zoom > 1.0)
+				zoom /= 1.1;
+			repaint();
+		}
+	}
+
+	public void showMatriz(ProbabilityMatriz matriz) {
+		setMatriz(matriz);
+		repaint();
+	}
+
+	public void setMatriz(ProbabilityMatriz matriz) {
+		this.matriz = matriz;
+	}
+}

HistogramFilter/src/ProbabilityMatriz.java

@@ -0,0 +1,100 @@
+import java.util.Iterator;
+
+import lejos.geom.Rectangle;
+
+public class ProbabilityMatriz implements Iterable<DiscretePoint> {
+	/*
+	 * Recebe as bordas dos triangulos da matriz (bounds)
+	 * ssize - squere size, tamanho do quadrado utilizado na discretizacao
+	 * alphad - em quantas partes sera dividido os angulos
+	 */
+	Rectangle bounds;
+	double[][][] matriz;
+	double ssize;
+	int alphad;
+	long totalsize;
+	ProbabilityMatriz (Rectangle bounds, double ssize, int alphad) {
+		this.bounds = bounds;
+		this.ssize = ssize;
+		this.alphad = alphad;
+		int x = (int) Math.ceil(bounds.getWidth()/ssize);
+		int y = (int) Math.ceil(bounds.getHeight()/ssize);
+		matriz = new double[x][y][alphad];
+		totalsize = matriz.length*matriz[0].length*matriz[0][0].length;
+		setAll(1);
+		normalize();
+	}
+	
+
+	/*
+	 * Retorna a probabilidade maxima
+	 */
+	double max () {
+		double max = 0;
+		for (DiscretePoint p: this)
+			max = Math.max(max, p.get());
+		return max;
+	}
+	
+	/*
+	 * adiciona a todos
+	 */
+	void setAll (double v) {
+		for (DiscretePoint p: this)
+			p.set(v);
+	}
+
+	/*
+	 * Retorna a soma de todos o pontos
+	 */
+	double sum () {
+		double x = 0;
+		for (DiscretePoint p: this)
+			x += p.get();
+		return x;
+	}
+	
+	/*
+	 * Faz a soma ser 1
+	 */
+	void normalize () {
+		double s = sum();
+		for (DiscretePoint p: this)
+			p.set(p.get()/s);
+	}
+
+	@Override
+	public Iterator<DiscretePoint> iterator() {
+		ProbabilityMatriz m = this;
+		return new Iterator<DiscretePoint>() {
+			long count = 0;
+			
+			@Override
+			public boolean hasNext() {
+				return count != totalsize;
+			}
+
+			@Override
+			public DiscretePoint next() {
+				int b = matriz[0].length;
+				int c = matriz[0][0].length;
+				
+				int x = (int) (count/(b*c));
+				int y = (int) (count%(c*b)/c);
+				int z = (int) (count%c);
+				DiscretePoint tmp = new DiscretePoint(m, x, y, z);
+				count++;
+				return tmp;
+			}
+			
+		};
+	}
+
+	public void set(double v, int x, int y, int z) {
+		matriz[x][y][z] = v;
+	}
+
+	public double get(int x, int y, int z) {
+		return matriz[x][y][z];
+	}
+}

HistogramFilter/src/ScannerImage.java

@@ -0,0 +1,121 @@
+import java.awt.Color;
+import java.awt.Graphics;
+import java.awt.image.BufferedImage;
+import java.io.File;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.concurrent.Semaphore;
+
+import javax.imageio.ImageIO;
+import javax.swing.JOptionPane;
+import javax.swing.JPanel;
+
+public class ScannerImage extends JPanel {
+	private ArrayList<SonarRead> lista_leituras;
+	private Semaphore semaphore;
+
+	class SonarRead {
+		public double distance;
+		public double ang;
+		public double expected;
+
+		SonarRead(double distance, double ang) {
+			this.ang = ang;
+			this.distance = distance;
+		}
+	}
+
+	@Override
+	protected void paintComponent(Graphics g) {
+		int w = this.getWidth() / 2;
+		int h = this.getHeight();
+		super.paintComponent(g);
+		int distance = h * 2 / 25;
+		g.setColor(new Color(0f, 0f, 0f, 0.4f));
+		for (int i = 1; i <= 18; i++) {
+			int r = distance * i;
+			g.drawArc(w - r, h - r, 2 * r, 2 * r, 0, 180);
+			g.drawString(new Integer(i * 20).toString(), w - 7, h - r);
+		}
+		g.setColor(new Color(0f, 0f, 0f, 0.5f));
+		for (int i = 1; i < 6; i++) {
+			int lw = (int) (Math.cos(Math.PI / 6.0 * i) * distance * 18);
+			int lh = (int) (Math.sin(Math.PI / 6.0 * i) * distance * 18);
+			g.drawLine(w, h, lw + w, h - lh);
+		}
+		g.setColor(new Color(0f, 0f, 0f, 0.1f));
+		for (int i = 1; i < 12; i++) {
+			int lw = (int) (Math.cos(Math.PI / 12.0 * i) * distance * 18);
+			int lh = (int) (Math.sin(Math.PI / 12.0 * i) * distance * 18);
+			g.drawLine(w, h, lw + w, h - lh);
+		}
+
+		if (semaphore.tryAcquire()) {
+			drawDots(g, w, h);
+			semaphore.release();
+		}
+	}
+
+	private void drawDots(Graphics g, int w, int h) {
+		int oval_size = 16;
+		int distance = h * 2 / 25;
+		double d = distance / 20.0;
+		double a = -oval_size / 4;
+		double x, y;
+
+		g.setColor(new Color(1f, 0f, 0f));
+		for (SonarRead r : lista_leituras) {
+			x = a + (d * r.distance) * Math.sin(Math.toRadians(r.ang));
+			y = a + (d * r.distance) * Math.cos(Math.toRadians(r.ang));
+			x = w - x;
+			y = h - y;
+			g.setColor(new Color(1f, 0f, 0f));
+			g.fillOval((int) (x - oval_size / 2.0), (int) (y - oval_size / 2.0), oval_size / 2, oval_size / 2);
+
+			x = a + (d * r.expected) * Math.sin(Math.toRadians(r.ang));
+			y = a + (d * r.expected) * Math.cos(Math.toRadians(r.ang));
+			x = w - x;
+			y = h - y;
+			g.setColor(new Color(0f, 0f, 1f));
+			g.fillOval((int) (x - oval_size / 2.0), (int) (y - oval_size / 2.0), oval_size / 2, oval_size / 2);
+		}
+	}
+
+	
+	public void addRead(double distance, double ang) {
+		if (semaphore.tryAcquire()) {
+			lista_leituras.add(new SonarRead(distance, ang + 90));
+			semaphore.release();
+		}
+		repaint();
+	}
+
+	public ScannerImage() {
+		semaphore = new Semaphore(1);
+		lista_leituras = new ArrayList<SonarRead>();
+	}
+
+	public void clean() {
+		if (semaphore.tryAcquire()) {
+			lista_leituras.clear();
+			semaphore.release();
+		}
+		repaint();
+	}
+
+    public void save () {
+	    	Integer name = new Integer((int) (Math.random()*1000000));
+	    	BufferedImage imagebuf = new BufferedImage(getWidth(), getHeight(), BufferedImage.TYPE_INT_RGB);
+	    	Graphics g = imagebuf.createGraphics();
+	    	g.fillRect(0, 0, imagebuf.getWidth(), imagebuf.getHeight());
+	    	print(g);
+	    	try {
+			ImageIO.write(imagebuf, "png",  new File(name.toString()+".png"));
+			JOptionPane.showMessageDialog(null, "Image saved.");
+		} catch (IOException e) {
+			e.printStackTrace();
+			JOptionPane.showMessageDialog(null, "Image not saved.");
+		}
+	}
+
+}

HistogramFilter/src/config/Map.java

@@ -0,0 +1,59 @@
+package config;
+
+import lejos.geom.Line;
+import lejos.geom.Rectangle;
+import lejos.robotics.mapping.LineMap;
+
+public class Map {
+	public static LineMap makeMap () {
+//		Line[] lines1 = {
+//			/* L-shape polygon */
+//			new Line(164,356,58,600),
+//			new Line(58,600,396,721),
+//			new Line(396,721,455,600),
+//			new Line(455,600,227,515),
+//			new Line(227,515,280,399),
+//			new Line(280,399,164,356),
+//			/* Triangle */
+//			new Line(778,526,1079,748),
+//			new Line(1079,748,1063,436),
+//			new Line(1063,436,778,526),
+//			/* Pentagon */
+//			new Line(503,76,333,267),
+//			new Line(333,267,481,452),
+//			new Line(481,452,730,409),
+//			new Line(730,409,704,150),
+//			new Line(704,150,503,76)
+//		};
+		// Line[] lines2 = {
+		// 	/* L-shape polygon */
+		// 	new Line(16.4f,35.6f,5.8f,60.0f),
+		// 	new Line(5.8f,60.0f,39.6f,72.1f),
+		// 	new Line(39.6f,72.1f,45.5f,60.0f),
+		// 	new Line(45.5f,60.0f,22.7f,51.5f),
+		// 	new Line(22.7f,51.5f,28.0f,39.9f),
+		// 	new Line(28.0f,39.9f,16.4f,35.6f),
+		// 	/* Triangle */
+		// 	new Line(77.8f,52.6f,107.9f,74.8f),
+		// 	new Line(107.9f,74.8f,106.3f,43.6f),
+		// 	new Line(106.3f,43.6f,77.8f,52.6f),
+		// 	/* Pentagon */
+		// 	new Line(50.3f,7.6f,33.3f,26.7f),
+		// 	new Line(33.3f,26.7f,48.1f,45.2f),
+		// 	new Line(48.1f,45.2f,73.0f,40.9f),
+		// 	new Line(73.0f,40.9f,70.4f,15.0f),
+		// 	new Line(70.4f,15.0f,50.3f,7.6f)
+		// };
+		Line[] lines2 = {
+			/* L-shape polygon */
+			new Line(0f, 0f, 155f, 0f),
+			new Line(155f, 0f, 110f, 135f),
+			new Line(110f, 135f, 0f, 132f),
+			new Line(0f, 132f, 0f, 0f)
+		};
+		Rectangle bounds = new Rectangle(0, 0, 160, 160);
+		LineMap  map = new LineMap(lines2, bounds);
+
+		return map;
+	}
+}

HistogramFilter/src/config/Models.java

@@ -0,0 +1,99 @@
+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;
+	}
+}

HistogramFilter/src/robots/BluetoothRobot.java

@@ -0,0 +1,339 @@
+package robots;
+
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.concurrent.Semaphore;
+
+import lejos.pc.comm.NXTComm;
+import lejos.pc.comm.NXTCommException;
+import lejos.pc.comm.NXTCommFactory;
+import lejos.pc.comm.NXTInfo;
+import lejos.robotics.navigation.Pose;
+
+public class BluetoothRobot implements Robot {
+	private String name;
+	private NXTComm nxtComm;
+	private RobotReturn rr;
+	private Semaphore readm;
+	private Semaphore sendm;
+	
+	private int lastcmd = -1;
+	
+	public static final byte FORWARD = 0;
+	public static final byte STOP = 1;
+	public static final byte EXIT = 2;
+	public static final byte LEFT = 3;
+	public static final byte RIGHT = 4;
+	public static final byte BACKWARD = 5;
+	
+	public static final byte STOPSCANN = 6;
+	public static final byte STARTSCANN = 7;
+	public static final byte MOVE = 8;
+	public static final byte ROTATE = 9;
+	public static final byte SETPOSE = 10;
+	public static final byte SETSCANANGLE = 11;
+	
+	public static final byte TYPE_INT = 1;
+	public static final byte TYPE_CMD = 2;
+	public static final byte TYPE_POSE = 3;
+	public static final byte TYPE_FLOAT = 4;
+	
+	private static final int scannangle = 5;
+
+	private DataOutputStream output;
+	private DataInputStream input;
+	private Receiver receivethread;
+	private Sender sendthread;
+	
+	private ArrayList<DataRead> reads = null;
+	private ArrayList<SendData> tosend;
+		
+	private class Receiver extends Thread {
+		public boolean run = true;
+		@Override
+		public void run() {
+			int bytes_valiable = -1;
+			
+			while(run) {
+				try {
+					bytes_valiable = input.available();
+				} catch (IOException e1) {
+					// TODO Auto-generated catch block
+					e1.printStackTrace();
+				}
+				if (bytes_valiable >= 0) {
+					try {
+						if (input.readByte() != '@') continue;
+						int angle = input.readByte();
+						float distance = input.readFloat();
+						System.out.println(distance);
+						DataRead d = new DataRead();
+						d.setDistance(distance);
+						d.setSensorAngle(-angle);
+						if (rr != null)
+							rr.robotData(d);
+						
+						if (reads != null) {
+							readm.tryAcquire();
+							reads.add(d);
+							readm.release();
+						}
+						
+					} catch (IOException e1) {
+						continue;
+					}
+				}
+			}
+		}
+	}
+
+	private class Sender extends Thread {
+		public boolean run = true;
+		@Override
+		public void run() {		
+			while(run) {
+				SendData value = null;
+				if (sendm.tryAcquire()) {
+					if (tosend.size() > 0) {
+						value = tosend.get(0);
+						tosend.remove(0);
+					}
+					sendm.release();
+				}
+				if (value != null)
+					value.send(output);
+				else
+					try {
+						Thread.sleep(100);
+					} catch (InterruptedException e) {
+
+					}
+			}
+		}
+	}
+	
+	private class SendData {
+		float f;
+		int i;
+		int cmd;
+		Pose p;
+		int type;
+		SendData(int cmd) {
+			this.cmd = cmd;
+			this.type = TYPE_CMD;
+		}
+		SendData(int cmd, int i) {
+			this.cmd = cmd;
+			this.i = i;
+			this.type = TYPE_INT;
+		}
+		SendData(int cmd, float f) {
+			this.cmd = cmd;
+			this.f = f;
+			this.type = TYPE_FLOAT;
+		}
+		SendData(int cmd, Pose p) {
+			this.cmd = cmd;
+			this.p = p;
+			this.type = TYPE_POSE;
+		}
+		public boolean send(DataOutputStream output) {
+			try {
+				switch (type) {
+				case TYPE_CMD:
+					output.write(cmd);
+					break;
+				case TYPE_INT:
+					output.write(cmd);
+					output.write(i);
+					break;
+				case TYPE_FLOAT:
+					output.write(cmd);
+					output.writeFloat(f);
+					break;
+				case TYPE_POSE:
+					output.write(cmd);
+					output.writeFloat(p.getX());
+					output.writeFloat(p.getY());
+					output.writeFloat(p.getHeading());
+					break;
+				default:
+					return false;
+				}
+				output.flush();
+			} catch (IOException e) {
+				return false;
+			}
+			return true;
+		}
+	}
+	
+	public BluetoothRobot (String name) {
+		
+		readm = new Semaphore(1);
+		
+		sendm = new Semaphore(1);
+		tosend = new ArrayList<SendData>();
+		
+		receivethread = new Receiver();
+		sendthread = new Sender();
+		
+		this.name = name;
+	}
+	
+	public void send (SendData sd) {
+		if (sendm.tryAcquire()) {
+			tosend.add(sd);
+			sendm.release();
+		}
+	}
+
+	@Override
+	public void moveForward() {
+		if (lastcmd == FORWARD) return;
+		send(new SendData(FORWARD));
+		lastcmd = FORWARD;
+	}
+
+	@Override
+	public void moveLeft() {
+		if (lastcmd == LEFT) return;
+		send(new SendData(LEFT));
+		lastcmd = LEFT;
+	}
+
+	@Override
+	public void moveRight() {
+		if (lastcmd == RIGHT) return;
+		send(new SendData(RIGHT));
+		lastcmd = RIGHT;
+	}
+
+	@Override
+	public void moveBackward() {
+		if (lastcmd == BACKWARD) return;
+		send(new SendData(BACKWARD));
+		lastcmd = BACKWARD;
+	}
+
+	@Override
+	public boolean connect () {
+		try {
+			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
+			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
+			if (nxtInfo.length == 0) { // failed to find a brick with the ID
+				System.err.println("NO NXT found");
+				return false;
+			}
+			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
+				System.err.println("Failed to open NXT");
+				return false;
+			}
+			
+			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
+			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
+			send(new SendData(SETSCANANGLE, scannangle)); // vai scanear em 5 em 5 graus
+		} catch (NXTCommException e) {
+			return false;
+		}
+		
+		receivethread.start();
+		sendthread.start();
+		
+		return true;	
+	}
+
+	@Override
+	public void stop() {
+		send(new SendData(STOP));
+		lastcmd = -1;
+	}
+
+
+	@Override
+	public void move(double x) {
+		send(new SendData(MOVE, (float)x));
+	}
+
+	@Override
+	public void rotate(double x) {
+		send(new SendData(ROTATE, (float)x));
+	}
+
+	@Override
+	public ArrayList<DataRead> scann(int ini, int end, int interval) {
+		int readsn = Math.abs(ini-end)/interval;
+		send(new SendData(SETSCANANGLE, interval));
+		ArrayList<DataRead> tmp;
+		reads = new ArrayList<DataRead>();
+		scann(null);
+		while(true) {
+			readm.tryAcquire();
+			if (reads.size() >= readsn+1) break;
+			readm.release();
+			try {
+				Thread.sleep(50);
+			} catch (InterruptedException e) {
+			}
+		}
+		while (readsn+1 < reads.size() ) {
+			reads.remove(reads.size()-1);
+		}
+		tmp = reads;
+		reads = null;
+		readm.release();
+		stopScann();
+		send(new SendData(SETSCANANGLE, scannangle));
+		return tmp;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		send(new SendData(STARTSCANN));
+	}
+
+	@Override
+	public void stopScann() {
+		send(new SendData(STOPSCANN));
+	}
+
+	@Override
+	public void disconnect() {
+		send(new SendData(EXIT));
+		if (receivethread == null) return;
+		receivethread.run = false;
+		try {
+			receivethread.join();
+		} catch (InterruptedException e1) {
+			System.out.println("Nao foi possivel finalizar as threads...");
+		}
+
+		if (sendthread == null) return;
+		sendthread.run = false;
+		try {
+			sendthread.join();
+		} catch (InterruptedException e1) {
+			System.out.println("Nao foi possivel finalizar as threads...");
+		}
+		
+		try {
+			nxtComm.close();
+		} catch (IOException e) {
+		}
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		send(new SendData(SETPOSE, new Pose(-y, x, a)));
+	}
+	
+	
+	@Override
+	public String toString() {
+		return "Bluetooth Mestre/Escravo";
+	}
+
+}

HistogramFilter/src/robots/DataRead.java

@@ -0,0 +1,23 @@
+package robots;
+
+
+public class DataRead {
+	private double distance;
+	private double sensorangle;
+	
+	public double getDistance() {
+		return distance;
+	}
+	
+	public void setDistance(double distance) {
+		this.distance = distance;
+	}
+
+	public double getSensorAngle() {
+		return sensorangle;
+	}
+
+	public void setSensorAngle(double sensorangle) {
+		this.sensorangle = sensorangle;
+	}
+}

HistogramFilter/src/robots/LCPRobot.java

@@ -0,0 +1,124 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.C;
+	//RemoteMotor ma = Motor.A;
+	//RemoteMotor mb = Motor.B;
+
+	public LCPRobot() {
+		pose = new Pose();
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataRead> scann(int ini, int end, int interval) {
+		ArrayList<DataRead> result = new ArrayList<DataRead>();
+		int val = 0;
+		int engine_mult = 5;
+		scannerm.setSpeed(80);
+		for (int j = ini; j <= end; j += interval) {
+			scannerm.rotateTo(-(j * engine_mult));
+			scannerm.waitComplete();
+			val = sonar.getDistance();
+
+			DataRead data = new DataRead();
+			data.setDistance(val);
+			data.setSensorAngle(j);
+			result.add(data);
+			System.out.println(pose);
+		}
+		scannerm.rotateTo(-10);
+		scannerm.waitComplete();
+		System.out.println("end");
+		return result;
+	}
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

HistogramFilter/src/robots/Robot.java

@@ -0,0 +1,40 @@
+package robots;
+
+import java.util.ArrayList;
+
+public interface Robot {
+	/* Faz o movimento até receber o stop */
+	public void moveForward ();
+	public void moveLeft ();
+	public void moveRight ();
+	public void moveBackward ();
+	public void stop ();
+	/* vai x cm para frente e para */
+	public void move(double x);
+	/* roda x graus no sentido horário e para */
+	public void rotate(double x);
+	
+	/* leitura sensores */
+	/**	
+	 * @param ini angulo inicial
+	 * @param end angulo final
+	 * @param interval de quantos em quantos graus vamos realizar a leitura
+	 * @return
+	 */
+	public ArrayList<DataRead> scann (int ini, int end, int interval);
+	/* inicia uma leitura conínua do sonar */
+	void scann (RobotReturn r);
+	/* para uma leitura contínua */
+	void stopScann ();
+	
+	/* funcoes de conexao */
+	public boolean connect ();
+	public void disconnect ();
+	
+	/* afimar nova pose para o robo */
+	public void setPose(float x, float y, float a);
+	
+	/* deve retorna o nome do robo */
+	public String toString();
+	
+}

HistogramFilter/src/robots/RobotReturn.java

@@ -0,0 +1,5 @@
+package robots;
+
+public interface RobotReturn {
+	public void robotData (DataRead data);
+}

HistogramFilter/src/robots/VirtualRobot.java

@@ -0,0 +1,161 @@
+package robots;
+
+import java.util.ArrayList;
+import java.util.Random;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+	private Random rand;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataRead data = new DataRead();
+
+				double dist = getDistance();
+
+				data.setDistance((int) dist);
+				data.setSensorAngle(angle + 90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo = -cone / 2; angulo <= cone / 2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist)
+				mindist = dist;
+		}
+		double v = mindist + rand.nextGaussian() * 4; //+ Math.random() * 2;
+		v = Math.min(255, v);
+		return v;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose(20,20,90);
+		pose.setHeading(0);
+		this.map = map;
+		rand = new Random();
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		System.out.println("VR: "+pose);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float) -x);
+	}
+
+	@Override
+	public ArrayList<DataRead> scann(int ini, int end, int interval) {
+		ArrayList<DataRead> result = new ArrayList<DataRead>();
+		for (angle = ini-90; angle <= end-90; angle += interval) {
+			DataRead data = new DataRead();
+			double dist = getDistance();
+			data.setDistance((int) dist);
+			data.setSensorAngle(angle+90);
+			result.add(data);
+		}
+		angle = 0;
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null)
+			return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

Sonar/src/config/Models.java

@@ -1,5 +1,7 @@
 package config;
 
+import java.util.ArrayList;
+
 import lejos.robotics.mapping.LineMap;
 import lejos.robotics.navigation.Pose;
 
@@ -23,4 +25,22 @@ public class Models {
 		} 
 		return mindist;
 	}
+	
+	/* retorna a probabilidade do robo estar em (ap, aang) sendo que ele saiu de (bp, bang) 
+	 * com um movimento de rotacao de ang graus.*/
+	public double rotateProbability (Pose pa, double aang, Pose pb, double bang, double ang) {
+		return 0;
+	}
+	
+	/* retorna a probabilidade do robo estar em (ap, aang) sendo que ele saiu de (bp, bang) 
+	 * com sendo que ele foi move centimetros para frente.*/
+	public double moveProbability (Pose pa, double aang, Pose pb, double bang, double move) {
+		return 0;
+	}
+	
+	/* retorna a probabilidade dele estar em (p, ang) sendo que realizou a leitura do
+	 *  conjunto x de medidas */
+	public double readProbability (Pose p, double angle, ArrayList<Integer> x) {
+		return 0;
+	}
 }

SonarBrick/src/Sonar.java

@@ -3,119 +3,77 @@ import java.io.DataOutputStream;
 import java.io.IOException;
 
 import lejos.nxt.Motor;
-import lejos.nxt.NXTRegulatedMotor;
 import lejos.nxt.SensorPort;
 import lejos.nxt.Sound;
 import lejos.nxt.UltrasonicSensor;
 import lejos.nxt.comm.BTConnection;
 import lejos.nxt.comm.Bluetooth;
 import lejos.robotics.navigation.DifferentialPilot;
-import lejos.robotics.navigation.Pose;
-import lejos.robotics.localization.*;
 
 class Scanner extends Thread {
 	DataOutputStream output;
-	OdometryPoseProvider provider;
 	boolean run;
 	public boolean scann;
-	public int increment = 5;
-	int position = 0;
-	
-	Scanner (DataOutputStream output, OdometryPoseProvider provider) {
+
+	Scanner(DataOutputStream output) {
 		super();
 		this.output = output;
-		this.provider = provider;
 		run = true;
 		scann = false;
 	}
+
+	public void scann() {
+		scann = true;
+	}
 	
-	public void stop () {
+
+	public void stop() {
 		run = false;
 	}
-	
+
 	public void run() {
-		NXTRegulatedMotor scannerm = Motor.C;
-		UltrasonicSensor sensor = new UltrasonicSensor(SensorPort.S1) ;
-		
+		UltrasonicSensor sensor = new UltrasonicSensor(SensorPort.S1);
 		while (run) {
-			if (scann == false) {
-				position = 0;
-				scannerm.rotateTo(0);
-				scannerm.waitComplete();
-			}
-			
-			scannerm.waitComplete();
-
-			if (scann == false)
-				continue;
-			
-			float distance = sensor.getDistance();
-						
-			Pose pose = provider.getPose();
-			float x = pose.getX();
-			float y = pose.getY();
-			float alpha = pose.getHeading();
-			
-			try {
-				output.write('@');
-				output.write(position);
-				output.writeFloat(alpha);
-				output.writeFloat(x);
-				output.writeFloat(distance);
-				output.writeFloat(y);
-				output.flush();
-			} catch (IOException e) {
+			if (scann) {
+				int distance = sensor.getDistance();
+				try {
+					output.write('@');
+					output.write(distance);
+					output.flush();
+					scann = false;
+					Thread.yield();
+				} catch (IOException e) {
+				}
 			}
-		
-			
-			position += increment;
-			if (position == 90 || position == -90)
-				increment *= -1;
-
-			scannerm.rotateTo(position*5);
 		}
-		
-		scannerm.rotateTo(0);
-		scannerm.waitComplete();
 	}
 }
 
 public class Sonar {
-	
-	private static final byte FORWARD = 0;
-	private static final byte STOP = 1;
-	private static final byte EXIT = 2;
-	private static final byte LEFT = 3;
-	private static final byte RIGHT = 4;
-	private static final byte BACKWARD = 5;
-	
-	public static final byte STOPSCANN = 6;
-	public static final byte STARTSCANN = 7;
-	public static final byte MOVE = 8;
-	public static final byte ROTATE = 9;
-	public static final byte SETPOSE = 10;
-	public static final byte SETSCANANGLE = 11;
-	
+
+	public static final byte EXIT = 10;
+	public static final byte READ = 2;
+	public static final byte MOVE = 3;
+
 	public static void main(String[] args) throws Exception {
-		
+
 		BTConnection btc = Bluetooth.waitForConnection();
-		//USBConnection btc = USB.waitForConnection();
-		
+		// USBConnection btc = USB.waitForConnection();
+
 		DataInputStream input = btc.openDataInputStream();
 		DataOutputStream output = btc.openDataOutputStream();
-		
-		DifferentialPilot pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A, false); 
-		OdometryPoseProvider provider = new OdometryPoseProvider(pilot);
+
+		DifferentialPilot pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A, false);
 		pilot.setRotateSpeed(5);
 		pilot.setTravelSpeed(20);
-		
-		Scanner scan = new Scanner(output, provider);
+
+		Scanner scan = new Scanner(output);
 		scan.start();
 		int input_byte;
 		boolean run = true;
-		
+
 		Sound.twoBeeps();
-		
+
 		while (run) {
 			if (input.available() <= 0) {
 				Thread.yield();
@@ -124,56 +82,21 @@ public class Sonar {
 			input_byte = input.readByte();
 
 			System.out.println(input_byte);
-			
+
 			switch (input_byte) {
-				case FORWARD:
-					pilot.forward();
-					break;
-				case STOP:
-					pilot.stop();
-					break;
-				case EXIT:
-					run = false;
-					break;
-				case LEFT:
-					pilot.rotateLeft();
-					break;
-				case RIGHT:
-					pilot.rotateRight();
-					break;
-				case BACKWARD:
-					pilot.backward();
-					break;
-				case STOPSCANN:
-					scan.scann = false;
-					break;
-				case STARTSCANN:
-					scan.position = 90;
-					scan.increment = -Math.abs(scan.increment);
-					scan.scann = true;
-					break;
-				case MOVE:
-					float d = input.readFloat();
-					pilot.travel(d);
-					break;
-				case ROTATE:
-					float r = input.readFloat();
-					pilot.rotate(r);
-					break;
-				case SETPOSE:
-					float x = input.readFloat();
-					float y = input.readFloat();
-					float a = input.readFloat();
-					System.out.println("x: "+x);
-					System.out.println("y: "+y);
-					System.out.println("a: "+a);
-					provider.setPose(new Pose(x,y,a));
-					break;
-				case SETSCANANGLE:
-					int i = input.readByte();
-					System.out.println("ang: "+i);
-					scan.increment = i;
-					break;
+			case READ:
+				scan.scann();
+				break;
+			case EXIT:
+				run = false;
+				break;
+			case MOVE:
+				float d = input.readFloat();
+				pilot.travel(d);
+				break;
+			default:
+				pilot.stop();
+				break;
 			}
 		}
 		Sound.beep();