Odometry Correction

Cleaned up the logic for Odometry Correction