Correction Commit

Changed some logic/math for the OdometryCorrection
Merge branch 'Navagation' of https://github.com/njfaries/ecse211grp6 into Navagation