Online lateral lag learning (#34531)

* Add struct and services

* Proof of concept

* Use vego

* Process replay support

* Fix issues

* Max not min

* Remove print

* Make lag positive

* Points and updates

* Sliding cross corr

* Stuff

* Remove redundant param

* Move to lagd

* Update alert

* Something that works

* Report isEstimated

* Corr field

* Use skimage masked corr

* Fixes

* Move to lat_accel_loc

* Use the delay it in the model

* Mark as executable

* Add 0.2 to initial

* add install line

* Back to 5m window

* Move the import up

* corr window 120

* Show is estimated

* Sort messages

* New impl

* Fix

* Params

* Set initial_lag elsewhere

* Add param

* Rename Param

* Fix type isues

* More blocks

* Masked fft NCC implementation

* Remove package installation

* Use 20hz data and interp the correlation peak

* Move ncc code to separate function

* steering_saturated support

* Constants

* Add recovery buffer

* Fix static

* min_valid_block_count nad liveDelay

* lateralDelayEstimate

* Fix comm issues. Do estimations at 4hz

* Fix

* Display the estimate

* Increase the block size

* Cache every minute

* DEBUG flag

* Add progress to the alert

* Fix crash

* points only in debug

* Fix BlockAverage restoration

* Comment

* Move estimators into estimator directory

* Remove lagd

* Feed only relevant messages

* Rewrite as a set literal

* np.diag with P_initial

* Fix static

* Struct renames

* Update ref commit

* Make it nicer

* Some renames

cereal/log.capnp

@@ -2278,6 +2278,21 @@ struct LiveTorqueParametersData {
   useParams @12 :Bool;
 }
 
+struct LiveDelayData {
+  lateralDelay @0 :Float32;
+  validBlocks @1 :Int32;
+  status @2 :Status;
+
+  lateralDelayEstimate @3 :Float32;
+  points @4 :List(Float32);
+
+  enum Status {
+    unestimated @0;
+    estimated @1;
+    invalid @2;
+  }
+}
+
 struct LiveMapDataDEPRECATED {
   speedLimitValid @0 :Bool;
   speedLimit @1 :Float32;
@@ -2508,6 +2523,7 @@ struct Event {
     gnssMeasurements @91 :GnssMeasurements;
     liveParameters @61 :LiveParametersData;
     liveTorqueParameters @94 :LiveTorqueParametersData;
+    liveDelay @146 : LiveDelayData;
     cameraOdometry @63 :CameraOdometry;
     thumbnail @66: Thumbnail;
     onroadEvents @134: List(OnroadEvent);

cereal/services.py

@@ -36,6 +36,7 @@ _services: dict[str, tuple] = {
   "errorLogMessage": (True, 0., 1),
   "liveCalibration": (True, 4., 4),
   "liveTorqueParameters": (True, 4., 1),
+  "liveDelay": (True, 4., 1),
   "androidLog": (True, 0.),
   "carState": (True, 100., 10),
   "carControl": (True, 100., 10),

common/params_keys.h

@@ -71,6 +71,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"LastPowerDropDetected", CLEAR_ON_MANAGER_START},
     {"LastUpdateException", CLEAR_ON_MANAGER_START},
     {"LastUpdateTime", PERSISTENT},
+    {"LiveLag", PERSISTENT},
     {"LiveParameters", PERSISTENT},
     {"LiveTorqueParameters", PERSISTENT | DONT_LOG},
     {"LocationFilterInitialState", PERSISTENT},

selfdrive/locationd/estimators/lateral_lag.py

@@ -0,0 +1,273 @@
+import numpy as np
+from collections import deque
+from functools import partial
+
+import cereal.messaging as messaging
+from cereal import log
+from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
+
+BLOCK_SIZE = 100
+BLOCK_NUM = 50
+BLOCK_NUM_NEEDED = 5
+MOVING_WINDOW_SEC = 300.0
+MIN_OKAY_WINDOW_SEC = 30.0
+MIN_RECOVERY_BUFFER_SEC = 2.0
+MIN_VEGO = 15.0
+MIN_ABS_YAW_RATE = np.radians(1.0)
+MIN_NCC = 0.95
+
+
+def parabolic_peak_interp(R, max_index):
+  if max_index == 0 or max_index == len(R) - 1:
+    return max_index
+
+  y_m1, y_0, y_p1 = R[max_index - 1], R[max_index], R[max_index + 1]
+  offset = 0.5 * (y_p1 - y_m1) / (2 * y_0 - y_p1 - y_m1)
+
+  return max_index + offset
+
+
+def masked_normalized_cross_correlation(expected_sig, actual_sig, mask):
+  """
+  References:
+    D. Padfield. "Masked FFT registration". In Proc. Computer Vision and
+    Pattern Recognition, pp. 2918-2925 (2010).
+    :DOI:`10.1109/CVPR.2010.5540032`
+  """
+
+  eps = np.finfo(np.float64).eps
+  expected_sig = np.asarray(expected_sig, dtype=np.float64)
+  actual_sig = np.asarray(actual_sig, dtype=np.float64)
+
+  expected_sig[~mask] = 0.0
+  actual_sig[~mask] = 0.0
+
+  rotated_expected_sig = expected_sig[::-1]
+  rotated_mask = mask[::-1]
+
+  n = len(expected_sig) + len(actual_sig) - 1
+  fft = partial(np.fft.fft, n=n)
+
+  actual_sig_fft = fft(actual_sig)
+  rotated_expected_sig_fft = fft(rotated_expected_sig)
+  actual_mask_fft = fft(mask.astype(np.float64))
+  rotated_mask_fft = fft(rotated_mask.astype(np.float64))
+
+  number_overlap_masked_samples = np.fft.ifft(rotated_mask_fft * actual_mask_fft).real
+  number_overlap_masked_samples[:] = np.round(number_overlap_masked_samples)
+  number_overlap_masked_samples[:] = np.fmax(number_overlap_masked_samples, eps)
+  masked_correlated_actual_fft = np.fft.ifft(rotated_mask_fft * actual_sig_fft).real
+  masked_correlated_expected_fft = np.fft.ifft(actual_mask_fft * rotated_expected_sig_fft).real
+
+  numerator = np.fft.ifft(rotated_expected_sig_fft * actual_sig_fft).real
+  numerator -= masked_correlated_actual_fft * masked_correlated_expected_fft / number_overlap_masked_samples
+
+  actual_squared_fft = fft(actual_sig ** 2)
+  actual_sig_denom = np.fft.ifft(rotated_mask_fft * actual_squared_fft).real
+  actual_sig_denom -= masked_correlated_actual_fft ** 2 / number_overlap_masked_samples
+  actual_sig_denom[:] = np.fmax(actual_sig_denom, 0.0)
+
+  rotated_expected_squared_fft = fft(rotated_expected_sig ** 2)
+  expected_sig_denom = np.fft.ifft(actual_mask_fft * rotated_expected_squared_fft).real
+  expected_sig_denom -= masked_correlated_expected_fft ** 2 / number_overlap_masked_samples
+  expected_sig_denom[:] = np.fmax(expected_sig_denom, 0.0)
+
+  denom = np.sqrt(actual_sig_denom * expected_sig_denom)
+
+  # zero-out samples with very small denominators
+  tol = 1e3 * eps * np.max(np.abs(denom), keepdims=True)
+  nonzero_indices = denom > tol
+
+  ncc = np.zeros_like(denom, dtype=np.float64)
+  ncc[nonzero_indices] = numerator[nonzero_indices] / denom[nonzero_indices]
+  np.clip(ncc, -1, 1, out=ncc)
+
+  return ncc
+
+
+class Points:
+  def __init__(self, num_points):
+    self.times = deque(maxlen=num_points)
+    self.okay = deque(maxlen=num_points)
+    self.desired = deque(maxlen=num_points)
+    self.actual = deque(maxlen=num_points)
+
+  @property
+  def num_points(self):
+    return len(self.desired)
+
+  @property
+  def num_okay(self):
+    return np.count_nonzero(self.okay)
+
+  def update(self, t, desired, actual, okay):
+    self.times.append(t)
+    self.okay.append(okay)
+    self.desired.append(desired)
+    self.actual.append(actual)
+
+  def get(self):
+    return np.array(self.times), np.array(self.desired), np.array(self.actual), np.array(self.okay)
+
+
+class BlockAverage:
+  def __init__(self, num_blocks, block_size, valid_blocks, initial_value):
+    self.num_blocks = num_blocks
+    self.block_size = block_size
+    self.block_idx = valid_blocks % block_size
+    self.idx = 0
+
+    self.values = np.tile(initial_value, (num_blocks, 1))
+    self.valid_blocks = valid_blocks
+
+  def update(self, value):
+    self.values[self.block_idx] = (self.idx * self.values[self.block_idx] + (self.block_size - self.idx) * value) / self.block_size
+    self.idx = (self.idx + 1) % self.block_size
+    if self.idx == 0:
+      self.block_idx = (self.block_idx + 1) % self.num_blocks
+      self.valid_blocks = min(self.valid_blocks + 1, self.num_blocks)
+
+  def get(self):
+    valid_block_idx = [i for i in range(self.valid_blocks) if i != self.block_idx]
+    if not valid_block_idx:
+      return None
+    return float(np.mean(self.values[valid_block_idx], axis=0).item())
+
+
+class LateralLagEstimator:
+  inputs = {"carControl", "carState", "controlsState", "liveCalibration", "livePose"}
+
+  def __init__(self, CP, dt,
+               block_count=BLOCK_NUM, min_valid_block_count=BLOCK_NUM_NEEDED, block_size=BLOCK_SIZE,
+               window_sec=MOVING_WINDOW_SEC, okay_window_sec=MIN_OKAY_WINDOW_SEC, min_recovery_buffer_sec=MIN_RECOVERY_BUFFER_SEC,
+               min_vego=MIN_VEGO, min_yr=MIN_ABS_YAW_RATE, min_ncc=MIN_NCC):
+    self.dt = dt
+    self.window_sec = window_sec
+    self.okay_window_sec = okay_window_sec
+    self.min_recovery_buffer_sec = min_recovery_buffer_sec
+    self.initial_lag = CP.steerActuatorDelay + 0.2
+    self.block_size = block_size
+    self.block_count = block_count
+    self.min_valid_block_count = min_valid_block_count
+    self.min_vego = min_vego
+    self.min_yr = min_yr
+    self.min_ncc = min_ncc
+
+    self.t = 0
+    self.lat_active = False
+    self.steering_pressed = False
+    self.steering_saturated = False
+    self.desired_curvature = 0
+    self.v_ego = 0
+    self.yaw_rate = 0
+
+    self.last_lat_inactive_t = 0
+    self.last_steering_pressed_t = 0
+    self.last_steering_saturated_t = 0
+    self.last_estimate_t = 0
+
+    self.calibrator = PoseCalibrator()
+
+    self.reset(self.initial_lag, 0)
+
+  def reset(self, initial_lag, valid_blocks):
+    window_len = int(self.window_sec / self.dt)
+    self.points = Points(window_len)
+    self.block_avg = BlockAverage(self.block_count, self.block_size, valid_blocks, initial_lag)
+
+  def get_msg(self, valid, debug=False):
+    msg = messaging.new_message('liveDelay')
+
+    msg.valid = valid
+
+    liveDelay = msg.liveDelay
+
+    estimated_lag = self.block_avg.get()
+    liveDelay.lateralDelayEstimate = estimated_lag or self.initial_lag
+    if self.block_avg.valid_blocks >= self.min_valid_block_count and estimated_lag is not None:
+      liveDelay.status = log.LiveDelayData.Status.estimated
+      liveDelay.lateralDelay = estimated_lag
+    else:
+      liveDelay.status = log.LiveDelayData.Status.unestimated
+      liveDelay.lateralDelay = self.initial_lag
+    liveDelay.validBlocks = self.block_avg.valid_blocks
+    if debug:
+      liveDelay.points = self.block_avg.values.flatten().tolist()
+
+    return msg
+
+  def handle_log(self, t, which, msg):
+    if which == "carControl":
+      self.lat_active = msg.latActive
+    elif which == "carState":
+      self.steering_pressed = msg.steeringPressed
+      self.v_ego = msg.vEgo
+    elif which == "controlsState":
+      self.steering_saturated = getattr(msg.lateralControlState, msg.lateralControlState.which()).saturated
+      self.desired_curvature = msg.desiredCurvature
+    elif which == "liveCalibration":
+      self.calibrator.feed_live_calib(msg)
+    elif which == "livePose":
+      device_pose = Pose.from_live_pose(msg)
+      calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
+      self.yaw_rate = calibrated_pose.angular_velocity.z
+    self.t = t
+
+  def points_valid(self):
+    return self.points.num_okay >= int(self.okay_window_sec / self.dt)
+
+  def update_points(self):
+    if not self.lat_active:
+      self.last_lat_inactive_t = self.t
+    if self.steering_pressed:
+      self.last_steering_pressed_t = self.t
+    if self.steering_saturated:
+      self.last_steering_saturated_t = self.t
+
+    la_desired = self.desired_curvature * self.v_ego * self.v_ego
+    la_actual_pose = self.yaw_rate * self.v_ego
+
+    fast = self.v_ego > self.min_vego
+    turning = np.abs(self.yaw_rate) >= self.min_yr
+    has_recovered = all( # wait for recovery after !lat_active, steering_pressed, steering_saturated
+      self.t - last_t >= self.min_recovery_buffer_sec
+      for last_t in [self.last_lat_inactive_t, self.last_steering_pressed_t, self.last_steering_saturated_t]
+    )
+    okay = self.lat_active and not self.steering_pressed and not self.steering_saturated and fast and turning and has_recovered
+
+    self.points.update(self.t, la_desired, la_actual_pose, okay)
+
+  def update_estimate(self):
+    # check if the points are valid overall
+    if not self.points_valid():
+      return
+
+    times, desired, actual, okay = self.points.get()
+    # check if there are any new valid data points since the last update
+    if self.last_estimate_t != 0 and times[0] <= self.last_estimate_t:
+      new_values_start_idx = next(-i for i, t in enumerate(reversed(times)) if t <= self.last_estimate_t)
+      if (new_values_start_idx == 0 or not np.any(okay[new_values_start_idx:])):
+        return
+
+    delay, corr = self.actuator_delay(desired, actual, okay, self.dt)
+    if corr < self.min_ncc:
+      return
+
+    self.block_avg.update(delay)
+    self.last_estimate_t = self.t
+
+  def correlation_lags(self, sig_len, dt):
+    return np.arange(0, sig_len) * dt
+
+  def actuator_delay(self, expected_sig, actual_sig, mask, dt, max_lag=1.):
+    ncc = masked_normalized_cross_correlation(expected_sig, actual_sig, mask)
+
+    # only consider lags from 0 to max_lag
+    max_lag_samples = int(max_lag / dt)
+    roi_ncc = ncc[len(expected_sig) - 1: len(expected_sig) - 1 + max_lag_samples]
+
+    max_corr_index = np.argmax(roi_ncc)
+    corr = roi_ncc[max_corr_index]
+    lag = parabolic_peak_interp(roi_ncc, max_corr_index) * dt
+
+    return lag, corr

selfdrive/locationd/estimators/vehicle_params.py

@@ -0,0 +1,197 @@
+import numpy as np
+import capnp
+
+import cereal.messaging as messaging
+from cereal import car, log
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.swaglog import cloudlog
+from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
+from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
+from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
+
+MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL  # Max 20 deg/s
+ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL  # 20deg in 1 second is well within curvature limits
+ROLL_MIN, ROLL_MAX = np.radians(-10), np.radians(10)
+ROLL_LOWERED_MAX = np.radians(8)
+ROLL_STD_MAX = np.radians(1.5)
+LATERAL_ACC_SENSOR_THRESHOLD = 4.0
+OFFSET_MAX = 10.0
+OFFSET_LOWERED_MAX = 8.0
+MIN_ACTIVE_SPEED = 1.0
+LOW_ACTIVE_SPEED = 10.0
+
+
+def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: float, lowered_threshold: float):
+  if current_valid:
+    current_valid = abs(val) < threshold
+  else:
+    current_valid = abs(val) < lowered_threshold
+  return current_valid
+
+
+class VehicleParamsLearner:
+  inputs = {'carState', 'liveCalibration', 'livePose'}
+
+  def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None):
+    self.kf = CarKalman(GENERATED_DIR)
+
+    self.x_initial = CarKalman.initial_x.copy()
+    self.x_initial[States.STEER_RATIO] = steer_ratio
+    self.x_initial[States.STIFFNESS] = stiffness_factor
+    self.x_initial[States.ANGLE_OFFSET] = angle_offset
+    self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial
+
+    self.kf.set_globals(
+      mass=CP.mass,
+      rotational_inertia=CP.rotationalInertia,
+      center_to_front=CP.centerToFront,
+      center_to_rear=CP.wheelbase - CP.centerToFront,
+      stiffness_front=CP.tireStiffnessFront,
+      stiffness_rear=CP.tireStiffnessRear
+    )
+
+    self.min_sr, self.max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
+
+    self.calibrator = PoseCalibrator()
+
+    self.observed_speed = 0.0
+    self.observed_yaw_rate = 0.0
+    self.observed_roll = 0.0
+
+    self.avg_offset_valid = True
+    self.total_offset_valid = True
+    self.roll_valid = True
+
+    self.reset(None)
+
+  def reset(self, t: float | None):
+    self.kf.init_state(self.x_initial, covs=self.P_initial, filter_time=t)
+
+    self.angle_offset, self.roll, self.active = np.degrees(self.x_initial[States.ANGLE_OFFSET].item()), 0.0, False
+    self.avg_angle_offset = self.angle_offset
+
+  def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader):
+    if which == 'livePose':
+      device_pose = Pose.from_live_pose(msg)
+      calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
+
+      yaw_rate, yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
+      yaw_rate_valid = msg.angularVelocityDevice.valid
+      yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10  # rad/s
+      yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1  # rad/s
+      if not yaw_rate_valid:
+        # This is done to bound the yaw rate estimate when localizer values are invalid or calibrating
+        yaw_rate, yaw_rate_std = 0.0, np.radians(10.0)
+      self.observed_yaw_rate = yaw_rate
+
+      localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std
+      localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std
+      roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK
+      if roll_valid:
+        roll = localizer_roll
+        # Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar?
+        roll_std = 2 * localizer_roll_std
+      else:
+        # This is done to bound the road roll estimate when localizer values are invalid
+        roll = 0.0
+        roll_std = np.radians(10.0)
+      self.observed_roll = np.clip(roll, self.observed_roll - ROLL_MAX_DELTA, self.observed_roll + ROLL_MAX_DELTA)
+
+      if self.active:
+        if msg.posenetOK:
+          self.kf.predict_and_observe(t,
+                                      ObservationKind.ROAD_FRAME_YAW_RATE,
+                                      np.array([[-self.observed_yaw_rate]]),
+                                      np.array([np.atleast_2d(yaw_rate_std**2)]))
+
+          self.kf.predict_and_observe(t,
+                                      ObservationKind.ROAD_ROLL,
+                                      np.array([[self.observed_roll]]),
+                                      np.array([np.atleast_2d(roll_std**2)]))
+        self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))
+
+        # We observe the current stiffness and steer ratio (with a high observation noise) to bound
+        # the respective estimate STD. Otherwise the STDs keep increasing, causing rapid changes in the
+        # states in longer routes (especially straight stretches).
+        stiffness = float(self.kf.x[States.STIFFNESS].item())
+        steer_ratio = float(self.kf.x[States.STEER_RATIO].item())
+        self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]]))
+        self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]]))
+
+    elif which == 'liveCalibration':
+      self.calibrator.feed_live_calib(msg)
+
+    elif which == 'carState':
+      steering_angle = msg.steeringAngleDeg
+
+      in_linear_region = abs(steering_angle) < 45
+      self.observed_speed = msg.vEgo
+      self.active = self.observed_speed > MIN_ACTIVE_SPEED and in_linear_region
+
+      if self.active:
+        self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[np.radians(steering_angle)]]))
+        self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.observed_speed]]))
+
+    if not self.active:
+      # Reset time when stopped so uncertainty doesn't grow
+      self.kf.filter.set_filter_time(t)  # type: ignore
+      self.kf.filter.reset_rewind()      # type: ignore
+
+  def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder:
+    x = self.kf.x
+    P = np.sqrt(self.kf.P.diagonal())
+    if not np.all(np.isfinite(x)):
+      cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
+      self.reset(self.kf.t)
+      x = self.kf.x
+
+    self.avg_angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item()),
+                                self.avg_angle_offset - MAX_ANGLE_OFFSET_DELTA, self.avg_angle_offset + MAX_ANGLE_OFFSET_DELTA)
+    self.angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
+                        self.angle_offset - MAX_ANGLE_OFFSET_DELTA, self.angle_offset + MAX_ANGLE_OFFSET_DELTA)
+    self.roll = np.clip(float(x[States.ROAD_ROLL].item()), self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
+    roll_std = float(P[States.ROAD_ROLL].item())
+    if self.active and self.observed_speed > LOW_ACTIVE_SPEED:
+      # Account for the opposite signs of the yaw rates
+      # At low speeds, bumping into a curb can cause the yaw rate to be very high
+      sensors_valid = bool(abs(self.observed_speed * (x[States.YAW_RATE].item() + self.observed_yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
+    else:
+      sensors_valid = True
+    self.avg_offset_valid = check_valid_with_hysteresis(self.avg_offset_valid, self.avg_angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
+    self.total_offset_valid = check_valid_with_hysteresis(self.total_offset_valid, self.angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
+    self.roll_valid = check_valid_with_hysteresis(self.roll_valid, self.roll, ROLL_MAX, ROLL_LOWERED_MAX)
+
+    msg = messaging.new_message('liveParameters')
+
+    msg.valid = valid
+
+    liveParameters = msg.liveParameters
+    liveParameters.posenetValid = True
+    liveParameters.sensorValid = sensors_valid
+    liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
+    liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
+    liveParameters.roll = float(self.roll)
+    liveParameters.angleOffsetAverageDeg = float(self.avg_angle_offset)
+    liveParameters.angleOffsetDeg = float(self.angle_offset)
+    liveParameters.steerRatioValid = self.min_sr <= liveParameters.steerRatio <= self.max_sr
+    liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0
+    liveParameters.angleOffsetAverageValid = bool(self.avg_offset_valid)
+    liveParameters.angleOffsetValid = bool(self.total_offset_valid)
+    liveParameters.valid = all((
+      liveParameters.angleOffsetAverageValid,
+      liveParameters.angleOffsetValid ,
+      self.roll_valid,
+      roll_std < ROLL_STD_MAX,
+      liveParameters.stiffnessFactorValid,
+      liveParameters.steerRatioValid,
+    ))
+    liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
+    liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
+    liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
+    liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
+    if debug:
+      liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message()
+      liveParameters.debugFilterState.value = x.tolist()
+      liveParameters.debugFilterState.std = P.tolist()
+
+    return msg

selfdrive/locationd/paramsd.py

@@ -2,201 +2,15 @@
 import os
 import json
 import numpy as np
-import capnp
 
 import cereal.messaging as messaging
 from cereal import car, log
+from cereal.services import SERVICE_LIST
 from openpilot.common.params import Params
-from openpilot.common.realtime import config_realtime_process, DT_MDL
+from openpilot.common.realtime import config_realtime_process
-from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
-from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
-from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 from openpilot.common.swaglog import cloudlog
-
+from openpilot.selfdrive.locationd.estimators.vehicle_params import VehicleParamsLearner
-MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL  # Max 20 deg/s
+from openpilot.selfdrive.locationd.estimators.lateral_lag import LateralLagEstimator
-ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL  # 20deg in 1 second is well within curvature limits
-ROLL_MIN, ROLL_MAX = np.radians(-10), np.radians(10)
-ROLL_LOWERED_MAX = np.radians(8)
-ROLL_STD_MAX = np.radians(1.5)
-LATERAL_ACC_SENSOR_THRESHOLD = 4.0
-OFFSET_MAX = 10.0
-OFFSET_LOWERED_MAX = 8.0
-MIN_ACTIVE_SPEED = 1.0
-LOW_ACTIVE_SPEED = 10.0
-
-
-class VehicleParamsLearner:
-  def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None):
-    self.kf = CarKalman(GENERATED_DIR)
-
-    self.x_initial = CarKalman.initial_x.copy()
-    self.x_initial[States.STEER_RATIO] = steer_ratio
-    self.x_initial[States.STIFFNESS] = stiffness_factor
-    self.x_initial[States.ANGLE_OFFSET] = angle_offset
-    self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial
-
-    self.kf.set_globals(
-      mass=CP.mass,
-      rotational_inertia=CP.rotationalInertia,
-      center_to_front=CP.centerToFront,
-      center_to_rear=CP.wheelbase - CP.centerToFront,
-      stiffness_front=CP.tireStiffnessFront,
-      stiffness_rear=CP.tireStiffnessRear
-    )
-
-    self.min_sr, self.max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
-
-    self.calibrator = PoseCalibrator()
-
-    self.observed_speed = 0.0
-    self.observed_yaw_rate = 0.0
-    self.observed_roll = 0.0
-
-    self.avg_offset_valid = True
-    self.total_offset_valid = True
-    self.roll_valid = True
-
-    self.reset(None)
-
-  def reset(self, t: float | None):
-    self.kf.init_state(self.x_initial, covs=self.P_initial, filter_time=t)
-
-    self.angle_offset, self.roll, self.active = np.degrees(self.x_initial[States.ANGLE_OFFSET].item()), 0.0, False
-    self.avg_angle_offset = self.angle_offset
-
-  def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader):
-    if which == 'livePose':
-      device_pose = Pose.from_live_pose(msg)
-      calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
-
-      yaw_rate, yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
-      yaw_rate_valid = msg.angularVelocityDevice.valid
-      yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10  # rad/s
-      yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1  # rad/s
-      if not yaw_rate_valid:
-        # This is done to bound the yaw rate estimate when localizer values are invalid or calibrating
-        yaw_rate, yaw_rate_std = 0.0, np.radians(10.0)
-      self.observed_yaw_rate = yaw_rate
-
-      localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std
-      localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std
-      roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK
-      if roll_valid:
-        roll = localizer_roll
-        # Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar?
-        roll_std = 2 * localizer_roll_std
-      else:
-        # This is done to bound the road roll estimate when localizer values are invalid
-        roll = 0.0
-        roll_std = np.radians(10.0)
-      self.observed_roll = np.clip(roll, self.observed_roll - ROLL_MAX_DELTA, self.observed_roll + ROLL_MAX_DELTA)
-
-      if self.active:
-        if msg.posenetOK:
-          self.kf.predict_and_observe(t,
-                                      ObservationKind.ROAD_FRAME_YAW_RATE,
-                                      np.array([[-self.observed_yaw_rate]]),
-                                      np.array([np.atleast_2d(yaw_rate_std**2)]))
-
-          self.kf.predict_and_observe(t,
-                                      ObservationKind.ROAD_ROLL,
-                                      np.array([[self.observed_roll]]),
-                                      np.array([np.atleast_2d(roll_std**2)]))
-        self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))
-
-        # We observe the current stiffness and steer ratio (with a high observation noise) to bound
-        # the respective estimate STD. Otherwise the STDs keep increasing, causing rapid changes in the
-        # states in longer routes (especially straight stretches).
-        stiffness = float(self.kf.x[States.STIFFNESS].item())
-        steer_ratio = float(self.kf.x[States.STEER_RATIO].item())
-        self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]]))
-        self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]]))
-
-    elif which == 'liveCalibration':
-      self.calibrator.feed_live_calib(msg)
-
-    elif which == 'carState':
-      steering_angle = msg.steeringAngleDeg
-
-      in_linear_region = abs(steering_angle) < 45
-      self.observed_speed = msg.vEgo
-      self.active = self.observed_speed > MIN_ACTIVE_SPEED and in_linear_region
-
-      if self.active:
-        self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[np.radians(steering_angle)]]))
-        self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.observed_speed]]))
-
-    if not self.active:
-      # Reset time when stopped so uncertainty doesn't grow
-      self.kf.filter.set_filter_time(t)  # type: ignore
-      self.kf.filter.reset_rewind()      # type: ignore
-
-  def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder:
-    x = self.kf.x
-    P = np.sqrt(self.kf.P.diagonal())
-    if not np.all(np.isfinite(x)):
-      cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
-      self.reset(self.kf.t)
-      x = self.kf.x
-
-    self.avg_angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item()),
-                                self.avg_angle_offset - MAX_ANGLE_OFFSET_DELTA, self.avg_angle_offset + MAX_ANGLE_OFFSET_DELTA)
-    self.angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
-                        self.angle_offset - MAX_ANGLE_OFFSET_DELTA, self.angle_offset + MAX_ANGLE_OFFSET_DELTA)
-    self.roll = np.clip(float(x[States.ROAD_ROLL].item()), self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
-    roll_std = float(P[States.ROAD_ROLL].item())
-    if self.active and self.observed_speed > LOW_ACTIVE_SPEED:
-      # Account for the opposite signs of the yaw rates
-      # At low speeds, bumping into a curb can cause the yaw rate to be very high
-      sensors_valid = bool(abs(self.observed_speed * (x[States.YAW_RATE].item() + self.observed_yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
-    else:
-      sensors_valid = True
-    self.avg_offset_valid = check_valid_with_hysteresis(self.avg_offset_valid, self.avg_angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
-    self.total_offset_valid = check_valid_with_hysteresis(self.total_offset_valid, self.angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
-    self.roll_valid = check_valid_with_hysteresis(self.roll_valid, self.roll, ROLL_MAX, ROLL_LOWERED_MAX)
-
-    msg = messaging.new_message('liveParameters')
-
-    msg.valid = valid
-
-    liveParameters = msg.liveParameters
-    liveParameters.posenetValid = True
-    liveParameters.sensorValid = sensors_valid
-    liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
-    liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
-    liveParameters.roll = float(self.roll)
-    liveParameters.angleOffsetAverageDeg = float(self.avg_angle_offset)
-    liveParameters.angleOffsetDeg = float(self.angle_offset)
-    liveParameters.steerRatioValid = self.min_sr <= liveParameters.steerRatio <= self.max_sr
-    liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0
-    liveParameters.angleOffsetAverageValid = bool(self.avg_offset_valid)
-    liveParameters.angleOffsetValid = bool(self.total_offset_valid)
-    liveParameters.valid = all((
-      liveParameters.angleOffsetAverageValid,
-      liveParameters.angleOffsetValid ,
-      self.roll_valid,
-      roll_std < ROLL_STD_MAX,
-      liveParameters.stiffnessFactorValid,
-      liveParameters.steerRatioValid,
-    ))
-    liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
-    liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
-    liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
-    liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
-    if debug:
-      liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message()
-      liveParameters.debugFilterState.value = x.tolist()
-      liveParameters.debugFilterState.std = P.tolist()
-
-    return msg
-
-
-def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: float, lowered_threshold: float):
-  if current_valid:
-    current_valid = abs(val) < threshold
-  else:
-    current_valid = abs(val) < lowered_threshold
-  return current_valid
 
 
 # TODO: Remove this function after few releases (added in 0.9.9)
@@ -258,22 +72,46 @@ def retrieve_initial_vehicle_params(params_reader: Params, CP: car.CarParams, re
   return steer_ratio, stiffness_factor, angle_offset_deg, p_initial
 
 
+def retrieve_initial_lag(params_reader: Params, CP: car.CarParams):
+  last_lag_data = params_reader.get("LiveLag")
+  last_carparams_data = params_reader.get("CarParamsPrevRoute")
+
+  if last_lag_data is not None:
+    try:
+      with log.Event.from_bytes(last_lag_data) as last_lag_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP:
+        ld = last_lag_msg.liveDelay
+        if last_CP.carFingerprint != CP.carFingerprint:
+          raise Exception("Car model mismatch")
+
+        lag, valid_blocks = ld.lateralDelayEstimate, ld.validBlocks
+        return lag, valid_blocks
+    except Exception as e:
+      cloudlog.error(f"Failed to retrieve initial lag: {e}")
+
+  return None
+
+
 def main():
   config_realtime_process([0, 1, 2, 3], 5)
 
   DEBUG = bool(int(os.getenv("DEBUG", "0")))
   REPLAY = bool(int(os.getenv("REPLAY", "0")))
 
-  pm = messaging.PubMaster(['liveParameters'])
+  pm = messaging.PubMaster(['liveParameters', 'liveDelay'])
-  sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState'], poll='livePose')
+  sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', 'controlsState', 'carControl'], poll='livePose')
 
   params_reader = Params()
   CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
 
   migrate_cached_vehicle_params_if_needed(params_reader)
 
-  steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params_reader, CP, REPLAY, DEBUG)
+  steer_ratio, stiffness_factor, angle_offset_deg, p_initial = retrieve_initial_vehicle_params(params_reader, CP, REPLAY, DEBUG)
-  learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial)
+  params_learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), p_initial)
+
+  lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency)
+  if (initial_lag_params := retrieve_initial_lag(params_reader, CP)) is not None:
+    lag, valid_blocks = initial_lag_params
+    lag_learner.reset(lag, valid_blocks)
 
   while True:
     sm.update()
@@ -281,17 +119,30 @@ def main():
       for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]):
         if sm.updated[which]:
           t = sm.logMonoTime[which] * 1e-9
-          learner.handle_log(t, which, sm[which])
+          if which in params_learner.inputs:
+            params_learner.handle_log(t, which, sm[which])
+          if which in lag_learner.inputs:
+            lag_learner.handle_log(t, which, sm[which])
+      lag_learner.update_points()
 
+    params_msg_dat, lag_msg_dat = None, None
     if sm.updated['livePose']:
-      msg = learner.get_msg(sm.all_checks(), debug=DEBUG)
+      params_msg = params_learner.get_msg(sm.all_checks(), debug=DEBUG)
-
+      params_msg_dat = params_msg.to_bytes()
-      msg_dat = msg.to_bytes()
+      pm.send('liveParameters', params_msg_dat)
-      if sm.frame % 1200 == 0:  # once a minute
+
-        params_reader.put_nonblocking("LiveParameters", msg_dat)
+    # 4Hz driven by livePose
-
+    if sm.frame % 5 == 0:
-      pm.send('liveParameters', msg_dat)
+      lag_learner.update_estimate()
-
+      lag_msg = lag_learner.get_msg(sm.all_checks(), DEBUG)
+      lag_msg_dat = lag_msg.to_bytes()
+      pm.send('liveDelay', lag_msg_dat)
+
+    if sm.frame % 1200 == 0: # cache every 60 seconds
+      if params_msg_dat is not None:
+        params_reader.put_nonblocking("LiveParameters", params_msg_dat)
+      if lag_msg_dat is not None:
+        params_reader.put_nonblocking("LiveLag", lag_msg_dat)
 
 if __name__ == "__main__":
   main()

selfdrive/test/process_replay/process_replay.py

@@ -542,8 +542,8 @@ CONFIGS = [
   ),
   ProcessConfig(
     proc_name="paramsd",
-    pubs=["livePose", "liveCalibration", "carState"],
+    pubs=["livePose", "liveCalibration", "carState", "carControl", "controlsState"],
-    subs=["liveParameters"],
+    subs=["liveParameters", "liveDelay"],
     ignore=["logMonoTime"],
     init_callback=get_car_params_callback,
     should_recv_callback=FrequencyBasedRcvCallback("livePose"),

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-887623a18d82088dc5ed9ecdced55eb0d3f718b1
+0ef214e7f4f06d6d591a2257d254f3c00db6a0e9