Remove getForwardAzimuthSimple. Correct getForwardAzimuth to put out
reasonable data.
Implemented getRangeToPosition
Correct output of getForwardAzimuth function.
Implements gps calculations
Added ROS subscriber for calculated IMU data. Includes magnetic heading
Changed rotation function to use magnetic heading
Fixed rotation function. Successful test on Asctec
Merges in getForwardAzimuth
Working over commit
Complete merge fix build