74 lines
2.8 KiB
Python
74 lines
2.8 KiB
Python
"""GNHPR attitude conventions and SO(3) interpolation."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
|
|
import numpy as np
|
|
from scipy.spatial.transform import Rotation, Slerp
|
|
|
|
@dataclass(frozen=True)
|
|
class GnhprConvention:
|
|
"""Interpretation of GNHPR angles as ``R_ENU_RTK``.
|
|
|
|
Heading is normally clockwise from north. With ENU and an x-forward RTK
|
|
frame this becomes yaw ``90 deg - heading``. Aircraft-positive pitch is
|
|
nose-up, which is the negative mathematical Y rotation in an FLU frame.
|
|
Alternative signs are retained for empirical protocol validation.
|
|
"""
|
|
|
|
name: str
|
|
heading_sign: float = -1.0
|
|
pitch_sign: float = -1.0
|
|
roll_sign: float = 1.0
|
|
|
|
|
|
EXPECTED_GNHPR = GnhprConvention("north_cw__pitch_nose_up__roll_right_down")
|
|
GNHPR_CANDIDATES = (
|
|
EXPECTED_GNHPR,
|
|
GnhprConvention("north_cw__pitch_opposite", -1.0, 1.0, 1.0),
|
|
GnhprConvention("heading_opposite__pitch_nose_up", 1.0, -1.0, 1.0),
|
|
GnhprConvention("heading_opposite__pitch_opposite", 1.0, 1.0, 1.0),
|
|
)
|
|
|
|
|
|
def gnhpr_to_rotation_enu_rtk(
|
|
heading_deg: np.ndarray,
|
|
pitch_deg: np.ndarray,
|
|
roll_deg: np.ndarray,
|
|
convention: GnhprConvention = EXPECTED_GNHPR,
|
|
) -> np.ndarray:
|
|
"""Build body-to-ENU matrices with an extrinsic Z-Y-X Euler sequence."""
|
|
|
|
heading = np.asarray(heading_deg, dtype=float).reshape(-1)
|
|
pitch = np.asarray(pitch_deg, dtype=float).reshape(-1)
|
|
roll = np.asarray(roll_deg, dtype=float).reshape(-1)
|
|
if not (heading.size == pitch.size == roll.size):
|
|
raise ValueError("heading, pitch and roll must have equal length")
|
|
yaw_rad = np.deg2rad(90.0 + convention.heading_sign * heading)
|
|
pitch_rad = np.deg2rad(convention.pitch_sign * pitch)
|
|
roll_rad = np.deg2rad(convention.roll_sign * roll)
|
|
angles = np.column_stack([yaw_rad, pitch_rad, roll_rad])
|
|
return Rotation.from_euler('ZYX', angles).as_matrix()
|
|
|
|
|
|
def interpolate_rotations(
|
|
source_t_s: np.ndarray,
|
|
rotations: np.ndarray,
|
|
query_t_s: np.ndarray,
|
|
) -> np.ndarray:
|
|
"""Slerp a monotonic SO(3) series without extrapolation."""
|
|
|
|
source_t = np.asarray(source_t_s, dtype=float).reshape(-1)
|
|
query_t = np.asarray(query_t_s, dtype=float).reshape(-1)
|
|
matrices = np.asarray(rotations, dtype=float).reshape(-1, 3, 3)
|
|
if source_t.size < 2 or matrices.shape[0] != source_t.size:
|
|
raise ValueError("need at least two timestamped rotations")
|
|
if np.any(np.diff(source_t) <= 0):
|
|
unique_t, unique_indices = np.unique(source_t, return_index=True)
|
|
source_t = unique_t
|
|
matrices = matrices[unique_indices]
|
|
if np.any(query_t < source_t[0]) or np.any(query_t > source_t[-1]):
|
|
raise ValueError("rotation interpolation does not extrapolate")
|
|
return Slerp(source_t, Rotation.from_matrix(matrices))(query_t).as_matrix()
|