def _compute_horizon_slope(attitude: SO3):
theta = ArtificialHorizonArtist._find_heading_angle(attitude)
H = attitude.inv() * np.array([[np.cos(theta)], [np.sin(theta)], [0.0]
])
DH = attitude.inv() * np.array([[-np.sin(theta)], [np.cos(theta)],
[0.0]])
DH2 = H[0, 0] * DH - DH[0, 0] * H
slope = np.arctan2(DH2[2, 0], DH2[1, 0])
return slope
def _compute_horizon(attitude: SO3):
angles = np.linspace(0, 2 * np.pi, 50)
base_horizon = np.vstack(
(np.cos(angles), np.sin(angles), np.zeros(angles.shape)))
true_horizon = attitude.inv() * base_horizon
true_horizon = true_horizon[:, true_horizon[0, :] > 0]
true_horizon = true_horizon / true_horizon[0, :]
true_horizon = true_horizon[1:, :]
return true_horizon
def _compute_horizon_height(attitude: SO3):
theta = ArtificialHorizonArtist._find_heading_angle(attitude)
H = attitude.inv() * np.array([[np.cos(theta)], [np.sin(theta)], [0.0]
])
H = H / H[0, 0]
return H[2, 0]
