cálculo de ángulos a partir de matrices de rotación combinadas

Question

Utilizo el siguiente código para construir una matriz de rotación

 function C = build3Drot(yaw,pitch,roll)

 X = [[1,0,0];[0,cos(roll),-sin(roll)];[0,sin(roll),cos(roll)]]

 Y = [[cos(pitch),0,sin(pitch)];[0,1,0];[-sin(pitch),0,cos(pitch)]]

 Z = [[cos(yaw),-sin(yaw),0];[sin(yaw),cos(yaw),0];[0,0,1]]
 C = Z * Y * X;

y la función inversa para obtener ángulos euleur dada una matriz de rotación

function [yaw,pitch,roll] = unbuild3DRot(X)
 yaw = atan2(X(2,1),X(1,1));
 pitch = atan(-X(3,1) / sqrt(X(3,2) * X(3,2) + X(3,3) * X(3,3)));
 roll = atan2(X(3,2),X(3,3));

Las dos funciones parecen correctas, pero obtengo resultados muy, muy extraños al extraer ángulos de matrices combinadas. Por ejemplo

A= build3Drot(0.052041,0.663198,-0.014) 
A =
    0.7870   -0.0606    0.6140
    0.0410    0.9981    0.0460
   -0.6156   -0.0110    0.7880

B = build3Drot(0.085,0.737,0.049)
B =
    0.7378   -0.0520    0.6730
    0.0629    0.9980    0.0082
   -0.6721    0.0363    0.7396
C = A * B
C =
    0.1642   -0.0791    0.9833
    0.0621    0.9956    0.0698
   -0.9845    0.0496    0.1684

[y,p,r] = unbuild3Drot(C)
y =
    0.3615
p =
    1.3944
r =
    0.2864

Así que empecé con ángulos de guiñada y balanceo muy pequeños para ambas matrices A y B y terminé con una guiñada final de (0.3615 * 180 / pi) = 20.7 grados y un balanceo de (0.2864 * 180 / pi) = 16.4 grados. ¿Es esto normal? Me parece muy extraño, ya que los ángulos iniciales en grados para A eran gui = 2.9817 cabeceo = 37.9984 alabeo = -0.8021 y para B gui = 4.8701 cabeceo = 42.2270 alabeo = 2.8075.

También me pregunto por qué si encadeno/multiplico varias matrices en un punto determinado el tono disminuye. Por ejemplo

[y,p,r]=unbuild3Drot(A * A * A)

[y,p,r]
 =
    3.1174    1.1478    2.9611

while 
[y,p,r]=unbuild3Drot(A * A * A * A)
y,p,r]
 = 
   -3.0863    0.4847    2.9904

El tono resultante de A * A * A * A es menor que el de A

Siento que estoy haciendo algo horriblemente mal... pero no puedo entender qué... gracias.

Answer 1

No puedo hablar de lo que está pasando en el $A$ , $B$ , $C$ ejemplo, pero tienes razón en que los poderes de $A$ apuntan a un error en el cálculo del paso. El problema es que al tomar la raíz cuadrada se está perdiendo el signo del coseno del paso, y esto está provocando que los ángulos se reflejen en $\pi/2$ . Ya que tienes el rollo, puedes usarlo para deshacer el rollo sin raíz cuadrada:

roll = atan2(X(3,2),X(3,3));
pitch = atan(-X(3,1) / (X(3,2) * sin (roll) + X(3,3) * cos (roll)));

Esto debería hacer que el tono de ciclo a través de $-\pi/2$ en lugar de reflejarse en $\pi/2$ lo cual es correcto, ya que la guiñada y el balanceo saltan por $\pi$ al mismo tiempo. Si no entiendes esa equivalencia, quizá quieras echar un vistazo a esto: ¿Cómo puedo encontrar ángulos de Euler equivalentes?

Tenga en cuenta que la división puede causar problemas si el denominador se hace cero; para evitarlo, puede utilizar la función atan2 como para los otros dos ángulos y descartar la información adicional espuria sobre el paso que arroja.

Pero no puedo escribir esta respuesta sin repetir que, si es posible, hay que intentar evitar todas estas complicaciones trabajando con cuaterniones.