Major new version of navigation code, merged with the IMU code.