added Tyler's navigation code and IMU code