From each step of my vision code I am able to get around 400 coordinates of where the robot thinks the walls are
I want to integrate this into Monte-Carlo observation step.
I'm storing the map of the maze as a set of Line segments. What would be a nice way to implement the sensor update, i.e. given the position (x,y) of the robot what is the probability that it is found there given the above described coordinates of the walls.
The main idea I currently have:
Transform points in polar coordinates. Then for each point (from vision output) compute a ray with this angle and find the first intersection with the maze. Now we have the predicted distance and real distance and we can compute the probability that this measurement is right.
The main drawback is that this is slow. For each point from vision output I have to iterate over all line segments to find the one with the closest intersection. The line segments number is around 50. So it gets to O(400*50*Particle number).