Skip to content

Instantly share code, notes, and snippets.

@glemaitre
Last active July 10, 2017 09:52
Show Gist options
  • Save glemaitre/e3009a8b51716526a7e8121a5cc1a1b6 to your computer and use it in GitHub Desktop.
Save glemaitre/e3009a8b51716526a7e8121a5cc1a1b6 to your computer and use it in GitHub Desktop.
def rep_boitier_inertiel(Ax, Ay, Az, q0, q1, q2, q3):
"""TODO: Docstring for rep_boitier_inertiel.
:Ax: Acc lin x
:Ay: Acc lin y
:Az: Acc lin z
:q0: quat q0
:q1: quat q1
:q2: quat q2
:q3: quat q3
:returns: array Ax, Ay, Az corrigé dans l'espace
"""
m_r = process_all_q2mr(q0, q1, q2, q3)
Ax_integral = []
Ay_integral = []
Az_integral = []
for Axi, Ayi, Axi, m_ri in zip(Ax, Ay, Az, m_r):
A = np.array([Axi, Ayi, Azi])
Ares = np.dot(m_ri, A.T)
Ax_integral.append(Ares[0])
Ay_integral.append(Ares[1])
Az_integral.append(Ares[2])
return np.array(Ax_integral), np.array(Ay_integral), np.array(Az_integral)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment