Cleaned up and hopefully fixed bugs in OdometryGyroPoseProvider.
Previously, moves would be very accurate but logical heading would change erratically, causing accurate navigation to the wrong waypoints.