Reconocimiento de la posicion

En este ejercicio, teníamos dos apartados:
Localizarnos:
En este apartado lo que realizamos es la toma de histogramas de cada punto, después tomamos otro histograma para comprobar en que punto nos encontramos, y realizamos el algoritmo.
El algoritmo consta de restar el histograma que tenemos actual con todos los que teníamos almacenados, una vez comparados, nos quedamos con el que nos haya dado el menor resultado y esa sera la posición en la que nos encontramos.
Posición estimada:
Para calcular la posición estimada, necesitamos primero realizar un escaneo de todos los puntos con el radar del primer apartado.
Una vez tenemos tomados todos los puntos con ese radar, realizamos un escaneo de la posición en la que estamos, utilizando el mismo radar.
Con el nuevo histograma tomado, y con el que teníamos almacenado con la posición conocida, empezamos a realizar comprobaciones y a rotar el nuevo histograma, de esta forma tras realizar todas las rotaciones y comparaciones, buscamos el valor de la resta menor, que sera los grados a los que esta de nuestro punto de origen