mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
67 lines
1.5 KiB
Python
67 lines
1.5 KiB
Python
from __future__ import print_function
|
|
from pylab import *
|
|
from pprint import pprint
|
|
|
|
#pylint: disable=all
|
|
|
|
phi = 0.1
|
|
theta = 0.2
|
|
psi = 0.3
|
|
|
|
cosPhi = cos(phi)
|
|
cosPhi_2 = cos(phi/2)
|
|
sinPhi = sin(phi)
|
|
sinPhi_2 = sin(phi/2)
|
|
|
|
cosTheta = cos(theta)
|
|
cosTheta_2 = cos(theta/2)
|
|
sinTheta = sin(theta)
|
|
sinTheta_2 = sin(theta/2)
|
|
|
|
cosPsi = cos(psi)
|
|
cosPsi_2 = cos(psi/2)
|
|
sinPsi = sin(psi)
|
|
sinPsi_2 = sin(psi/2)
|
|
|
|
C_nb = array([
|
|
[cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi],
|
|
[cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi],
|
|
[-sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]])
|
|
|
|
print('\nC_nb')
|
|
pprint(C_nb)
|
|
|
|
theta = arcsin(-C_nb[2,0])
|
|
phi = arctan(C_nb[2,1]/ C_nb[2,2])
|
|
psi = arctan(C_nb[1,0]/ C_nb[0,0])
|
|
print('\nphi {:f}, theta {:f}, psi {:f}\n'.format(phi, theta, psi))
|
|
|
|
q = array([
|
|
cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2,
|
|
sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2,
|
|
cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2,
|
|
cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2])
|
|
|
|
a = q[0]
|
|
b = q[1]
|
|
c = q[2]
|
|
d = q[3]
|
|
|
|
print('\nq')
|
|
pprint(q.T)
|
|
|
|
a2 = a*a
|
|
b2 = b*b
|
|
c2 = c*c
|
|
d2 = d*d
|
|
|
|
C2_nb = array([
|
|
[a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c)],
|
|
[2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b)],
|
|
[2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]])
|
|
|
|
print('\nC2_nb')
|
|
pprint(C2_nb)
|
|
|
|
# vim: set et ft=python fenc=utf-8 ff=unix sts=4 sw=4 ts=8 :
|