Make code compile
merge error fix
Implemented heading kalman filter
made test file
Merge branch 'vehicle_refactor_2014' of https://github.com/aalberg/tortuga into vehicle_refactor_2014