Added odometry correction