Added new error quaternion formation per Roumeliotis