In (3), X, Y and Z are position coordinates of the center
of mass of the vehicle with respect to the world frame, and
p, q and r are angular velocities expressed in the body fixed
frame. The vector ξ which appears in Equation (2), describes
the position and the orientation of the vehicle with respect
to the world frame, and is defined as