Как рассчитать углы крена, рыскания и тангажа по трехмерным координатам (углы Эйлера)

Найдите вектор, идущий от хвоста к голове, и нормализуйте его: вызовите результат$\hat{Z}^\prime$.

Найдите вектор, соединяющий точку левого крыла с точкой правого крыла, и нормализуйте его: вызовите результат$\hat{Y}^\prime$.

Необязательно: проверьте работоспособность$\left<\hat{Y}^\prime,\,\hat{Z}^\prime\right>=0$

Теперь рассчитайте$\hat{X}^\prime = \hat{Y}^\prime\times\hat{Z}^\prime$.

Соберите три вектора как векторы -столбцы в матрицу$U = \left(\hat{X}^\prime\, \hat{Y}^\prime\,\hat{Z}^\prime\right)$. Это матрица вращения, которая вращает вашу ссылку$\hat{X},\,\hat{Y},\,\hat{Z}$основу в основу, выровненную по мухе.

Чтобы преобразовать в углы, нам нужно вычислить ось вращения и угол поворота. Это проще всего сделать, взглянув на формулу Родригеса для общего члена.$\exp\left(H_{3\times 3}\right)$из$SO(3)$"назад"

$$\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}$$

где:

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

и

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

где$x,\,y,\,z$компоненты оси вращения и$\sqrt{x^2+y^2+y^2}$угол поворота в радианах.$H_{3\times3}$является членом алгебры Ли$\mathfrak{so}(3)$что возводится в степень к матрице вращения.

Итак, берем матрицу$U$вы нашли выше и сравните его с (1): вы можете видеть в (1), что кососимметричная часть:

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

и это позволит вам прочитать$H_{3\times3}$и угол поворота$||H_{3\times 3}||$.

Между прочим, если бы вы делали это, чтобы построить систему слежения за мухой, вам нужно было бы сделать что-то вроде поиска наименьших квадратов, наиболее подходящих для векторов.$\hat{Z}^\prime$и$\hat{Y}^\prime$: векторы необработанных данных не будут полностью ортогональны из-за зашумленных данных. Кроме того, вы можете выбрать 3 точки на лету, отследить их и найти$\hat{Z}^\prime$и$\hat{Y}^\prime$как ортогональное основание для трех сторон треугольника. Тогда вам не нужно было бы лучше всего подходить методом наименьших квадратов, но ваше отслеживание может быть менее точным.