Online Odometry correction based on IR sensors.

First attempt to additionally correct by using planes in front of the robot