Code_Quaternion-kinematics-for-the-error-state-Kalman-filter