diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 6840a6273..4b221115e 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -6,13 +6,10 @@ import cereal.messaging as messaging from cereal import car, log from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.common.params import Params -from openpilot.common.realtime import config_realtime_process, DT_MDL, DT_CTRL +from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose -import matplotlib.pyplot as plt -import pickle -plt.ioff() HISTORY = 5 # secs POINTS_PER_BUCKET = 1500 @@ -29,9 +26,8 @@ FRICTION_SANITY_QLOG = 0.8 STEER_MIN_THRESHOLD = 0.02 MIN_FILTER_DECAY = 50 MAX_FILTER_DECAY = 250 -LAT_ACC_THRESHOLD = 4 -# STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1), (0.1, 0.2), (0.2, 0.3), (0.3, 0.5)] -# MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100]) +LAT_ACC_THRESHOLD = 4 # m/s^2 maximum lateral acceleration allowed +LOOKBACK = 0.5 # secs for sensor standard deviation calculation STEER_BUCKET_BOUNDS = [ (-1.0, -0.9), (-0.9, -0.8), (-0.8, -0.7), (-0.7, -0.6), (-0.6, -0.5), @@ -46,52 +42,45 @@ MIN_BUCKET_POINTS = np.array([ 500, 300, 100, 100, 100, 100, 100, 100, ]) - MIN_ENGAGE_BUFFER = 2 # secs VERSION = 2 # bump this to invalidate old parameter caches ALLOWED_BRANDS = ['toyota', 'hyundai', 'rivian'] ALLOWED_CARS = ['MAZDA_3_2019'] -NON_LINEAR_TORQUE_PARAMS = { - "MAZDA_3_2019": (3.8818, 0.6873, 0.0999, 0.3605), -} +def sig_centered(z): + pos = 1.0 / (1.0 + np.exp(-z)) - 0.5 + neg = np.exp(z) / (1.0 + np.exp(z)) - 0.5 + return np.where(z >= 0.0, pos, neg) # branch-free vectorised -def sigmoid(z): - z = np.clip(z, -50.0, 50.0) # avoid overflow - return 1.0 / (1.0 + np.exp(-z)) def model(x, a, b, c, d): xs = x - d - return sigmoid(a * xs) * b + c * xs - 0.5 * b + return sig_centered(a * xs) * b + c * xs + def jacobian(x, a, b, c, d): xs = x - d - s = sigmoid(a * xs) - ds = s * (1.0 - s) # σ′ - # ∂f/∂a, ∂f/∂b, ∂f/∂c, ∂f/∂d (N×4) + # plain σ for derivative (cheaper than calling centred helper again) + s = 1.0 / (1.0 + np.exp(-np.clip(a * xs, -50.0, 50.0))) + ds = s * (1.0 - s) # σ′(z) + sc = s - 0.5 # (σ − 0.5) value + + # Cols: ∂f/∂a, ∂f/∂b, ∂f/∂c, ∂f/∂d (N × 4) return np.column_stack([ - b * ds * xs, # a-derivative - s - 0.5, # b-derivative - xs, # c-derivative - -b * a * ds - c # d-derivative + b * ds * xs, # a-derivative + sc, # b-derivative + xs, # c-derivative + -b * a * ds - c # d-derivative ]) + def slope2rot(slope): sin = np.sqrt(slope ** 2 / (slope ** 2 + 1)) cos = np.sqrt(1 / (slope ** 2 + 1)) return np.array([[cos, -sin], [sin, cos]]) -def op_friction(points): - try: - _, _, v = np.linalg.svd(points, full_matrices=False) - slope, _ = -v.T[0:2, 2] / v.T[2, 2] - _, spread = np.matmul(points[:, [0, 2]], slope2rot(slope)).T - friction_coeff = np.std(spread) * FRICTION_FACTOR - except np.linalg.LinAlgError as e: - return 0 - return friction_coeff class TorqueBuckets(PointBuckets): def add_point(self, x, y): @@ -103,7 +92,7 @@ class TorqueBuckets(PointBuckets): class TorqueEstimator(ParameterEstimator): def __init__(self, CP, decimated=False, track_all_points=False): - self.hist_len = int(HISTORY / DT_CTRL) + self.hist_len = int(HISTORY / DT_MDL) self.lag = 0.0 self.track_all_points = track_all_points # for offline analysis, without max lateral accel or max steer torque filters if decimated: @@ -122,13 +111,19 @@ class TorqueEstimator(ParameterEstimator): self.offline_friction = 0.0 self.offline_latAccelFactor = 0.0 + self.offline_sigmoidSharpness = 0.0 + self.offline_sigmoidTorqueGain = 0.0 + self.resets = 0.0 - self.use_params = CP.brand in ALLOWED_BRANDS and CP.lateralTuning.which() == 'torque' - self.use_params |= CP.carFingerprint in ALLOWED_CARS + self.use_params = CP.brand in ALLOWED_CARS and CP.lateralTuning.which() == 'torque' if CP.lateralTuning.which() == 'torque': self.offline_friction = CP.lateralTuning.torque.friction self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor + self.offline_sigmoidSharpness = CP.lateralTuning.torque.sigmoidSharpness + self.offline_sigmoidTorqueGain = CP.lateralTuning.torque.sigmoidTorqueGain + + print(f"offline params: {self.offline_sigmoidSharpness=}, {self.offline_sigmoidTorqueGain=}, {self.offline_latAccelFactor=}, {self.offline_friction=}") self.calibrator = PoseCalibrator() @@ -138,11 +133,17 @@ class TorqueEstimator(ParameterEstimator): 'latAccelFactor': self.offline_latAccelFactor, 'latAccelOffset': 0.0, 'frictionCoefficient': self.offline_friction, + 'sigmoidSharpness': self.offline_sigmoidSharpness, + 'sigmoidTorqueGain': self.offline_sigmoidTorqueGain, 'points': [] } self.decay = MIN_FILTER_DECAY self.min_lataccel_factor = (1.0 - self.factor_sanity) * self.offline_latAccelFactor self.max_lataccel_factor = (1.0 + self.factor_sanity) * self.offline_latAccelFactor + self.min_sigmoid_sharpness = (1.0 - self.factor_sanity) * self.offline_sigmoidSharpness + self.max_sigmoid_sharpness = (1.0 + self.factor_sanity) * self.offline_sigmoidSharpness + self.min_sigmoid_torque_gain = (1.0 - self.factor_sanity) * self.offline_sigmoidTorqueGain + self.max_sigmoid_torque_gain = (1.0 + self.factor_sanity) * self.offline_sigmoidTorqueGain self.min_friction = (1.0 - self.friction_sanity) * self.offline_friction self.max_friction = (1.0 + self.friction_sanity) * self.offline_friction @@ -161,7 +162,9 @@ class TorqueEstimator(ParameterEstimator): initial_params = { 'latAccelFactor': cache_ltp.latAccelFactorFiltered, 'latAccelOffset': cache_ltp.latAccelOffsetFiltered, - 'frictionCoefficient': cache_ltp.frictionCoefficientFiltered + 'frictionCoefficient': cache_ltp.frictionCoefficientFiltered, + 'sigmoidSharpness': cache_ltp.sigmoidSharpnessFiltered, + 'sigmoidTorqueGain': cache_ltp.sigmoidTorqueGainFiltered, } initial_params['points'] = cache_ltp.points self.decay = cache_ltp.decay @@ -170,18 +173,27 @@ class TorqueEstimator(ParameterEstimator): except Exception: cloudlog.exception("failed to restore cached torque params") params.remove("LiveTorqueParameters") + self.pre_load_points(initial_params) + self.estimate_params() + + else: + self.pre_load_points(initial_params) + self.estimate_params() self.filtered_params = {} for param in initial_params: - self.filtered_params[param] = FirstOrderFilter(initial_params[param], self.decay, DT_CTRL) + self.filtered_params[param] = FirstOrderFilter(initial_params[param], self.decay, DT_MDL) @staticmethod def get_restore_key(CP, version): - a, b = None, None + a, b , c, d = None, None , None, None if CP.lateralTuning.which() == 'torque': - a = CP.lateralTuning.torque.friction - b = CP.lateralTuning.torque.latAccelFactor - return (CP.carFingerprint, CP.lateralTuning.which(), a, b, version) + a = CP.lateralTuning.torque.sigmoidSharpness + b = CP.lateralTuning.torque.sigmoidTorqueGain + c = CP.lateralTuning.torque.friction + d = CP.lateralTuning.torque.latAccelFactor + + return (CP.carFingerprint, CP.lateralTuning.which(), a, b, c, d, version) def reset(self): self.resets += 1.0 @@ -194,34 +206,19 @@ class TorqueEstimator(ParameterEstimator): rowsize=3) self.all_torque_points = [] - def estimate_params_linear(self): - points = self.filtered_points.get_points(self.fit_points) - # total least square solution as both x and y are noisy observations - # this is empirically the slope of the hysteresis parallelogram as opposed to the line through the diagonals - try: - _, _, v = np.linalg.svd(points, full_matrices=False) - slope, offset = -v.T[0:2, 2] / v.T[2, 2] - _, spread = np.matmul(points[:, [0, 2]], slope2rot(slope)).T - friction_coeff = np.std(spread) * FRICTION_FACTOR - except np.linalg.LinAlgError as e: - cloudlog.exception(f"Error computing live torque params: {e}") - slope = offset = friction_coeff = np.nan - return slope, offset, friction_coeff - - - def estimate_params(self): + def estimate_params(self) -> tuple: """ Fit the 4-parameter steering-torque curve and extract a single static-friction amplitude (sigma_f). - Returns (a, b, c, d, sigma_f) or (None, …) on failure. + Returns (a, b, c, d, sigma_f) or (None, …) or (.., np.nan) on failure. """ # ── 1. gather data ────────────────────────────────────────── pts = self.filtered_points.get_points(self.fit_points) if pts.size == 0: - cloudlog.info("No points to fit.") - return (np.nan,)*5 - + cloudlog.info("No points to fit.") + return (np.nan,)*5 + # ── 2 linear fit for friction estimate ─────────────────── try: _, _, v = np.linalg.svd(pts, full_matrices=False) slope, _ = -v.T[0:2, 2] / v.T[2, 2] @@ -234,48 +231,42 @@ class TorqueEstimator(ParameterEstimator): x = pts[:, 2].astype(float) # lateral acceleration y = pts[:, 0].astype(float) # steering torque - # ── 2. Gauss-Newton / LM fit for (a,b,c,d) ───────────────── - # paste this into Desmos to visualize the fit: - """ - S_{sigmoid}\left(x\right)=\frac{1}{\left(1+e^{-x}\right)}-.5 - f\left(x\right)\ =\ S_{sigmoid}\left(x\cdot a_{sigmoidSharpness}\right)\cdot b_{sigmoidTorqueGain}+\left(x\cdot c_{latAccelFactor}\right) - a_{sigmoidSharpness}=3 - b_{sigmoidTorqueGain}=1 - c_{latAccelFactor}=.1 - """ - - b0 = np.clip(np.ptp(y), 0.1, 2.0) - params = np.array([3.0, b0, 0.0, 0.0]) # [a,b,c,d] - lam, tol, it_max = 1e-3, 1e-5, 50 + # ── 3. Gauss-Newton / LM fit for (a,b,c,d) ───────────────── + b0 = np.clip(np.ptp(y), 0.1, 2.0) + params = np.array([3.0, b0, 0.0, 0.0]) # [a,b,c,d] + lam, tol, it_max = 1e-3, 1e-5, 50 # λ lambda, tolerance, max iters for it in range(it_max): - a, b, c, d = params - r = model(x, a, b, c, d) - y - J = jacobian(x, a, b, c, d) - H = J.T @ J - g = J.T @ r + a, b, c, d = params + r = model(x, a, b, c, d) - y + J = jacobian(x, a, b, c, d) + H = J.T @ J + g = J.T @ r + try: delta = np.linalg.solve(H + lam*np.eye(4), -g) + except np.linalg.LinAlgError: + cloudlog.warning("GN fit failed to solve for delta") + return (None,)*5 + if not np.all(np.isfinite(delta)): + cloudlog.warning("Non-finite GN step – aborting") + return (None,)*5 - if not np.all(np.isfinite(delta)): - cloudlog.warning("Non-finite GN step – aborting") - return (None,)*5 + params_new = params + delta + # bounds + # params_new[0] = np.clip(params_new[0], 0.0, 10.0) # a: sigmoid sharpness + # params_new[1] = np.clip(params_new[1], 0.0, 2.0) # b: sigmoid torque gain + # params_new[2] = np.clip(params_new[2], 0.0, 5.0) # c: lat accel factor + # params_new[3] = np.clip(params_new[3], -.3, 0.3) # d: lat accel offset - params_new = params + delta - # bounds - params_new[0] = np.clip(params_new[0], 0.0, 10.0) # a - params_new[1] = np.clip(params_new[1], 0.0, 2.0) # b - params_new[2] = np.clip(params_new[2], 0.0, 5.0) # c - params_new[3] = np.clip(params_new[3], -.3, 0.3) # d - - if np.max(np.abs(delta)) < tol: - params = params_new - break + if np.max(np.abs(delta)) < tol: params = params_new + break + params = params_new a, b, c, d = params if not np.all(np.isfinite(params)): - cloudlog.warning("Invalid parameters after GN fit") - return (None,)*5 + cloudlog.warning("Invalid parameters after GN fit") + return (None,)*5 # ── 3. friction estimate from residual envelope ─────────── # resid = y - model(x, a, b, c, d) @@ -296,17 +287,18 @@ class TorqueEstimator(ParameterEstimator): # sigma_f = max(peak_sigma - tail_sigma, 0.0) * FRICTION_FACTOR cloudlog.info( - f"GN fit {it+1:02d} iters: " - f"a={a:.4f} b={b:.4f} c={c:.4f} d={d:.4f} σ_f={friction_coeff:.4f}" + f"GN fit {it+1:02d} iters: " + f"a={a:.4f} b={b:.4f} c={c:.4f} d={d:.4f} σ_f={friction_coeff:.4f}" ) - print(f"GN fit {it+1:02d} iters: a={a:.4f} b={b:.4f} c={c:.4f} d={d:.4f} σ_f={friction_coeff:.4f}") + + #print(f"GN fit {it+1:02d} iters: a={a:.4f} b={b:.4f} c={c:.4f} d={d:.4f} σ_f={friction_coeff:.4f}") self.nonlinear_params = np.array([a, b, c, d]) self.friction_coeff = friction_coeff return a, b, c, d, friction_coeff def update_params(self, params): - self.decay = min(self.decay + DT_CTRL, MAX_FILTER_DECAY) + self.decay = min(self.decay + DT_MDL, MAX_FILTER_DECAY) for param, value in params.items(): self.filtered_params[param].update(value) self.filtered_params[param].update_alpha(self.decay) @@ -335,39 +327,26 @@ class TorqueEstimator(ParameterEstimator): calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) angular_velocity_calibrated = calibrated_pose.angular_velocity - yaw_rate = angular_velocity_calibrated.yaw roll = device_pose.orientation.roll # check lat active up to now (without lag compensation) - lat_active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_CTRL), + lat_active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_MDL), self.raw_points['carControl_t'], self.raw_points['lat_active']).astype(bool) - steer_override = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_CTRL), + steer_override = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_MDL), self.raw_points['carState_t'], self.raw_points['steer_override']).astype(bool) vego = np.interp(t, self.raw_points['carState_t'], self.raw_points['vego']) steer = np.interp(t, self.raw_points['carOutput_t'], self.raw_points['steer_torque']).item() lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY).item() - # average the steering torque over the last .25 seconds and add it to the filtered points instead of the raw point. the buffer has 500 samples at 100hz - LOOKBACK = 0.5 - lookback_t = np.arange(t - LOOKBACK, t + self.lag, DT_CTRL) - - # avg_steer = np.mean(np.interp(lookback_t, - # self.raw_points['carOutput_t'], self.raw_points['steer_torque'])) - steering_angle_std = np.std(np.interp(lookback_t, - self.raw_points['carState_t'], self.raw_points['steer_angle'])) - # steering_angle = np.interp(t, self.raw_points['carState_t'], self.raw_points['steer_angle']).item() - - - #print(avg_steer, steer) - #print(self.raw_points['carControl_t']) - if all(lat_active) and not any(steer_override) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD) and steering_angle_std < 1.0: + steering_angle_std = np.std(np.interp(np.arange(t - LOOKBACK, t + self.lag, DT_MDL), + self.raw_points['carState_t'], self.raw_points['steer_angle'])) + if all(lat_active) and not any(steer_override) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD) and (steering_angle_std < 1.0): if abs(lateral_acc) <= LAT_ACC_THRESHOLD: - #print(f"t: {t:.4f}, lateral_acc: {lateral_acc:.4f}, steer: {avg_steer:.4f}, steer_angle: {steering_angle:.4f}, steer_angle_std: {steering_angle_std:.4f}") - #self.filtered_points.add_point(avg_steer, lateral_acc) self.filtered_points.add_point(steer, lateral_acc) if self.track_all_points: self.all_torque_points.append([steer, lateral_acc]) + def get_msg(self, valid=True, with_points=False): msg = messaging.new_message('liveTorqueParameters') msg.valid = valid @@ -377,13 +356,15 @@ class TorqueEstimator(ParameterEstimator): # Calculate raw estimates when possible, only update filters when enough points are gathered if self.filtered_points.is_calculable(): - latAccelFactor, latAccelOffset, frictionCoeff = self.estimate_params() + sigmoidSharpness, sigmoidTorqueGain, latAccelFactor, latAccelOffset, frictionCoeff = self.estimate_params() liveTorqueParameters.latAccelFactorRaw = float(latAccelFactor) liveTorqueParameters.latAccelOffsetRaw = float(latAccelOffset) liveTorqueParameters.frictionCoefficientRaw = float(frictionCoeff) + liveTorqueParameters.sigmoidSharpnessRaw = float(sigmoidSharpness) + liveTorqueParameters.sigmoidTorqueGainRaw = float(sigmoidTorqueGain) if self.filtered_points.is_valid(): - if any(val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff]): + if any(val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff, sigmoidSharpness, sigmoidTorqueGain]): cloudlog.exception("Live torque parameters are invalid.") liveTorqueParameters.liveValid = False self.reset() @@ -391,7 +372,12 @@ class TorqueEstimator(ParameterEstimator): liveTorqueParameters.liveValid = True latAccelFactor = np.clip(latAccelFactor, self.min_lataccel_factor, self.max_lataccel_factor) frictionCoeff = np.clip(frictionCoeff, self.min_friction, self.max_friction) - self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': frictionCoeff}) + self.update_params({'latAccelFactor': latAccelFactor, + 'latAccelOffset': latAccelOffset, + 'frictionCoefficient': frictionCoeff, + 'sigmoidSharpness': sigmoidSharpness, + 'sigmoidTorqueGain': sigmoidTorqueGain, + }) if with_points: liveTorqueParameters.points = self.filtered_points.get_points()[:, [0, 2]].tolist() @@ -399,62 +385,49 @@ class TorqueEstimator(ParameterEstimator): liveTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x) liveTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x) liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x) + liveTorqueParameters.sigmoidSharpnessFiltered = float(self.filtered_params['sigmoidSharpness'].x) + liveTorqueParameters.sigmoidTorqueGainFiltered = float(self.filtered_params['sigmoidTorqueGain'].x) liveTorqueParameters.totalBucketPoints = len(self.filtered_points) liveTorqueParameters.decay = self.decay liveTorqueParameters.maxResets = self.resets return msg - def pre_load_points(self, initial_tune) -> None: + def pre_load_points(self, initial_params) -> None: """ Seed the buckets with synthetic points built from the initial tune. Parameters ---------- - initial_tune : tuple[float, float, float, float] - (a, b, c, d) sigmoid parameters, - a: 'steepness' of the curve\n - b: 'max' torque\n - c: linear slope offset\n - d: horizonital offset in the torque curve\n + initial_params : tuple + (latAccelFactor, latAccelOffset, frictionCoefficient, sigmoidSharpness, sigmoidTorqueGain) + (c,d,f,a,b) """ + print("Pre-loading points for synthetic data") - a, b, c, d, friction = initial_tune - d = 0.0 # d is the offset in the torque curve - def sig_centered(z): - """ σ(z) − 0.5 in a numerically stable form. """ - pos = 1.0 / (1.0 + np.exp(-z)) - 0.5 - neg = np.exp(z) / (1.0 + np.exp(z)) - 0.5 - return np.where(z >= 0.0, pos, neg) - - def torque(lat_acc): - xs = lat_acc - d - return sig_centered(a * xs) * b + c * xs # note: NO “-0.5·b” - - # ------------------------------------------------------------- + a = initial_params['sigmoidSharpness'] = 3.8818 + b = initial_params['sigmoidTorqueGain'] = 0.6873 + c = initial_params['latAccelFactor'] = 0.0999 + d = initial_params['latAccelOffset'] = 0.0 + friction = initial_params['frictionCoefficient'] + assert d == 0.0, "latAccelOffset must be 0.0 for synthetic data" rng = np.random.default_rng(42) - x_sample = rng.uniform(-4, 4, 20_000) - - # Sensor noise (horizontal jitter) - lat_accel_jitter = x_sample + rng.normal(0, 0.10, size=x_sample.shape) - - # Torque noise: baseline + friction-shaped bump at centre + x_sample = rng.uniform(-4, 4, 40_000) sigma_base = 0.10 - sigma_friction = friction + lat_accel_jitter = x_sample + rng.normal(0, sigma_base, size=x_sample.shape) envelope = np.exp(-(lat_accel_jitter / 1.0) ** 2) steer_jitter = ( - torque(lat_accel_jitter) + model(lat_accel_jitter, a, b, c, d) + rng.normal(0, sigma_base, size=x_sample.shape) - + rng.normal(0, sigma_friction * envelope, size=x_sample.shape) + + rng.normal(0, friction * envelope, size=x_sample.shape) ) - # Add synthetic points to the buckets for τ, a_lat in zip(steer_jitter, lat_accel_jitter): self.filtered_points.add_point(τ, a_lat) - def save_filtered_points(self, base_filename="bucket_plot", file_ext=".png"): + import matplotlib.pyplot as plt all_points = [] # Collect all bucket points for the combined plot # Iterate over each bucket in the filtered_points object @@ -485,23 +458,22 @@ class TorqueEstimator(ParameterEstimator): # ── figure ─────────────────────────────────────────────── plt.figure(figsize=(10, 6)) - plt.scatter(lateral_all, steer_all, s=8, alpha=0.4, label="Filtered samples") + # fitted curve + friction band a, b, c, d = self.nonlinear_params # 4-tuple sigma_f = getattr(self, "friction_coeff", 0.0) - sigma_f_op = op_friction(self.filtered_points.get_points(self.fit_points)) - print(f"OP friction: {sigma_f_op:.4f}") x_line = np.linspace(-4, 4, 400) y_fit = model(x_line, a, b, c, d) plt.plot(x_line, y_fit, color="red", lw=2, label="Fitted curve") if sigma_f > 0: - # plt.plot(x_line, y_fit + sigma_f, color="red", ls="--", lw=1.5, label="+σ_f") - # plt.plot(x_line, y_fit - sigma_f, color="red", ls="--", lw=1.5, label="−σ_f") - plt.plot(x_line, y_fit + sigma_f_op, color="blue", ls="--", lw=1.5, label="+OP_σ_f") - plt.plot(x_line, y_fit - sigma_f_op, color="blue", ls="--", lw=1.5, label="−OP_σ_f") + plt.plot(x_line, y_fit + sigma_f, color="blue", ls="--", lw=1.5, label="friction band") + plt.plot(x_line, y_fit - sigma_f, color="blue", ls="--", lw=1.5, label="") + # fill in the area between the two curves + plt.fill_between(x_line, y_fit - sigma_f, y_fit + sigma_f, color="grey", alpha=0.3) + plt.scatter(lateral_all, steer_all, s=8, alpha=0.4, label="Filtered samples") # ── cosmetics ──────────────────────────────────────────── plt.xlim(-4, 4) @@ -509,6 +481,16 @@ class TorqueEstimator(ParameterEstimator): plt.xlabel("Lateral acceleration (m/s²)") plt.ylabel("Steering torque (Nm equiv)") plt.title("Torque vs lateral acceleration (all buckets)") + # print the current parameters + plt.text(0.05, 0.9, f"Friction: {self.friction_coeff:.3f}", transform=plt.gca().transAxes) + plt.text(0.05, 0.85, f"LatAccelFactor: {self.filtered_params['latAccelFactor'].x:.3f}", transform=plt.gca().transAxes) + plt.text(0.05, 0.8, f"SigmoidSharpness: {self.filtered_params['sigmoidSharpness'].x:.3f}", transform=plt.gca().transAxes) + plt.text(0.05, 0.75, f"SigmoidTorqueGain: {self.filtered_params['sigmoidTorqueGain'].x:.3f}", transform=plt.gca().transAxes) + plt.text(0.05, 0.7, f"LatAccelOffset: {self.filtered_params['latAccelOffset'].x:.3f}", transform=plt.gca().transAxes) + plt.text(0.05, 0.65, f"Decay: {self.decay:.3f}", transform=plt.gca().transAxes) + plt.text(0.05, 0.6, f"Valid: {self.filtered_points.is_valid()}", transform=plt.gca().transAxes) + + plt.grid(True) plt.legend() plt.tight_layout() @@ -518,77 +500,17 @@ class TorqueEstimator(ParameterEstimator): plt.close() print(f"Combined plot saved as {filename_all}") - def save_points(self, filename="torque_estimator.pkl"): - # Save both raw_points and filtered_points (buckets) as plain data - data = { - 'raw_points': {k: list(v) for k, v in dict(self.raw_points).items()}, - 'filtered_points': {k: v for k, v in self.filtered_points.buckets.items()} - } - with open(filename, "wb") as f: - pickle.dump(data, f) - print(f"Estimator saved to {filename}") - - def load_points(self, filename="torque_estimator.pkl"): - with open(filename, "rb") as f: - data = pickle.load(f) - # Reconstruct raw_points as a defaultdict with deques - self.raw_points = defaultdict(lambda: deque(maxlen=self.hist_len)) - for key, value in data.get('raw_points', {}).items(): - self.raw_points[key] = deque(value, maxlen=self.hist_len) - # Reconstruct filtered_points buckets directly - if 'filtered_points' in data: - self.filtered_points.buckets = data['filtered_points'] - print(f"Points loaded from {filename}") - - def main(demo=False): + config_realtime_process([0, 1, 2, 3], 5) pm = messaging.PubMaster(['liveTorqueParameters']) sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose', 'liveDelay'], poll='livePose') params = Params() + estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)) - if demo: - import time - # benchmark - start_time = time.time() - estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParamsPrevRoute", block=True), car.CarParams)) - print("Time taken to create TorqueEstimator:", time.time() - start_time) - step_time = time.time() - estimator.pre_load_points(NON_LINEAR_TORQUE_PARAMS['MAZDA_3_2019']) - print("Time taken to pre-load points:", time.time() - step_time) - step_time = time.time() - estimator.estimate_params() - print("Time taken to estimate params:", time.time() - step_time) - exit(0) - - msg_filter = ['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose', 'liveDelay', 'carParams'] - - #estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)) - from openpilot.tools.lib.logreader import LogReader - lr = LogReader("3b58edf884ab4eaf|00000012--a994169ecb/0:", sort_by_time=True) - estimator = None - for msg in lr: - t = msg.logMonoTime * 1e-9 - which = msg.which() - if which not in msg_filter: - continue - if estimator is None and msg.which() == "carParams": - estimator = TorqueEstimator(msg.carParams) - estimator.pre_load_points(NON_LINEAR_TORQUE_PARAMS['MAZDA_3_2019']) - estimator.estimate_params() - estimator.save_filtered_points() - - if estimator: - estimator.handle_log(t, which, msg._get(which)) - - estimator.estimate_params() - estimator.save_filtered_points() - #estimator.save_points() - - exit(0) while True: sm.update() if sm.all_checks(): @@ -600,6 +522,8 @@ def main(demo=False): # 4Hz driven by livePose if sm.frame % 5 == 0: pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks())) + if sm.frame % 500 == 0: + estimator.save_filtered_points() # Cache points every 60 seconds while onroad if sm.frame % 240 == 0: @@ -609,8 +533,7 @@ def main(demo=False): if __name__ == "__main__": import argparse - parser = argparse.ArgumentParser(description='Process the --demo argument.') parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.') args = parser.parse_args() - main(demo=args.demo) + main(demo=args.demo) \ No newline at end of file