Add sensor quaternions and use for xsens.
Fix xsens GPS observations.