import numpy as np
from numpy.typing import NDArray
from perda.core_data_structures import (
DataInstance,
ResampleMethod,
SingleRunData,
left_join_data_instances,
trim_single_run_data,
)
from perda.units import from_seconds, to_seconds
from scipy.signal import butter, sosfiltfilt
from .kinematics import BODY_SLIP_ANGLE
from .utils import estimate_fs
from .yaw_segmentation_models import (
MaskReport,
StationarityCell,
StationarityReport,
SubwindowSummary,
)
AX_FROM_SPEED = "ax.fromSpeed"
[docs]
def add_ax_from_speed(
data: SingleRunData,
speed_col: str = "pcm.vnav.velocityBody.x",
out_col: str = AX_FROM_SPEED,
smooth_hz: float = 2.0,
) -> SingleRunData:
"""Add longitudinal acceleration as the time derivative of speed.
Differentiation amplifies high-frequency noise, so the derivative is
lowpass-filtered at ``smooth_hz`` (zero-phase, so no group delay is
introduced). Duplicate and out-of-order timestamps are repaired first --
a single bad sample would otherwise poison the whole signal through the
filter.
Parameters
----------
data : SingleRunData
speed_col : str
Source speed channel (m/s).
out_col : str
Destination acceleration channel (m/s^2).
smooth_hz : float
Lowpass cutoff (Hz); 0 disables smoothing.
Returns
-------
SingleRunData
``data``, with ``out_col`` added, returned for chaining.
"""
if speed_col not in data:
raise KeyError(f"add_ax_from_speed: missing variable '{speed_col}'.")
di = data[speed_col]
ts_raw = di.timestamp_np
t_s = np.asarray(to_seconds(ts_raw.astype(np.float64), data.timestamp_unit), dtype=np.float64)
v = di.value_np.astype(np.float64)
order = np.argsort(t_s, kind="stable")
t_sorted, v_sorted = t_s[order], v[order]
keep = np.concatenate(([True], np.diff(t_sorted) > 0))
t_clean, v_clean = t_sorted[keep], v_sorted[keep]
ax_clean = np.gradient(v_clean, t_clean)
if smooth_hz:
fs = estimate_fs(t_clean)
nyquist = 0.5 * fs
if fs > 0 and smooth_hz < nyquist:
sos = butter(N=4, Wn=smooth_hz / nyquist, btype="low", output="sos")
ax_clean = sosfiltfilt(sos, ax_clean)
data[out_col] = DataInstance(
timestamp_np=ts_raw,
value_np=np.interp(t_s, t_clean, ax_clean),
label="Longitudinal Acceleration from Speed (m/s^2)",
cpp_name=out_col,
)
return data
[docs]
def scan_speed_stationarity(
data: SingleRunData,
win_lengths_s: tuple[float, ...] = (3.0, 5.0, 8.0, 12.0, 20.0),
tol_pcts: tuple[float, ...] = (2.5, 5.0, 10.0, 15.0),
speed_col: str = "pcm.vnav.velocityBody.x",
overlap: float = 0.5,
) -> StationarityReport:
"""Measure how often the car holds an approximately constant speed.
For each ``(window length, tolerance)`` pair, count the fraction of sliding
windows whose peak-to-peak speed stays within ``tolerance`` percent of the
window mean. Near-zero fractions at the long windows are the evidence that
a nonparametric FRF will not work on this data.
Parameters
----------
data : SingleRunData
win_lengths_s : tuple of float
Sliding-window lengths to test (s).
tol_pcts : tuple of float
Allowed peak-to-peak speed variation, as a percent of the window mean.
speed_col : str
overlap : float
Window overlap fraction (0 = none, 0.5 = 50%).
Returns
-------
StationarityReport
Notes
-----
Lapping data is not a frequency-response test: the car is accelerating,
braking, and occasionally sliding. This scan quantifies how rarely the
speed is even approximately constant, and is the justification for
identifying the yaw response with a parametric LPV output-error fit
(``yaw_fit``) rather than a nonparametric Welch FRF, which would need
long constant-speed holds this data does not contain.
"""
if speed_col not in data:
raise KeyError(f"scan_speed_stationarity: missing variable '{speed_col}'.")
t_s = np.asarray(
to_seconds(data[speed_col].timestamp_np.astype(np.float64), data.timestamp_unit),
dtype=np.float64,
)
v = data[speed_col].value_np.astype(np.float64)
fs = estimate_fs(t_s)
if fs <= 0:
raise ValueError("Could not estimate sample rate from the speed timestamps.")
cells: list[StationarityCell] = []
for window_s in win_lengths_s:
n_win = max(2, int(round(window_s * fs)))
step = max(1, int(round(n_win * (1.0 - overlap))))
if n_win > len(v):
cells.extend(
StationarityCell(
window_s=window_s,
tol_pct=tol,
n_windows=0,
n_passing=0,
fraction_passing=float("nan"),
)
for tol in tol_pcts
)
continue
starts = np.arange(0, len(v) - n_win + 1, step)
means = np.array([v[s : s + n_win].mean() for s in starts])
ranges = np.array([np.ptp(v[s : s + n_win]) for s in starts])
# A window straddling a standstill has a near-zero mean; its relative
# variation is meaningless rather than infinite.
safe_means = np.where(np.abs(means) < 1e-3, np.nan, means)
relative_pp = 100.0 * ranges / safe_means
for tol in tol_pcts:
n_passing = int(np.sum(relative_pp <= tol))
cells.append(
StationarityCell(
window_s=window_s,
tol_pct=tol,
n_windows=int(len(starts)),
n_passing=n_passing,
fraction_passing=n_passing / len(starts),
)
)
return StationarityReport(speed_col=speed_col, overlap=overlap, cells=cells)
def _contiguous_runs(mask: NDArray[np.bool_]) -> list[tuple[int, int]]:
"""``[start, end)`` index pairs for each run of True in ``mask``."""
if mask.size == 0:
return []
diff = np.diff(mask.astype(np.int8))
starts = list(np.where(diff == 1)[0] + 1)
ends = list(np.where(diff == -1)[0] + 1)
if mask[0]:
starts.insert(0, 0)
if mask[-1]:
ends.append(mask.size)
return list(zip(starts, ends))
[docs]
def auto_segment(
data: SingleRunData,
manual_windows: list[tuple[float, float]],
min_speed_mps: float = 4.0,
max_slip_deg: float = 7.0,
min_window_s: float = 8.0,
speed_col: str = "pcm.vnav.velocityBody.x",
slip_col: str = BODY_SLIP_ANGLE,
) -> tuple[list[SingleRunData], list[MaskReport]]:
"""Split each coarse window into clean sub-windows by masking.
Within each window, samples below ``min_speed_mps`` (the car is not really
driving, and every 1/V term blows up) or above ``max_slip_deg`` of body
slip (the tires are past the linear region the bicycle model describes) are
dropped. Surviving runs shorter than ``min_window_s`` carry too little
low-frequency content to identify anything and are dropped too.
Parameters
----------
data : SingleRunData
manual_windows : list of (t_start, t_end)
Coarse windows (s), typically read off the GPS trajectory.
min_speed_mps, max_slip_deg, min_window_s : float
Mask and length thresholds.
speed_col, slip_col : str
Returns
-------
subwindows : list of SingleRunData
A flat list of every accepted sub-window, across all manual windows.
reports : list of MaskReport
One report per manual window.
"""
for col in (speed_col, slip_col):
if col not in data:
raise KeyError(
f"auto_segment: missing variable '{col}'. Run add_groundspeed / "
"add_track_frame_velocities / add_body_slip_angle first."
)
speed_di, slip_di = left_join_data_instances(data[speed_col], [data[slip_col]])
t_s = np.asarray(
to_seconds(speed_di.timestamp_np.astype(np.float64), data.timestamp_unit),
dtype=np.float64,
)
v = speed_di.value_np.astype(np.float64)
slip = slip_di.value_np.astype(np.float64)
subwindows: list[SingleRunData] = []
reports: list[MaskReport] = []
for w_start, w_end in manual_windows:
in_window = (t_s >= w_start) & (t_s <= w_end)
if not in_window.any():
reports.append(
MaskReport(
manual_window=(w_start, w_end),
n_input_pts=0,
n_kept_pts=0,
n_low_speed_pts=0,
n_high_slip_pts=0,
sub_windows=[],
rejected_short=[],
)
)
continue
idx = np.where(in_window)[0]
v_w, slip_w, t_w = v[idx], slip[idx], t_s[idx]
low_speed = v_w < min_speed_mps
high_slip = np.abs(slip_w) > max_slip_deg
valid = ~(low_speed | high_slip | np.isnan(v_w) | np.isnan(slip_w))
accepted: list[tuple[float, float]] = []
rejected: list[tuple[float, float]] = []
for start, end in _contiguous_runs(valid):
t_start, t_end = float(t_w[start]), float(t_w[end - 1])
if (t_end - t_start) < min_window_s:
rejected.append((t_start, t_end))
continue
subwindows.append(
trim_single_run_data(
data,
float(from_seconds(t_start, data.timestamp_unit)),
float(from_seconds(t_end, data.timestamp_unit)),
)
)
accepted.append((t_start, t_end))
reports.append(
MaskReport(
manual_window=(w_start, w_end),
n_input_pts=int(in_window.sum()),
n_kept_pts=int(valid.sum()),
n_low_speed_pts=int(low_speed.sum()),
n_high_slip_pts=int(high_slip.sum()),
sub_windows=accepted,
rejected_short=rejected,
)
)
return subwindows, reports
[docs]
def resample_subwindows(
subwindows: list[SingleRunData],
target_hz: float = 100.0,
method: ResampleMethod = ResampleMethod.LINEAR,
) -> list[SingleRunData]:
"""Put every sub-window channel on a common uniform grid at ``target_hz``.
Raw logs run near 1 kHz with dt jitter, but the yaw mode is only a few Hz
wide, so 100 Hz is ample and roughly ten times cheaper to simulate. Landing
every channel on the *same* grid also makes the later left-join a no-op.
Parameters
----------
subwindows : list of SingleRunData
Output of ``auto_segment``.
target_hz : float
method : ResampleMethod
Returns
-------
list of SingleRunData
New sub-windows on the uniform grid. Channels with fewer than two
samples are dropped, and a sub-window left with no channels at all is
omitted.
"""
if target_hz <= 0:
raise ValueError(f"target_hz must be > 0, got {target_hz}")
out: list[SingleRunData] = []
for sub in subwindows:
resampled = SingleRunData(
id_to_instance={},
cpp_name_to_id={},
id_to_cpp_name={},
id_to_descript={},
total_data_points=0,
data_start_time=sub.data_start_time,
data_end_time=sub.data_end_time,
timestamp_unit=sub.timestamp_unit,
concat_boundaries=[],
)
for cpp_name in sub.cpp_name_to_id:
di = sub[cpp_name]
if len(di.value_np) < 2:
continue
resampled[cpp_name] = di.resample_to_freq(target_hz, sub.timestamp_unit, method)
if resampled.cpp_name_to_id:
out.append(resampled)
return out
[docs]
def summarize_subwindows(
subwindows: list[SingleRunData],
ax_col: str = AX_FROM_SPEED,
input_col: str = "ludwig.steeringWheel.angle",
speed_col: str = "pcm.vnav.velocityBody.x",
slip_col: str = BODY_SLIP_ANGLE,
) -> list[SubwindowSummary]:
"""Per-sub-window quality summary, for triaging windows before fitting.
Duration and steering excitation say whether a window can identify
anything; speed hold and ``|a_x|`` say how hard the LPV scheduling is
working; slip RMS says how close to the linear region it stayed.
Parameters
----------
subwindows : list of SingleRunData
ax_col, input_col, speed_col, slip_col : str
Channels to summarize. Any that are absent are reported as NaN.
Returns
-------
list of SubwindowSummary
"""
summaries: list[SubwindowSummary] = []
for i, sub in enumerate(subwindows):
reference = next(iter(sub.cpp_name_to_id))
t_s = np.asarray(
to_seconds(sub[reference].timestamp_np.astype(np.float64), sub.timestamp_unit),
dtype=np.float64,
)
row = SubwindowSummary(
sub_index=i,
n_pts=len(sub[reference].value_np),
duration_s=float(t_s[-1] - t_s[0]) if len(t_s) > 1 else 0.0,
fs_hz=estimate_fs(t_s),
)
if speed_col in sub:
speed = sub[speed_col].value_np.astype(np.float64)
mean = float(np.nanmean(speed))
row.speed_mean = mean
if mean != 0:
row.speed_pp_pct = 100.0 * (np.nanmax(speed) - np.nanmin(speed)) / abs(mean)
if ax_col in sub:
ax = sub[ax_col].value_np.astype(np.float64)
row.ax_rms = float(np.sqrt(np.nanmean(ax**2)))
if input_col in sub:
steer = sub[input_col].value_np.astype(np.float64)
row.input_pp = float(np.nanmax(steer) - np.nanmin(steer))
if slip_col in sub:
slip = sub[slip_col].value_np.astype(np.float64)
row.slip_rms = float(np.sqrt(np.nanmean(slip**2)))
summaries.append(row)
return summaries