Skipped working on odometry because it is riddled with errors. Check comments