def Quaternion2DCM(quaternion):
'''
Calcul la matrice de passage DCM (Direct Cosine Matrix) à partir du quaternion q.
Si q = q0+ q1 i + q2 j + q3 k alors
DCM =
[
1-2q2^2-2q3^2 2q1q2-2q0q3 2q1q3+2q0q2
2q1q2+2q0q3 1-2q1^2-2q3^2 2q2q3-2q0q1
2q1q3-2q0q2 2q2q3+2q0q1 1-2q1^2-2q2^2
]
'''
check.vecteur_n(4, quaternion, "bloc outils/Quaternion2DCM")
q = quaternion
dcm = np.empty([3, 3])
dcm[0][0] = 1 - 2 * q[2]**2 - 2 * q[3]**2
dcm[1][0] = 2 * q[1] * q[2] + 2 * q[0] * q[3]
dcm[2][0] = 2 * q[1] * q[3] - 2 * q[0] * q[2]
dcm[0][1] = 2 * q[1] * q[2] - 2 * q[0] * q[3]
dcm[1][1] = 1 - 2 * q[1]**2 - 2 * q[3]**2
dcm[2][1] = 2 * q[2] * q[3] + 2 * q[0] * q[1]
dcm[0][2] = 2 * q[1] * q[3] + 2 * q[0] * q[2]
dcm[1][2] = 2 * q[2] * q[3] - 2 * q[0] * q[1]
dcm[2][2] = 1 - 2 * q[1]**2 - 2 * q[2]**2
dcm = dcm.T
return dcm
def Euler2Quaternion(euler):
'''
Calcul du quaternion q à partir des angles d'Euler (phi,theta,psi)
q = q0 + q1 i + q2 j + q3 k
avec :
q0 = cos(0.5*psi)cos(0.5*theta)cos(0.5*phi) + sin(0.5*psi)sin(0.5*theta)sin(0.5*phi)
q1 = sin(0.5*psi)cos(0.5*theta)cos(0.5*phi) - cos(0.5*psi)sin(0.5*theta)sin(0.5*phi)
q2 = cos(0.5*psi)sin(0.5*theta)cos(0.5*phi) + sin(0.5*psi)cos(0.5*theta)sin(0.5*phi)
q3 = cos(0.5*psi)cos(0.5*theta)sin(0.5*phi) - sin(0.5*psi)sin(0.5*theta)cos(0.5*phi)
'''
check.vecteur_n(3, euler, "bloc outils/Euler2Quaternion")
phi = euler[0]
theta = euler[1]
psi = euler[2]
u = np.empty(6)
u[0] = np.sin(phi / 2)
u[1] = np.cos(phi / 2)
u[2] = np.sin(theta / 2)
u[3] = np.cos(theta / 2)
u[4] = np.sin(psi / 2)
u[5] = np.cos(psi / 2)
quaternion = np.empty(4)
quaternion[0] = u[1] * u[3] * u[5] + u[0] * u[2] * u[4]
quaternion[1] = u[0] * u[3] * u[5] - u[1] * u[2] * u[4]
quaternion[2] = u[1] * u[2] * u[5] + u[0] * u[3] * u[4]
quaternion[3] = u[1] * u[3] * u[4] - u[0] * u[2] * u[5]
return quaternion
def vectoriel(u, v):
'''
Calcul le produit vectoriel de deux vecteur 3x1
'''
params = [u, v]
for i, p in enumerate(params):
check.vecteur_n(3, p, "bloc outils/vectoriel")
w = np.cross(u, v)
return w
def Inert2Miss(euler, u):
'''
Calcul les composantes d'un vecteur du repère inertiel dans le repère missile.
'''
check.vecteur_n(3, euler, "bloc outils/Inert2Miss")
check.vecteur_n(3, u, "bloc outils/Inert2Miss")
dcm = Euler2DCM(euler)
v = dcm.dot(u)
return v
def execute(self, ecef):
'''
Transforme les coordonnées dans le repère ECEF (Earth-Centered
Earth Fixed) [m,m,m] en coordonnées géographiques LLA
[rad,rad,m] par rapport à l'ellipsoïde de référence)
'''
self.input = ecef
check.vecteur_n(3, self.input, "bloc outils/ECEF2LLA")
r = self.__calcul_r()
phi = self.__calcul_latitude(r)
lmbda = self.__calcul_longitude()
h = self.__calcul_h(phi, r)
self.output = np.array([phi, lmbda, h])
def execute(self, enu):
'''
Transforme les coordonnées dans le repère ENU local
(East North Up) xyz= [m,m,m] défini par le plan tangent
à l'ellipsoïde au point origine lla0 [rad,rad,m] et le
vecteur normal défini positivement suivant les altitudes
dans le repère ECEF (Earth Centered Earth Fixed).
'''
self.input = enu
check.vecteur_n(3, enu, "bloc outils/xyz_ENU2ECEF")
xyz0 = self.__calcul_xyz0()
m_ned2ecef = self.__calcul_M_NED2ECEF()
ned = np.array([self.input[1], self.input[0], -self.input[2]])
self.output = xyz0 + m_ned2ecef.dot(ned)
def Euler2DCM(euler):
'''
Calcul la matrice de passage DCM (Direct Cosine Matrix) à partir des angles d'Euler.
DCM =
[
c(theta)c(psi) -c(phi)s(psi)+s(phi)s(theta)c(psi) s(phi)s(psi)+c(phi)s(theta)c(psi)
c(theta)s(psi) c(phi)c(psi)+s(phi)s(theta)s(psi) -s(phi)c(psi)+c(phi)s(theta)s(psi)
-s(theta) s(phi)c(theta) c(phi)c(theta)
]^T
où c correspond à cos, et s correspond à sin
'''
check.vecteur_n(3, euler, "bloc outils/Euler2DCM")
phi = euler[0]
theta = euler[1]
psi = euler[2]
u = np.empty(6)
u[0] = np.sin(phi)
u[1] = np.cos(phi)
u[2] = np.sin(theta)
u[3] = np.cos(theta)
u[4] = np.sin(psi)
u[5] = np.cos(psi)
dcm = np.empty([3, 3])
dcm[0][0] = u[3] * u[5]
dcm[1][0] = u[3] * u[4]
dcm[2][0] = -u[2]
dcm[0][1] = -u[1] * u[4] + u[0] * u[2] * u[5]
dcm[1][1] = u[1] * u[5] + u[0] * u[2] * u[4]
dcm[2][1] = u[0] * u[3]
dcm[0][2] = u[0] * u[4] + u[1] * u[2] * u[5]
dcm[1][2] = -u[0] * u[5] + u[1] * u[2] * u[4]
dcm[2][2] = u[1] * u[3]
dcm = dcm.T
return dcm
def Quaternion2Euler(quaternion):
'''
Calcul des angles d'Euler (phi,theta,psi) à partir du quaternion q.
Si q = q0 + q1 i + q2 j + q3 k , alors
phi = atan2( 2(q0q1+q2q3) , q0^2-q1^2 - q2^2+q3^2)
theta = asin(2q0q2-2q1q3)
psi = atan2( 2(q1q2+q0q3) , q0^2+q1^2 - q2^2-q3^2)
'''
check.vecteur_n(4, quaternion, "bloc outils/Quaternion2Euler")
q = quaternion
euler = np.empty(3)
euler[0] = math.atan2(2 * (q[0] * q[1] + q[2] * q[3]),
q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2)
euler[1] = math.asin(np.clip(2 * (q[0] * q[2] - q[1] * q[3]), -1, 1))
euler[2] = math.atan2(2 * (q[1] * q[2] + q[0] * q[3]),
q[0]**2 + q[1]**2 - q[2]**2 - q[3]**2)
return euler
def execute(self, euler):
'''
Calcul du quaternion à partir des vitesses de rotation du missile et de
l'attidude initiale du missile.
'''
self.input = euler
check.vecteur_n(3, euler, "bloc outils/pqr2Quaternion")
if self.time != -1:
eidot = self.__pqr2eidot()
y = self.__integrator(eidot)
self.output = y / norme(y)
eidot = self.__pqr2eidot()
self.int_xdot = eidot
self.time += self.time_step
else:
self.output = self.int_x / norme(self.int_x)
eidot = self.__pqr2eidot()
self.int_xdot = eidot
self.time = 0
def execute(self, ned):
'''
Calcul les coordonnées LLA (latitude, longitude, altitude) à
partir des coordonnées xyz exprimées dans le repère NED
(North, East, Down) dont l'origine du repère est définit par une
position LLA de référence.
'''
self.input = ned
check.vecteur_n(3, self.input, "bloc outils/NED2LLA")
enu = np.array([self.input[1], self.input[0], -self.input[2]])
enu2ecef = ENU2ECEF(self.lla_ref[0], self.lla_ref[1], self.lla_ref[2],
self.ellipsoide[0], self.ellipsoide[1],
self.ellipsoide[2])
enu2ecef.execute(enu)
ecef2lla = ECEF2LLA(self.ellipsoide[0], self.ellipsoide[1],
self.ellipsoide[2])
ecef2lla.execute(enu2ecef.output)
self.output = ecef2lla.output