Advertisement
Guest User

Untitled

a guest
Mar 28th, 2020
104
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 2.04 KB | None | 0 0
  1. def jac_c_b_v_angles(angles, acc): # uff...
  2.     """
  3.  
  4.    :param angles: Euler angles, np.ndarray, shape: (3,1)
  5.    :param acc: accelerations, np.ndarray, shape: (3, 1)
  6.    :return: the derivetive of C_b_v function by angles.
  7.                np.ndarray, shape: (3, 3)
  8.    """
  9.  
  10.     phi, theta, psi = angles.flatten()
  11.     a_x, a_y, a_z = acc.flatten()
  12.  
  13.     result = np.zeros(shape=(3,3))
  14.  
  15.     # first row
  16.     result[0, 0] = a_y * (np.cos(psi) * np.sin(theta) * np.cos(phi) + np.sin(psi) * np.sin(phi)) + \
  17.                    a_z * (-np.cos(psi) * np.sin(theta) * np.sin(phi) + np.sin(psi) * np.cos(phi))
  18.     result[0, 1] = a_x * (-np.cos(psi) * np.sin(theta)) + \
  19.                    a_y * (np.cos(psi) * np.cos(theta) * np.sin(phi)) + \
  20.                    a_z * (np.cos(psi) * np.cos(theta) * np.cos(phi))
  21.     result[0, 2] = a_x * (-np.sin(psi) * np.cos(theta)) + \
  22.                    a_y * (-np.sin(psi) * np.sin(theta) * np.sin(phi) - np.cos(psi) * np.cos(phi)) + \
  23.                    a_z * (-np.sin(psi) * np.sin(theta) * np.cos(phi) + np.cos(psi) * np.sin(phi))
  24.  
  25.     # second row
  26.     result[1, 0] = a_y * (np.sin(psi) * np.sin(theta) * np.cos(phi) - np.cos(psi) * np.sin(phi)) + \
  27.                    a_z * (-np.sin(psi) * np.sin(theta) * np.sin(phi) - np.cos(psi) * np.cos(phi))
  28.     result[1, 1] = a_x * (-np.sin(psi) * np.sin(theta)) + \
  29.                    a_y * (np.sin(psi) * np.cos(theta) * np.sin(phi)) + \
  30.                    a_z * (np.sin(psi) * np.cos(theta) * np.cos(phi))
  31.     result[1, 2] = a_x * (np.cos(psi) * np.cos(theta)) + \
  32.                    a_y * (np.cos(psi) * np.sin(theta) * np.sin(phi) - np.sin(psi) * np.cos(phi)) + \
  33.                    a_z * (np.cos(psi) * np.sin(theta) * np.cos(phi) + np.sin(psi) * np.sin(phi))
  34.  
  35.     result[2, 0] = a_y * (np.cos(theta) * np.cos(psi)) + \
  36.                    a_z(-np.cos(theta) * np.sin(phi))
  37.     result[2, 1] = a_x * (-np.cos(theta)) + \
  38.                    a_y * (-np.sin(theta) * np.sin(phi)) + \
  39.                    a_z * (-np.sin(theta) * np.cos(phi))
  40.     result[2, 2] = 0
  41.  
  42.     return result
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement