import numpy as np
import plotly.graph_objects as go
from numpy.typing import NDArray
from perda.core_data_structures.data_instance import left_join_data_instances
from perda.core_data_structures.single_run_data import SingleRunData
from perda.units import _from_seconds, _to_seconds
from pydantic import BaseModel, ConfigDict, Field
from ..constants import G
from ..plotting.plot_2d import plot2D
from ..plotting.plot_polynomial_fit import *
from ..plotting.plotting_constants import (
DEFAULT_FONT_CONFIG,
DEFAULT_LAYOUT_CONFIG,
FontConfig,
LayoutConfig,
)
from .kinematics import BODY_LAT_ACC, GROUND_SPEED
from .steering import BODY_FRAME_STEER_ANGLE, FRONT_BICYCLE_MODEL_STEER_ANGLE
[docs]
class UndersteerResult(BaseModel):
"""Output of ``measure_understeer_gradient``.
Holds the fitted polynomial coefficients and goodness-of-fit statistics,
plus the raw filtered arrays needed to build a diagnostic plot.
"""
model_config = ConfigDict(arbitrary_types_allowed=True)
coeffs: NDArray[np.float64] = Field(
description="Polynomial coefficients in descending power order (for np.polyval)"
)
r_squared: float = Field(description="Coefficient of determination for the fit")
normalized_residual_std: float = Field(
description="Residual standard deviation normalized by the x spread (deg/g)"
)
lateral_accel: NDArray[np.float64] = Field(
description="Lateral acceleration of points used for fitting, filtered by min_lat_accel_g (g)"
)
understeer_angle: NDArray[np.float64] = Field(
description="Understeer angle of points used for fitting (deg)"
)
timestamps: NDArray[np.float64] = Field(
description="Timestamps of the filtered fit points (s)"
)
timestamps_full: NDArray[np.float64] = Field(
description="Timestamps for the full analysis window, used for diagnostic time-series (s)"
)
steering_wheel_angles: NDArray[np.float64] = Field(
description="Steering wheel angle over the full analysis window (deg)"
)
groundspeeds: NDArray[np.float64] = Field(
description="Ground speed over the full analysis window (m/s)"
)
@property
def K(self) -> float:
"""Linear understeer gradient (degree-1 coefficient). Units: deg/g."""
# coeffs is descending: [c_n, ..., c_1, c_0], so c_1 is at index -2
return float(self.coeffs[-2]) if len(self.coeffs) > 1 else 0.0
@property
def intercept(self) -> float:
"""Intercept (degree-0 coefficient). Units: deg."""
return float(self.coeffs[-1]) if len(self.coeffs) > 0 else 0.0
[docs]
def measure_understeer_gradient(
data: SingleRunData,
time_range: tuple[float, float],
fit_degree: int = 1,
symmetric: bool = False,
min_lat_accel_g: float = 0.1,
force_zero_intercept: bool = False,
steering_wheel_angle: str = "ludwig.steeringWheel.angle",
) -> UndersteerResult:
"""Measure the understeer gradient K from a steady-state test window.
Computes ``understeer_angle = |delta_bicycle| - |delta_kinematic|``
and fits a polynomial of ``fit_degree`` against ``|a_y|`` in g's.
The degree-1 coefficient is the understeer gradient K (deg/g).
K > 0 = understeer, K < 0 = oversteer.
Requires ``add_groundspeed``, ``add_curvature``, ``add_accelerations``,
``add_kinematic_steer_angle``, and ``add_bicycle_steer_angle`` to have
been run first.
Parameters
----------
data : SingleRunData
time_range : (t_start_s, t_end_s)
Analysis window in seconds.
fit_degree : int
Maximum polynomial degree to use when fitting curve
1 = linear, 2 = quadratic, 3 = cubic, etc.
symmetric : bool
Use only even powers (0, 2, 4, ...) in the fit to enforce symmetry around a_y=0.
force_zero_intercept : bool
Force the fit through the origin.
min_lat_accel_g : float
Minimum lateral acceleration (Units: g) to include in the fit.
steering_wheel_angle : str
Steering wheel angle channel name.
Returns
-------
UndersteerResult
"""
required = [
steering_wheel_angle,
BODY_LAT_ACC,
BODY_FRAME_STEER_ANGLE,
FRONT_BICYCLE_MODEL_STEER_ANGLE,
GROUND_SPEED,
]
missing = [c for c in required if c not in data]
if missing:
raise KeyError(
f"measure_understeer_gradient: missing variable(s) {missing}.\n"
f"Try running the following steps first: [add_groundspeed, add_track_frame_velocities,\n"
f"add_curvature, add_accelerations, add_kinematic_steer_angle, add_bicycle_steer_angle]"
)
# Slice all channels to the requested time window
ts0 = int(_from_seconds(time_range[0], data.timestamp_unit))
ts1 = int(_from_seconds(time_range[1], data.timestamp_unit))
bicycle_di = data[FRONT_BICYCLE_MODEL_STEER_ANGLE].trim(ts0, ts1)
if len(bicycle_di) == 0:
raise ValueError(
f"No data in time_range=({time_range[0]}, {time_range[1]}) s. "
f"Check that the window overlaps the log."
)
# Align all channels onto bicycle timestamps
(
bicycle_di,
body_frame_steer_angle,
body_frame_lat_accel,
steering_wheel_angles,
groundspeeds,
) = left_join_data_instances(
bicycle_di,
[
data[BODY_FRAME_STEER_ANGLE].trim(ts0, ts1),
data[BODY_LAT_ACC].trim(ts0, ts1),
data[steering_wheel_angle].trim(ts0, ts1),
data[GROUND_SPEED].trim(ts0, ts1),
],
)
# Compute understeer angle and lateral acceleration
understeer_deg = np.abs(bicycle_di.value_np) - np.abs(
body_frame_steer_angle.value_np
)
lateral_accel_raw_g = np.abs(body_frame_lat_accel.value_np) / G
timestamps = _to_seconds(bicycle_di.timestamp_np, data.timestamp_unit)
# Filter out low-accel noise
mask = lateral_accel_raw_g >= min_lat_accel_g
lateral_accel_filtered_g = lateral_accel_raw_g[mask]
understeer_filtered = understeer_deg[mask]
timestamps_filtered = timestamps[mask]
if len(lateral_accel_filtered_g) < 10:
raise ValueError(
f"Only {len(lateral_accel_filtered_g)} points above min_lat_accel_g={min_lat_accel_g} g "
f"in window ({time_range[0]}, {time_range[1]}) s. Insufficient data for fitting."
)
# Build the set of polynomial powers to fit
powers = [
p
for p in range(fit_degree + 1)
if (
not symmetric or p % 2 == 0
) # When symmetric=True, only include even powers
and (
not (force_zero_intercept and p == 0)
) # When force_zero_intercept=True, exclude the constant term (power 0)
]
if not powers:
raise ValueError("No valid polynomial powers after applying constraints.")
# Least squares fit
regression_matrix = np.column_stack([lateral_accel_filtered_g**p for p in powers])
sol, *_ = np.linalg.lstsq(regression_matrix, understeer_filtered, rcond=None)
# Build full coefficient vector (descending powers)
coeffs = np.zeros(fit_degree + 1, dtype=np.float64)
for p, c in zip(powers, sol):
coeffs[fit_degree - p] = c
# Evaluate fit
understeer_predicted = np.polyval(coeffs, lateral_accel_filtered_g)
ss_res = np.sum((understeer_filtered - understeer_predicted) ** 2)
ss_tot = np.sum((understeer_filtered - np.mean(understeer_filtered)) ** 2)
r_squared = 1.0 - ss_res / ss_tot if ss_tot > 0 else 0.0
n = len(lateral_accel_filtered_g)
k = len(powers)
dof = n - k
if dof <= 0:
normalized_residual_std = float("inf")
else:
residual_std = np.sqrt(ss_res / dof)
x_ss_tot = np.sum(
(lateral_accel_filtered_g - lateral_accel_filtered_g.mean()) ** 2
)
normalized_residual_std = residual_std / np.sqrt(x_ss_tot)
result = UndersteerResult(
coeffs=coeffs,
r_squared=r_squared,
normalized_residual_std=normalized_residual_std,
lateral_accel=lateral_accel_filtered_g.astype(np.float64),
understeer_angle=understeer_filtered.astype(np.float64),
timestamps=timestamps_filtered.astype(np.float64),
timestamps_full=timestamps.astype(np.float64),
steering_wheel_angles=steering_wheel_angles.value_np.astype(np.float64),
groundspeeds=groundspeeds.value_np.astype(np.float64),
)
return result
[docs]
def plot_understeer(
result: UndersteerResult,
test_type: str = "constant_steer",
font_config: FontConfig = DEFAULT_FONT_CONFIG,
layout_config: LayoutConfig = DEFAULT_LAYOUT_CONFIG,
) -> tuple[go.Figure, go.Figure]:
"""Return (fit_fig, diagnostic_fig) for an UndersteerResult.
fit_fig: scatter + polynomial fit of understeer angle vs lateral acceleration.
diagnostic_fig: time-series of the held-constant channel over the analysis window.
Parameters
----------
result : UndersteerResult
test_type : str
``"constant_steer"`` or ``"constant_speed"``. Determines which channel
is shown in the diagnostic figure.
font_config : FontConfig
layout_config : LayoutConfig
"""
eq = format_polynomial_equation(result.coeffs)
if test_type == "constant_steer":
diag_vals, diag_label = result.steering_wheel_angles, "Steering Angle (deg)"
else:
diag_vals, diag_label = result.groundspeeds, "Speed (m/s)"
fit_fig = plot_polynomial_fit(
x=result.lateral_accel,
y=result.understeer_angle,
timestamps=result.timestamps,
coeffs=result.coeffs,
title="Understeer Gradient",
x_label="Lateral Acceleration (g)",
y_label="Understeer Angle (deg)",
subtitle=f"{eq} (R-squared = {result.r_squared:.3f})",
font_config=font_config,
layout_config=layout_config,
)
diag_fig = plot2D(
x_list=result.timestamps_full,
y_list=diag_vals,
title=f"{diag_label}",
x_axis="Time (s)",
y_axis=diag_label,
font_config=font_config,
layout_config=layout_config,
)
return fit_fig, diag_fig