Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- def jac_c_b_v_angles(angles, acc): # uff...
- """
- :param angles: Euler angles, np.ndarray, shape: (3,1)
- :param acc: accelerations, np.ndarray, shape: (3, 1)
- :return: the derivetive of C_b_v function by angles.
- np.ndarray, shape: (3, 3)
- """
- phi, theta, psi = angles.flatten()
- a_x, a_y, a_z = acc.flatten()
- result = np.zeros(shape=(3,3))
- # first row
- result[0, 0] = a_y * (np.cos(psi) * np.sin(theta) * np.cos(phi) + np.sin(psi) * np.sin(phi)) + \
- a_z * (-np.cos(psi) * np.sin(theta) * np.sin(phi) + np.sin(psi) * np.cos(phi))
- result[0, 1] = a_x * (-np.cos(psi) * np.sin(theta)) + \
- a_y * (np.cos(psi) * np.cos(theta) * np.sin(phi)) + \
- a_z * (np.cos(psi) * np.cos(theta) * np.cos(phi))
- result[0, 2] = a_x * (-np.sin(psi) * np.cos(theta)) + \
- a_y * (-np.sin(psi) * np.sin(theta) * np.sin(phi) - np.cos(psi) * np.cos(phi)) + \
- a_z * (-np.sin(psi) * np.sin(theta) * np.cos(phi) + np.cos(psi) * np.sin(phi))
- # second row
- result[1, 0] = a_y * (np.sin(psi) * np.sin(theta) * np.cos(phi) - np.cos(psi) * np.sin(phi)) + \
- a_z * (-np.sin(psi) * np.sin(theta) * np.sin(phi) - np.cos(psi) * np.cos(phi))
- result[1, 1] = a_x * (-np.sin(psi) * np.sin(theta)) + \
- a_y * (np.sin(psi) * np.cos(theta) * np.sin(phi)) + \
- a_z * (np.sin(psi) * np.cos(theta) * np.cos(phi))
- result[1, 2] = a_x * (np.cos(psi) * np.cos(theta)) + \
- a_y * (np.cos(psi) * np.sin(theta) * np.sin(phi) - np.sin(psi) * np.cos(phi)) + \
- a_z * (np.cos(psi) * np.sin(theta) * np.cos(phi) + np.sin(psi) * np.sin(phi))
- result[2, 0] = a_y * (np.cos(theta) * np.cos(psi)) + \
- a_z(-np.cos(theta) * np.sin(phi))
- result[2, 1] = a_x * (-np.cos(theta)) + \
- a_y * (-np.sin(theta) * np.sin(phi)) + \
- a_z * (-np.sin(theta) * np.cos(phi))
- result[2, 2] = 0
- return result
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement