import numpy as np
from perda.core_data_structures.data_instance import (
DataInstance,
left_join_data_instances,
)
from perda.core_data_structures.single_run_data import SingleRunData
from perda.units import _to_seconds
from .preprocess_gps import POS_X, POS_Y
from .utils import safe_gradient
GROUND_SPEED = "groundSpeed"
CUMULATIVE_DISTANCE = "lapDist"
VEL_N = "velN"
VEL_E = "velE"
VEL_D = "velD"
TRACK_VELOCITY_X = "track.velocity.x"
TRACK_VELOCITY_Y = "track.velocity.y"
TRACK_HEADING = "track.heading"
BODY_SLIP_ANGLE = "body.slipAngle"
CURVATURE_BODY = "body.curvature"
RADIUS_BODY = "body.radius"
CURVATURE_TRACK = "track.curvature"
RADIUS_TRACK = "track.radius"
BODY_LAT_ACC = "body.accLat"
TRACK_LAT_ACC = "track.accLat"
[docs]
def add_groundspeed(
data: SingleRunData,
fl_col: str = "pcm.wheelSpeeds.frontLeft",
fr_col: str = "pcm.wheelSpeeds.frontRight",
) -> SingleRunData:
"""
Add ground speed as the average of the two front (undriven) wheel speeds. Stores it
in the variable ``groundSpeed``, accessible via the constant ``GROUND_SPEED``.
Requires the ``convert_wheelspeeds_to_m_per_s`` preprocessing step.
Stores ``groundSpeed``.
Parameters
----------
data : SingleRunData
fl_col : str
Column name for front left wheel speed in m/s.
fr_col : str
Column name for front right wheel speed in m/s.
Returns
-------
SingleRunData
"""
gs = (data[fl_col] + data[fr_col]) / 2.0
data[GROUND_SPEED] = DataInstance(
timestamp_np=gs.timestamp_np,
value_np=gs.value_np,
label="Ground Speed (m/s)",
cpp_name=GROUND_SPEED,
)
return data
[docs]
def add_cumulative_distance(data: SingleRunData) -> SingleRunData:
"""
Add cumulative arc-length distance to the data. Uses the same distance calculation as
track_factory.from_perda_logs, to ensure consistency when comparing laps against a track
generated from GPS data.
Stores the cumulative distance in the variable ``lapDist``, accessible via the constant
``CUMULATIVE_DISTANCE``.
Parameters
----------
data : SingleRunData
Input data containing position information.
Returns
-------
SingleRunData
"""
missing = [c for c in (POS_X, POS_Y) if c not in data]
if missing:
raise KeyError(
f"add_cumulative_distance: missing variables(s) {missing}.\n"
f"Try running preprocess_gps_data_instances first."
)
pos_x_ref, pos_y_aligned = left_join_data_instances(data[POS_X], data[POS_Y])
dX = np.diff(pos_x_ref.value_np, prepend=pos_x_ref.value_np[0])
dY = np.diff(pos_y_aligned.value_np, prepend=pos_y_aligned.value_np[0])
arc_steps = np.sqrt(dX**2 + dY**2)
arc_steps[0] = 0.0
cumulative_dist_raw = np.cumsum(arc_steps)
data[CUMULATIVE_DISTANCE] = DataInstance(
timestamp_np=pos_x_ref.timestamp_np,
value_np=cumulative_dist_raw,
label="Cumulative Arc-Length (m)",
cpp_name=CUMULATIVE_DISTANCE,
)
return data
[docs]
def add_ned_velocities(
data: SingleRunData,
vel_x_col: str = "pcm.vnav.velocityBody.x",
vel_y_col: str = "pcm.vnav.velocityBody.y",
vel_z_col: str = "pcm.vnav.velocityBody.z",
yaw_col: str = "pcm.vnav.yawPitchRoll.yaw",
) -> SingleRunData:
"""
Add NED velocity components by using body-frame velocity (FRD) and yaw rotation.
velN = v_fwd * cos(yaw) - v_right * sin(yaw)
velE = v_fwd * sin(yaw) + v_right * cos(yaw)
velD = v_down (unchanged)
Stores results in ``velN``, ``velE``, and ``velD``, accessible via the constants
``VEL_N``, ``VEL_E``, and ``VEL_D``.
Parameters
----------
data : SingleRunData
Input data containing position information.
Returns
-------
SingleRunData
Notes
------
In older data, the VectorNav was configured such that velocityBody columns contained NED velocities.
Such data can be patched with the ``patch_ned_velocity`` preprocessing step from PERDA. In that case,
this function is unnecessary and roughly a no-op.
This function intentionally follows PERDA's naming convention of velN, velE, and velD for NED velocity components.
"""
v_fwd, v_right, v_down, yaw = left_join_data_instances(
data[vel_x_col], [data[vel_y_col], data[vel_z_col], data[yaw_col]]
)
yaw_rad = np.radians(yaw.value_np)
cos_y = np.cos(yaw_rad)
sin_y = np.sin(yaw_rad)
vel_n = v_fwd.value_np * cos_y - v_right.value_np * sin_y
vel_e = v_fwd.value_np * sin_y + v_right.value_np * cos_y
vel_d = v_down.value_np
data[VEL_N] = DataInstance(
timestamp_np=v_fwd.timestamp_np,
value_np=vel_n,
label="North Velocity (m/s)",
cpp_name=VEL_N,
)
data[VEL_E] = DataInstance(
timestamp_np=v_fwd.timestamp_np,
value_np=vel_e,
label="East Velocity (m/s)",
cpp_name=VEL_E,
)
data[VEL_D] = DataInstance(
timestamp_np=v_fwd.timestamp_np,
value_np=vel_d,
label="Down Velocity (m/s)",
cpp_name=VEL_D,
)
return data
[docs]
def add_track_frame_velocities(
data: SingleRunData,
vel_n_col: str = "velN",
vel_e_col: str = "velE",
yaw_col: str = "pcm.vnav.yawPitchRoll.yaw",
low_speed_thresh: float = 3.0,
) -> SingleRunData:
"""Add track-frame speed and course heading from NED velocity components.
Track heading uses the direction of travel (0 = North, 90 = East, wraps +/-180°).
Below ``low_speed_thresh`` heading falls back to INS yaw to avoid noise.
Stores results in ``track.velocity.x``, ``track.velocity.y``, and ``track.heading``,
accesible via the constants ``TRACK_VELOCITY_X``, ``TRACK_VELOCITY_Y``, and ``TRACK_HEADING``.
Parameters
----------
low_speed_thresh : float
Speed (m/s) below which heading is replaced with INS yaw.
"""
missing = [c for c in (vel_n_col, vel_e_col, yaw_col) if c not in data]
if missing:
raise KeyError(
f"add_track_frame_velocities: missing variables(s) {missing}.\n"
f"Try running add_ned_velocities first"
)
vel_n = data[vel_n_col]
vel_n, vel_e_aln, yaw_aln = left_join_data_instances(
vel_n, [data[vel_e_col], data[yaw_col]]
)
ts = vel_n.timestamp_np
vn, ve = vel_n.value_np, vel_e_aln.value_np
speed = np.sqrt(vn**2 + ve**2)
heading = np.degrees(np.arctan2(ve, vn))
heading[speed < low_speed_thresh] = yaw_aln.value_np[speed < low_speed_thresh]
data[TRACK_VELOCITY_X] = DataInstance(
timestamp_np=ts,
value_np=speed,
label="Track Frame Speed (m/s)",
cpp_name=TRACK_VELOCITY_X,
)
data[TRACK_VELOCITY_Y] = DataInstance(
timestamp_np=ts,
value_np=np.zeros_like(speed),
label="Track Frame Lateral Speed (m/s)",
cpp_name=TRACK_VELOCITY_Y,
)
data[TRACK_HEADING] = DataInstance(
timestamp_np=ts,
value_np=heading,
label="Track Heading (deg)",
cpp_name=TRACK_HEADING,
)
return data
[docs]
def add_body_slip_angle(
data: SingleRunData,
yaw_col: str = "pcm.vnav.yawPitchRoll.yaw",
vel_body_col: str = "pcm.vnav.velocityBody.x",
low_speed_thresh: float = 3.0,
) -> SingleRunData:
"""Add body slip angle (beta) using SAE convention: beta = track_heading - yaw
and stores it in the variable ``body.slipAngle``, accessible via the constant ``BODY_SLIP_ANGLE``.
Zeroed below ``low_speed_thresh`` where course heading is unreliable.
Requires ``add_track_frame_velocities``.
Parameters
----------
low_speed_thresh : float
Speed (m/s) below which slip angle is forced to zero.
"""
missing = [c for c in (TRACK_HEADING, yaw_col, vel_body_col) if c not in data]
if missing:
raise KeyError(
f"add_body_slip_angle: missing variables(s) {missing}.\n"
f"Try running add_track_frame_velocities first."
)
th = data[TRACK_HEADING]
th, yaw_aln, vbx_aln = left_join_data_instances(
th, [data[yaw_col], data[vel_body_col]]
)
slip = (th.value_np - yaw_aln.value_np + 180.0) % 360.0 - 180.0
slip[np.abs(vbx_aln.value_np) < low_speed_thresh] = 0.0
data[BODY_SLIP_ANGLE] = DataInstance(
timestamp_np=th.timestamp_np,
value_np=slip,
label="Body Slip Angle (deg)",
cpp_name=BODY_SLIP_ANGLE,
)
return data
[docs]
def add_curvature(
data: SingleRunData,
yaw_rate_col: str = "pcm.vnav.compensatedAngularRate.z",
vel_body_col: str = "pcm.vnav.velocityBody.x",
low_speed_thresh: float = 3.0,
median_window: int = 8,
radius_clip: float = 500.0,
) -> SingleRunData:
"""Add path curvature and radius in body and track frames and stores results
in ``body.curvature``, ``body.radius``, ``track.curvature``, and ``track.radius``,
accessible via the constants ``CURVATURE_BODY``, ``RADIUS_BODY``, ``CURVATURE_TRACK``,
and ``RADIUS_TRACK``.
Body-frame curvature uses yaw rate / forward speed. Track-frame curvature
differentiates the track heading. Both are smoothed with a rolling median.
Requires ``add_track_frame_velocities``.
Parameters
----------
low_speed_thresh : float
Speed (m/s) below which curvature is forced to zero.
median_window : int
Rolling median half-window for noise suppression.
radius_clip : float
Maximum absolute radius (m) before clipping.
"""
missing = [c for c in (TRACK_HEADING, TRACK_VELOCITY_X) if c not in data]
if missing:
raise KeyError(
f"add_curvature: missing {missing}.\n"
f"Try running add_track_frame_velocities first."
)
# Body frame: curvature = yaw_rate / v_body
yr = data[yaw_rate_col]
yr, vbx_aln = left_join_data_instances(yr, [data[vel_body_col]])
ts_body = yr.timestamp_np
speed_body = vbx_aln.value_np
curv_body_raw = np.zeros_like(speed_body, dtype=np.float64)
mask = np.abs(speed_body) > low_speed_thresh
curv_body_raw[mask] = yr.value_np[mask] / speed_body[mask]
half = median_window // 2
curv_body = np.array(
[
np.median(curv_body_raw[max(0, i - half) : i + half + 1])
for i in range(len(curv_body_raw))
]
)
r_body = np.full_like(curv_body, radius_clip, dtype=np.float64)
mask = curv_body != 0
r_body[mask] = 1.0 / curv_body[mask]
r_body = np.clip(r_body, -radius_clip, radius_clip)
data[CURVATURE_BODY] = DataInstance(
timestamp_np=ts_body,
value_np=curv_body,
label="Body Frame Curvature (1/m)",
cpp_name=CURVATURE_BODY,
)
data[RADIUS_BODY] = DataInstance(
timestamp_np=ts_body,
value_np=r_body,
label="Body Frame Radius (m)",
cpp_name=RADIUS_BODY,
)
# Track frame: curvature = d(heading)/dt / v_track
th = data[TRACK_HEADING]
th, vtx_aln = left_join_data_instances(th, [data[TRACK_VELOCITY_X]])
ts_track = th.timestamp_np
t_s = _to_seconds(ts_track, data.timestamp_unit)
heading_rate = safe_gradient(np.unwrap(np.radians(th.value_np)), np.gradient(t_s))
speed_track = vtx_aln.value_np
curv_track_raw = np.zeros_like(speed_track, dtype=np.float64)
mask = speed_track > low_speed_thresh
curv_track_raw[mask] = heading_rate[mask] / speed_track[mask]
curv_track = np.array(
[
np.median(curv_track_raw[max(0, i - half) : i + half + 1])
for i in range(len(curv_track_raw))
]
)
r_track = np.full_like(curv_track, radius_clip, dtype=np.float64)
mask = curv_track != 0
r_track[mask] = 1.0 / curv_track[mask]
r_track = np.clip(r_track, -radius_clip, radius_clip)
data[CURVATURE_TRACK] = DataInstance(
timestamp_np=ts_track,
value_np=curv_track,
label="Track Frame Curvature (1/m)",
cpp_name=CURVATURE_TRACK,
)
data[RADIUS_TRACK] = DataInstance(
timestamp_np=ts_track,
value_np=r_track,
label="Track Frame Radius (m)",
cpp_name=RADIUS_TRACK,
)
return data
[docs]
def add_accelerations(
data: SingleRunData,
vel_body_col: str = "pcm.vnav.velocityBody.x",
) -> SingleRunData:
"""Add lateral acceleration as v^2 * curvature in body and track frames and
stores results in ``body.accLat`` and ``track.accLat``, accessible via the constants
``BODY_LAT_ACC`` and ``TRACK_LAT_ACC``.
Requires ``add_curvature`` and ``add_track_frame_velocities``.
"""
missing = [
c
for c in (vel_body_col, CURVATURE_BODY, TRACK_VELOCITY_X, CURVATURE_TRACK)
if c not in data
]
if missing:
raise KeyError(
f"add_accelerations: missing variables(s) {missing}.\n"
f"Try running add_curvature and add_track_frame_velocities first."
)
curv_body = data[CURVATURE_BODY]
curv_body, vbx_aln = left_join_data_instances(curv_body, [data[vel_body_col]])
data[BODY_LAT_ACC] = DataInstance(
timestamp_np=curv_body.timestamp_np,
value_np=vbx_aln.value_np**2 * curv_body.value_np,
label="Body Lateral Acceleration (m/s²)",
cpp_name=BODY_LAT_ACC,
)
curv_track = data[CURVATURE_TRACK]
curv_track, vtx_aln = left_join_data_instances(curv_track, [data[TRACK_VELOCITY_X]])
data[TRACK_LAT_ACC] = DataInstance(
timestamp_np=curv_track.timestamp_np,
value_np=vtx_aln.value_np**2 * curv_track.value_np,
label="Track Lateral Acceleration (m/s²)",
cpp_name=TRACK_LAT_ACC,
)
return data