#added after getting the data in https://github.com/0x1beef/uap_nb/blob/main/src/gimbal_adjust_clouds.ipynb
from scipy.spatial.transform import Rotation
import math
def normalize(v):
return v / np.linalg.norm(v)
# rotate a vector by a certain angle around an axis.
def rotate(vec, axis, angle_degrees):
angle = math.radians(angle_degrees)
rot = Rotation.from_rotvec(angle * normalize(axis))
return rot.apply(vec)
# get the angle between vectors 'a' and 'b'.
# the sign is relative to the vector 'c' which is orthogonal to 'a' and 'b'.
def signed_angle(a, b, c):
# using https://stackoverflow.com/a/33920320
return math.degrees(math.atan2(np.dot(np.cross(a, b), c), np.dot(a, b)))
def apply_jet_roll_pitch(vec, jet_roll, jet_pitch):
vec = rotate(vec, [0, 0, 1], -jet_roll)
vec = rotate(vec, [1, 0, 0], jet_pitch)
return vec
# get the angle of the horizon in the pod's eye view without dero
def get_pod_horizon(jet_roll, jet_pitch, pod_pitch, pod_roll):
# a vector point forward along the jet's boreline:
jet_forward = apply_jet_roll_pitch([0, 0, -1], jet_roll, jet_pitch)
# a vector pointing right in the jet's wing plane:
jet_right = apply_jet_roll_pitch([1, 0, 0], jet_roll, jet_pitch)
# the pod's horizon: a vector initially pointing towards jet right, rotated by pod roll
pod_right = rotate(jet_right, -jet_forward, -pod_roll)
# a vector pointing at the target, rotated according to pod roll/pitch:
pod_forward = rotate(jet_forward, pod_right, -pod_pitch)
# pod_forward,the global 'az' viewing angle and a vector pointing up are coplanar.
# the global horizon is a vector pointing right, orthogonal to that plane.
right = np.cross(pod_forward, [0, 1, 0])
# a signed angle between the global horizon and the pod horizon:
return signed_angle(right, pod_right, pod_forward)
import matplotlib.pyplot as plt
def plot_diff(series, title):
abs(series[0:650] - series[0]).plot(label = title)
plot_diff(object_data.jet_roll, 'jet roll')
df = common.gimbal_fix_wh_to_bh(object_data, ['glare_angle'])
plot_diff(df.glare_angle, 'glare angle')
# frustum_roll_df is a dataframe containing Zaine's spreadsheet data
plot_diff(frustum_roll_df['Total rotation'], 'total rotation with "frustum roll"')
def pod_horizon_for_frame(d):
return get_pod_horizon(d.jet_roll, d.jet_pitch, d.pod_pitch, d.pod_roll_glare)
plot_diff(df.apply(pod_horizon_for_frame, axis=1), 'pod horizon')
plt.legend()
plt.ylabel('degrees')
plt.xlabel('frames')
plt.plot()