import numpy as np def enu_to_body_frame(enu_vector, roll, pitch, yaw): # Construct the direction cosine matrix (DCM) c_roll = np.cos(roll) s_roll = np.sin(roll) c_pitch = np.cos(pitch) s_pitch = np.sin(pitch) c_yaw = np.cos(yaw) s_yaw = np.sin(yaw) dcm = np.array([ [c_pitch * c_yaw, -c_roll * s_yaw + s_roll * s_pitch * c_yaw, s_roll * s_yaw + c_roll * s_pitch * c_yaw], [c_pitch * s_yaw, c_roll * c_yaw + s_roll * s_pitch * s_yaw, -s_roll * c_yaw + c_roll * s_pitch * s_yaw], [-s_pitch, s_roll * c_pitch, c_roll * c_pitch] ]) # Transform the vector from ENU to body frame body_vector = np.dot(dcm, enu_vector) return body_vector # Example usage enu_vector = np.array([1.0, 0.0, 0.0]) # Vector in ENU frame roll = 0.1 # Roll angle in radians pitch = 0.2 # Pitch angle in radians yaw = 0.3 # Yaw angle in radians body_vector = enu_to_body_frame(enu_vector, roll, pitch, yaw) print("Body frame vector:", body_vector)
Preview:
downloadDownload PNG
downloadDownload JPEG
downloadDownload SVG
Tip: You can change the style, width & colours of the snippet with the inspect tool before clicking Download!
Click to optimize width for Twitter