Xsens quat

I want to use quaternions due to possible interpolation and xsens can output quaternions.

Also, found a silly bug in the GPS stuff I added, sorry about that.