Added some code to correct the orientation estimate when it jumps from - to + PI. Changed code based on the behavior of the robot on the maze. The robot is too big to rotate confortably on most places in the maze, it would be much simpler to work with a robot that ensures that it can rotate on spot without colliding with any wall.