more testing on gps measurement
fix poseutils cos/sin problem
more testing on orientation