add correct R2Quaternion and complete measurement update