Cómo calcular los ángulos de balanceo, guiñada y cabeceo a partir de coordenadas 3D (Ángulos de Euler)

Encuentre el vector que corre de la cola a la cabeza y normalícelo: llame al resultado$\hat{Z}^\prime$.

Encuentre el vector que une el punto del ala izquierda con el punto del ala derecha y normalícelo: llame al resultado$\hat{Y}^\prime$.

Opcional: verifique la cordura de que$\left<\hat{Y}^\prime,\,\hat{Z}^\prime\right>=0$

ahora calcula$\hat{X}^\prime = \hat{Y}^\prime\times\hat{Z}^\prime$.

Reúna los tres vectores como vectores de columna en la matriz$U = \left(\hat{X}^\prime\, \hat{Y}^\prime\,\hat{Z}^\prime\right)$. Esta es la matriz de rotación que rota tu referencia.$\hat{X},\,\hat{Y},\,\hat{Z}$base en la base alineada con la marcha.

Para convertir a ángulos, necesitamos calcular el eje de rotación y el ángulo de rotación. Esto se hace más fácilmente observando la fórmula de Rodrigues para un miembro general$\exp\left(H_{3\times 3}\right)$de$SO(3)$"hacia atrás"

$$\exp\left(H_{3\times 3}\right)=I_{3\times3}+\frac{\sin\left(||H_{3\times 3}||\right)}{||H_{3\times 3}||}\,H_{3\times 3} +\frac{1-\cos\left(||H_{3\times 3}||\right)}{||H_{3\times 3}||^2}\,H_{3\times 3}^2\tag{1}$$

dónde:

$$H_{3\times3} = \left(\begin{array}{ccc}0&z&-y\\-z&0&x\\y&-x&0\end{array}\right)\tag{2}$$

y

$$||H_{3\times3}||=\sqrt{x^2+y^2+y^2}\tag{3}$$

dónde$x,\,y,\,z$son las componentes del eje de rotación y$\sqrt{x^2+y^2+y^2}$es el ángulo de rotación en radianes.$H_{3\times3}$es el miembro del álgebra de Lie$\mathfrak{so}(3)$que exponencia a la matriz de rotación.

Entonces tomamos la matriz$U$que encontró arriba y compárelo con (1): puede ver en (1) que la parte simétrica sesgada es:

$$\frac{1}{2}(U-U^T) = \frac{\sin\left(||H_{3\times 3}||\right)}{||H_{3\times 3}||}\,H_{3\times 3}\tag{4}$$

y esto te permitirá leer$H_{3\times3}$y el ángulo de rotación$||H_{3\times 3}||$.

Por cierto, si estuviera haciendo esto para construir un sistema de seguimiento para la mosca, necesitaría hacer algo como encontrar los mínimos cuadrados que mejor se ajusten a los vectores.$\hat{Z}^\prime$y$\hat{Y}^\prime$: los vectores de datos sin procesar no serían del todo ortogonales debido a los datos ruidosos. Alternativamente, puede elegir 3 puntos sobre la marcha, rastrearlos y encontrar$\hat{Z}^\prime$y$\hat{Y}^\prime$como base ortogonal de los tres lados del triángulo. Entonces no tendría que hacer el mejor ajuste de mínimos cuadrados, pero su seguimiento podría ser menos preciso.