Cambiada la corrección de los ángulos para evitar gimbal lock por la segunda solución que da la función Matrix3x3::getRPY()