Estimate orientation using quaternions
Mostly fixed the exploding quaternion problem.  Drift is still an issue