Correct GPS position according to IMU datum