IQ.Pilot Release Commit @ bec7652

iqpilot/selfdrive/locationd/.gitignore

@@ -0,0 +1,3 @@
+params_learner
+paramsd
+locationd

iqpilot/selfdrive/locationd/SConscript

@@ -0,0 +1,21 @@
+Import('env', 'rednose')
+
+# build ekf models
+rednose_gen_dir = 'models/generated'
+rednose_gen_deps = [
+  "models/constants.py",
+]
+pose_ekf = env.RednoseCompileFilter(
+  target='pose',
+  filter_gen_script='models/pose_kf.py',
+  output_dir=rednose_gen_dir,
+  extra_gen_artifacts=[],
+  gen_script_deps=rednose_gen_deps,
+)
+car_ekf = env.RednoseCompileFilter(
+  target='car',
+  filter_gen_script='models/car_kf.py',
+  output_dir=rednose_gen_dir,
+  extra_gen_artifacts=[],
+  gen_script_deps=rednose_gen_deps,
+)

iqpilot/selfdrive/locationd/calibration_helpers.py

@@ -0,0 +1,32 @@
+import math
+
+import numpy as np
+from iqpilot.cereal import log
+
+from iqpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT, HEIGHT_SANE_MIN, HEIGHT_SANE_MAX
+
+
+def get_calibrated_rpy(live_calib: log.ExtrinsicsCalibration) -> np.ndarray | None:
+  if live_calib.calStatus != log.ExtrinsicsCalibration.Status.calibrated:
+    return None
+
+  if len(live_calib.rpyCalib) != 3:
+    return None
+
+  calib_rpy = np.asarray(live_calib.rpyCalib, dtype=np.float32)
+  return calib_rpy if np.isfinite(calib_rpy).all() else None
+
+
+def get_render_path_height(live_calib: log.ExtrinsicsCalibration) -> float:
+  if live_calib.calStatus != log.ExtrinsicsCalibration.Status.calibrated:
+    return float(HEIGHT_INIT[0])
+
+  if len(live_calib.height) != 1:
+    return float(HEIGHT_INIT[0])
+
+  height = float(live_calib.height[0])
+  if not math.isfinite(height):
+    return float(HEIGHT_INIT[0])
+  if not (HEIGHT_SANE_MIN <= height <= HEIGHT_SANE_MAX):
+    return float(HEIGHT_INIT[0])
+  return height

iqpilot/selfdrive/locationd/calibrationd.py

@@ -0,0 +1,367 @@
+#!/usr/bin/env python3
+'''
+This process finds calibration values. More info on what these calibration values
+are can be found here https://github.com/commaai/openpilot/tree/master/common/transformations
+While the roll calibration is a real value that can be estimated, here we assume it's zero,
+and the image input into the neural network is not corrected for roll.
+'''
+
+import os
+import capnp
+import numpy as np
+from typing import NoReturn
+
+from iqpilot.cereal import log, car
+import iqpilot.cereal.messaging as messaging
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
+from iqpilot.common.issue_debug import log_issue_limited
+from iqpilot.common.realtime import config_realtime_process
+from iqpilot.common.transformations.orientation import rot_from_euler, euler_from_rot
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.system.hardware import HARDWARE
+
+MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS
+MAX_VEL_ANGLE_STD = np.radians(0.25)
+MAX_YAW_RATE_FILTER = np.radians(2)  # per second
+
+MAX_HEIGHT_STD = np.exp(-3.5)
+
+# This is at model frequency, blocks needed for efficiency
+SMOOTH_CYCLES = 10
+BLOCK_SIZE = 100
+INPUTS_NEEDED = 5   # Minimum blocks needed for valid calibration
+INPUTS_WANTED = 50   # We want a little bit more than we need for stability
+MAX_ALLOWED_YAW_SPREAD = np.radians(2)
+MAX_ALLOWED_PITCH_SPREAD = np.radians(4)
+TICI_FAMILY_PITCH_SPREAD_RESET = np.radians(3)
+RPY_INIT = np.array([0.0,0.0,0.0])
+WIDE_FROM_DEVICE_EULER_INIT = np.array([0.0, 0.0, 0.0])
+HEIGHT_INIT = np.array([1.22])
+HEIGHT_SANE_MIN, HEIGHT_SANE_MAX = 0.9, 2.0
+DEVICE_IS_TICI_FAMILY = HARDWARE.get_device_type() in ("tici", "tizi")
+
+# These values are needed to accommodate the model frame in the narrow cam
+if HARDWARE.get_device_type() == 'mici':
+  PITCH_LIMITS = np.array([-0.143101, 0.22235988])
+else:
+  PITCH_LIMITS = np.array([-0.09074112085129739, 0.17])
+YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235])
+DEBUG = os.getenv("DEBUG") is not None
+
+def is_calibration_valid(rpy: np.ndarray) -> bool:
+  return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1])
+
+
+def sanity_clip(rpy: np.ndarray) -> np.ndarray:
+  if np.isnan(rpy).any():
+    rpy = RPY_INIT
+  return np.array([rpy[0],
+                   np.clip(rpy[1], PITCH_LIMITS[0] - .005, PITCH_LIMITS[1] + .005),
+                   np.clip(rpy[2], YAW_LIMITS[0] - .005, YAW_LIMITS[1] + .005)])
+
+def moving_avg_with_linear_decay(prev_mean: np.ndarray, new_val: np.ndarray, idx: int, block_size: float) -> np.ndarray:
+  return (idx*prev_mean + (block_size - idx) * new_val) / block_size
+
+class Calibrator:
+  def __init__(self, param_put: bool = False):
+    self.param_put = param_put
+
+    self.not_car = False
+    self.stable_rpy = RPY_INIT.copy()
+    self.stable_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy()
+    self.stable_height = HEIGHT_INIT.copy()
+    self.has_stable_snapshot = False
+
+    # Read saved calibration
+    self.params = Params()
+    calibration_params = self.params.get("CalibrationParams")
+    rpy_init = RPY_INIT
+    wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
+    height = HEIGHT_INIT
+    valid_blocks = 0
+    self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated
+
+    if param_put and calibration_params:
+      try:
+        with log.Event.from_bytes(calibration_params) as msg:
+          rpy_init = np.array(msg.extrinsicsCalibration.rpyCalib)
+          valid_blocks = msg.extrinsicsCalibration.validBlocks
+          wide_from_device_euler = np.array(msg.extrinsicsCalibration.wideFromDeviceEuler)
+          height = np.array(msg.extrinsicsCalibration.height)
+      except Exception:
+        cloudlog.exception("Error reading cached CalibrationParams")
+
+    self.reset(rpy_init, valid_blocks, wide_from_device_euler, height)
+    self.update_status()
+
+    # If saved calibration is immediately invalid (e.g. bad params from a previous
+    # bootstrap bug or device remount), auto-clear it so we recalibrate from scratch
+    # instead of getting permanently stuck in the "Calibration Invalid" state.
+    if self.cal_status == log.ExtrinsicsCalibration.Status.invalid:
+      cloudlog.warning("calibrationd: saved CalibrationParams are invalid, clearing and starting fresh")
+      if param_put:
+        self.params.remove("CalibrationParams")
+      self.reset()
+      self.update_status()
+
+  def _remember_stable_solution(self) -> None:
+    self.stable_rpy = self.rpy.copy()
+    self.stable_wide_from_device_euler = self.wide_from_device_euler.copy()
+    self.stable_height = self.height.copy()
+    self.has_stable_snapshot = True
+
+  def reset(self, rpy_init: np.ndarray = RPY_INIT,
+                  valid_blocks: int = 0,
+                  wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT,
+                  height_init: np.ndarray = HEIGHT_INIT,
+                  smooth_from: np.ndarray | None = None) -> None:
+    if not np.isfinite(rpy_init).all():
+      self.rpy = RPY_INIT.copy()
+    else:
+      self.rpy = rpy_init.copy()
+
+    if not np.isfinite(height_init).all() or len(height_init) != 1:
+      self.height = HEIGHT_INIT.copy()
+    else:
+      self.height = height_init.copy()
+
+    if not np.isfinite(wide_from_device_euler_init).all() or len(wide_from_device_euler_init) != 3:
+      self.wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy()
+    else:
+      self.wide_from_device_euler = wide_from_device_euler_init.copy()
+
+    if not np.isfinite(valid_blocks) or valid_blocks < 0:
+      self.valid_blocks = 0
+    else:
+      self.valid_blocks = valid_blocks
+
+    self.rpys = np.tile(self.rpy, (INPUTS_WANTED, 1))
+    self.wide_from_device_eulers = np.tile(self.wide_from_device_euler, (INPUTS_WANTED, 1))
+    self.heights = np.tile(self.height, (INPUTS_WANTED, 1))
+
+    self.idx = 0
+    self.block_idx = 0
+    self.v_ego = 0.0
+
+    if smooth_from is None:
+      self.old_rpy = RPY_INIT
+      self.old_rpy_weight = 0.0
+    else:
+      self.old_rpy = smooth_from
+      self.old_rpy_weight = 1.0
+
+  def get_valid_idxs(self) -> list[int]:
+    # exclude current block_idx from validity window
+    before_current = list(range(self.block_idx))
+    after_current = list(range(min(self.valid_blocks, self.block_idx + 1), self.valid_blocks))
+    return before_current + after_current
+
+  def update_status(self) -> None:
+    valid_idxs = self.get_valid_idxs()
+    if valid_idxs:
+      self.wide_from_device_euler = np.mean(self.wide_from_device_eulers[valid_idxs], axis=0)
+      self.height = np.mean(self.heights[valid_idxs], axis=0)
+      rpys = self.rpys[valid_idxs]
+      self.rpy = np.mean(rpys, axis=0)
+      max_rpy_calib = np.array(np.max(rpys, axis=0))
+      min_rpy_calib = np.array(np.min(rpys, axis=0))
+      self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib)
+    else:
+      self.calib_spread = np.zeros(3)
+
+    if self.valid_blocks < INPUTS_NEEDED:
+      if self.cal_status == log.ExtrinsicsCalibration.Status.recalibrating:
+        self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating
+      else:
+        self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated
+    elif is_calibration_valid(self.rpy):
+      self.cal_status = log.ExtrinsicsCalibration.Status.calibrated
+    else:
+      self.cal_status = log.ExtrinsicsCalibration.Status.invalid
+
+    # If spread is too high, assume mounting was changed and reset to last block.
+    # Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model.
+    # TODO: add height spread check with smooth transition too
+    pitch_spread_limit = TICI_FAMILY_PITCH_SPREAD_RESET if DEVICE_IS_TICI_FAMILY else MAX_ALLOWED_PITCH_SPREAD
+    spread_too_high = self.calib_spread[1] > pitch_spread_limit or self.calib_spread[2] > MAX_ALLOWED_YAW_SPREAD
+    if self.cal_status == log.ExtrinsicsCalibration.Status.calibrated and not spread_too_high:
+      self._remember_stable_solution()
+
+    if spread_too_high and self.cal_status == log.ExtrinsicsCalibration.Status.calibrated:
+      use_stable_snapshot = DEVICE_IS_TICI_FAMILY and self.has_stable_snapshot
+      if use_stable_snapshot:
+        reset_rpy = self.stable_rpy
+        reset_wide = self.stable_wide_from_device_euler
+        reset_height = self.stable_height
+      else:
+        reset_rpy = self.rpys[self.block_idx - 1]
+        reset_wide = self.wide_from_device_eulers[self.block_idx - 1]
+        reset_height = self.heights[self.block_idx - 1]
+
+      log_issue_limited(
+        "calibrationd_reset_spread",
+        "calibration",
+        f"calibrationd reset unstable solution pitchSpread={self.calib_spread[1]:.6f} "
+        f"yawSpread={self.calib_spread[2]:.6f} pitchLimit={pitch_spread_limit:.6f} "
+        f"use_stable_snapshot={use_stable_snapshot} rpy={self.rpy.tolist()}",
+        interval_sec=0.5,
+      )
+      self.reset(reset_rpy, valid_blocks=1, wide_from_device_euler_init=reset_wide,
+                 height_init=reset_height, smooth_from=self.stable_rpy if use_stable_snapshot else self.rpy)
+      self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating
+
+    write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
+    if self.param_put and write_this_cycle:
+      self.params.put_nonblocking("CalibrationParams", self.get_msg(True).to_bytes())
+
+  def handle_v_ego(self, v_ego: float) -> None:
+    self.v_ego = v_ego
+
+  def get_smooth_rpy(self) -> np.ndarray:
+    if self.old_rpy_weight > 0:
+      return self.old_rpy_weight * self.old_rpy + (1.0 - self.old_rpy_weight) * self.rpy
+    else:
+      return self.rpy
+
+  def handle_cam_odom(self, trans: list[float],
+                            rot: list[float],
+                            wide_from_device_euler: list[float],
+                            trans_std: list[float],
+                            road_transform_trans: list[float],
+                            road_transform_trans_std: list[float]) -> np.ndarray | None:
+    self.old_rpy_weight = max(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
+
+    fast_enough = self.v_ego > MIN_SPEED_FILTER
+    motion_speed = max(float(self.v_ego), float(trans[0]))
+    cam_fast_enough = motion_speed > MIN_SPEED_FILTER
+    yaw_ok = abs(rot[2]) < MAX_YAW_RATE_FILTER
+    straight_and_fast = fast_enough and cam_fast_enough and yaw_ok
+    angle_std_threshold = MAX_VEL_ANGLE_STD
+    height_std_threshold = MAX_HEIGHT_STD
+    rpy_certain = np.arctan2(trans_std[1], motion_speed) < angle_std_threshold
+    if len(road_transform_trans_std) == 3:
+      height_certain = road_transform_trans_std[2] < height_std_threshold
+    else:
+      height_certain = True
+
+    certain_if_calib = rpy_certain
+    if not (straight_and_fast and certain_if_calib):
+      log_issue_limited(
+        "calibrationd_rejected_sample",
+        "calibration",
+        f"calibrationd rejected sample vEgo={self.v_ego:.2f} trans0={trans[0]:.2f} yawRate={rot[2]:.4f} "
+        f"fast_enough={fast_enough} cam_fast_enough={cam_fast_enough} motion_speed={motion_speed:.2f} yaw_ok={yaw_ok} "
+        f"rpy_certain={rpy_certain} height_certain={height_certain} valid_blocks={self.valid_blocks} idx={self.idx}",
+        interval_sec=1.0,
+      )
+      return None
+
+    observed_rpy = np.array([0,
+                             -np.arctan2(trans[2], trans[0]),
+                             np.arctan2(trans[1], trans[0])])
+    new_rpy = euler_from_rot(rot_from_euler(self.get_smooth_rpy()).dot(rot_from_euler(observed_rpy)))
+    new_rpy = sanity_clip(new_rpy)
+
+    if len(wide_from_device_euler) == 3:
+      new_wide_from_device_euler = np.array(wide_from_device_euler)
+    else:
+      new_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
+
+    if len(road_transform_trans) == 3 and HEIGHT_SANE_MIN <= road_transform_trans[2] <= HEIGHT_SANE_MAX:
+      new_height = np.array([road_transform_trans[2]])
+    else:
+      new_height = HEIGHT_INIT
+
+    self.rpys[self.block_idx] = moving_avg_with_linear_decay(self.rpys[self.block_idx], new_rpy, self.idx, float(BLOCK_SIZE))
+    self.wide_from_device_eulers[self.block_idx] = moving_avg_with_linear_decay(self.wide_from_device_eulers[self.block_idx],
+                                                                                new_wide_from_device_euler, self.idx, float(BLOCK_SIZE))
+    self.heights[self.block_idx] = moving_avg_with_linear_decay(self.heights[self.block_idx], new_height, self.idx, float(BLOCK_SIZE))
+
+    self.idx = (self.idx + 1) % BLOCK_SIZE
+    if self.idx == 0:
+      self.block_idx += 1
+      self.valid_blocks = max(self.block_idx, self.valid_blocks)
+      self.block_idx = self.block_idx % INPUTS_WANTED
+
+    self.update_status()
+
+    if self.idx == 0:
+      log_issue_limited(
+        "calibrationd_progress_block",
+        "calibration",
+        f"calibrationd progress status={int(self.cal_status)} valid_blocks={self.valid_blocks} "
+        f"calPerc={min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)} "
+        f"rpy={self.rpy.tolist()} spread={self.calib_spread.tolist()}",
+        interval_sec=0.5,
+      )
+
+    return new_rpy
+
+  def get_msg(self, valid: bool) -> capnp.lib.capnp._DynamicStructBuilder:
+    smooth_rpy = self.get_smooth_rpy()
+
+    msg = messaging.new_message('extrinsicsCalibration')
+    msg.valid = valid
+
+    extrinsicsCalibration = msg.extrinsicsCalibration
+    extrinsicsCalibration.validBlocks = self.valid_blocks
+    extrinsicsCalibration.calStatus = self.cal_status
+    extrinsicsCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
+    extrinsicsCalibration.rpyCalib = smooth_rpy.tolist()
+    extrinsicsCalibration.rpyCalibSpread = self.calib_spread.tolist()
+    extrinsicsCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist()
+    extrinsicsCalibration.height = self.height.tolist()
+
+    return msg
+
+  def send_data(self, pm: messaging.PubMaster, valid: bool) -> None:
+    pm.send('extrinsicsCalibration', self.get_msg(valid))
+
+
+def main() -> NoReturn:
+  config_realtime_process([0, 1, 2, 3], 5)
+
+  pm = messaging.PubMaster(['extrinsicsCalibration'])
+  sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll='cameraOdometry')
+
+  params_reader = Params()
+  CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
+
+  calibrator = Calibrator(param_put=True)
+  calibrator.not_car = CP.notCar
+
+  while 1:
+    timeout = 0 if sm.frame == -1 else 100
+    sm.update(timeout)
+
+    if sm.updated['cameraOdometry']:
+      calibrator.handle_v_ego(sm['carState'].vEgo)
+      new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
+                                           sm['cameraOdometry'].rot,
+                                           sm['cameraOdometry'].wideFromDeviceEuler,
+                                           sm['cameraOdometry'].transStd,
+                                           sm['cameraOdometry'].roadTransformTrans,
+                                           sm['cameraOdometry'].roadTransformTransStd)
+
+      if DEBUG and new_rpy is not None:
+        print('got new rpy', new_rpy)
+
+    # 4Hz driven by cameraOdometry
+    if sm.frame % 5 == 0:
+      checks_ok = sm.all_checks()
+      if not checks_ok:
+        ft = sm.freq_tracker
+        recv_hz = {s: (round(1.0 / ft[s].avg_dt.get_average(), 2) if ft[s].avg_dt.count else None) for s in sm.services}
+        log_issue_limited(
+          "calibrationd_checks_failed",
+          "calibration",
+          f"calibrationd all_checks failed alive={sm.alive} freq_ok={sm.freq_ok} valid={sm.valid} "
+          f"seen={sm.seen} recv_hz={recv_hz}",
+          interval_sec=5.0,
+        )
+      calibrator.send_data(pm, checks_ok)
+
+
+if __name__ == "__main__":
+  main()

iqpilot/selfdrive/locationd/estimatord.py

@@ -0,0 +1,103 @@
+#!/usr/bin/env python3
+import os
+
+import numpy as np
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import car
+from iqpilot.cereal.services import SERVICE_LIST
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import config_realtime_process
+from iqpilot.common.steer_delay import SteerDelayPublisher
+from iqpilot.selfdrive.locationd.lagd import LateralLagEstimator, retrieve_initial_lag
+from iqpilot.selfdrive.locationd.paramsd import (
+  VehicleParamsEstimator,
+  migrate_cached_vehicle_params_if_needed,
+  retrieve_initial_vehicle_params,
+)
+from iqpilot.selfdrive.locationd.torqued import TorqueEstimator
+
+
+PARAMS_SERVICES = ['deviceMotion', 'extrinsicsCalibration', 'carState']
+LAG_SERVICES = ['deviceMotion', 'extrinsicsCalibration', 'carState', 'controlsState', 'carControl']
+TORQUE_SERVICES = ['carControl', 'carOutput', 'carState', 'extrinsicsCalibration', 'deviceMotion']
+SUBSCRIBED_SERVICES = list(dict.fromkeys(PARAMS_SERVICES + LAG_SERVICES + TORQUE_SERVICES))
+
+
+def main() -> None:
+  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(['vehicleParameters', 'lateralDelay', 'lateralTorqueParameters'])
+  sm = messaging.SubMaster(SUBSCRIBED_SERVICES, poll='deviceMotion')
+
+  params = Params()
+  CP = messaging.log_from_bytes(params.get('CarParams', block=True), car.CarParams)
+
+  migrate_cached_vehicle_params_if_needed(params)
+  steer_ratio, stiffness_factor, angle_offset_deg, p_initial = retrieve_initial_vehicle_params(params, CP, replay, debug)
+  params_estimator = VehicleParamsEstimator(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), p_initial)
+
+  lag_estimator = LateralLagEstimator(CP, 1. / SERVICE_LIST['deviceMotion'].frequency)
+  if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None:
+    lag, valid_blocks = initial_lag_params
+    lag_estimator.reset(lag, valid_blocks)
+
+  torque_estimator = TorqueEstimator(CP)
+  steer_delay_publisher = SteerDelayPublisher(CP)
+  estimators = (
+    (params_estimator, PARAMS_SERVICES),
+    (lag_estimator, LAG_SERVICES),
+    (torque_estimator, TORQUE_SERVICES),
+  )
+
+  while True:
+    sm.update()
+    valid = sm.all_checks()
+
+    if valid:
+      for which in sorted(sm.updated, key=lambda x: sm.logMonoTime[x]):
+        if not sm.updated[which]:
+          continue
+
+        t = sm.logMonoTime[which] * 1e-9
+        for estimator, services in estimators:
+          if which in services:
+            estimator.handle_log(t, which, sm[which])
+      lag_estimator.update_points()
+
+    if not sm.updated['deviceMotion']:
+      continue
+
+    params_msg = params_estimator.get_msg(valid, debug=debug)
+    params_msg_dat = params_msg.to_bytes()
+    if sm.frame % 1200 == 0:
+      params.put_nonblocking('LiveParametersV2', params_msg_dat)
+    pm.send('vehicleParameters', params_msg_dat)
+
+    if sm.frame % 5 != 0:
+      continue
+
+    lag_estimator.update_estimate()
+    lag_msg = lag_estimator.get_msg(valid, debug)
+    lag_msg_dat = lag_msg.to_bytes()
+    pm.send('lateralDelay', lag_msg_dat)
+
+    torque_estimator.handle_log(sm.logMonoTime['deviceMotion'] * 1e-9, 'lateralDelay', lag_msg.lateralDelay)
+    pm.send('lateralTorqueParameters', torque_estimator.get_msg(valid=valid, with_points=debug))
+
+    if sm.frame % 1200 == 0:
+      params.put_nonblocking('LiveDelay', lag_msg_dat)
+
+    if sm.frame % 60 == 0:
+      steer_delay_publisher.update(lag_msg)
+
+    if sm.frame % 240 == 0:
+      torque_msg = torque_estimator.get_msg(valid=valid, with_points=True)
+      params.put_nonblocking('LiveTorqueParameters', torque_msg.to_bytes())
+
+
+if __name__ == '__main__':
+  main()

iqpilot/selfdrive/locationd/helpers.py

@@ -0,0 +1,188 @@
+import numpy as np
+from typing import Any
+from functools import cache
+
+from iqpilot.cereal import log
+from iqpilot.common.transformations.orientation import rot_from_euler, euler_from_rot
+from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
+
+
+@cache
+def fft_next_good_size(n: int) -> int:
+    """
+    smallest composite of 2, 3, 5, 7, 11 that is >= n
+    inspired by pocketfft
+    """
+    if n <= 6:
+      return n
+    best, f2 = 2 * n, 1
+    while f2 < best:
+        f23 = f2
+        while f23 < best:
+            f235 = f23
+            while f235 < best:
+                f2357 = f235
+                while f2357 < best:
+                    f235711 = f2357
+                    while f235711 < best:
+                        best = f235711 if f235711 >= n else best
+                        f235711 *= 11
+                    f2357 *= 7
+                f235 *= 5
+            f23 *= 3
+        f2 *= 2
+    return best
+
+
+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 rotate_cov(rot_matrix, cov_in):
+  return rot_matrix @ cov_in @ rot_matrix.T
+
+
+def rotate_std(rot_matrix, std_in):
+  return np.sqrt(np.diag(rotate_cov(rot_matrix, np.diag(std_in**2))))
+
+
+class NPQueue:
+  def __init__(self, maxlen: int, rowsize: int) -> None:
+    self.maxlen = maxlen
+    self.arr = np.empty((0, rowsize))
+
+  def __len__(self) -> int:
+    return len(self.arr)
+
+  def append(self, pt: list[float]) -> None:
+    if len(self.arr) < self.maxlen:
+      self.arr = np.append(self.arr, [pt], axis=0)
+    else:
+      self.arr[:-1] = self.arr[1:]
+      self.arr[-1] = pt
+
+
+class PointBuckets:
+  def __init__(self, x_bounds: list[tuple[float, float]], min_points: list[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None:
+    self.x_bounds = x_bounds
+    self.buckets = {bounds: NPQueue(maxlen=points_per_bucket, rowsize=rowsize) for bounds in x_bounds}
+    self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True))
+    self.min_points_total = min_points_total
+
+  def __len__(self) -> int:
+    return sum([len(v) for v in self.buckets.values()])
+
+  def is_valid(self) -> bool:
+    individual_buckets_valid = all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values(), strict=True))
+    total_points_valid = self.__len__() >= self.min_points_total
+    return individual_buckets_valid and total_points_valid
+
+  def get_valid_percent(self) -> int:
+    total_points_perc = min(self.__len__() / self.min_points_total * 100, 100)
+    individual_buckets_perc = min(min(len(v) / min_pts * 100 for v, min_pts in
+                                      zip(self.buckets.values(), self.buckets_min_points.values(), strict=True)), 100)
+    return int((total_points_perc + individual_buckets_perc) / 2)
+
+  def is_calculable(self) -> bool:
+    return all(len(v) > 0 for v in self.buckets.values())
+
+  def add_point(self, x: float, y: float) -> None:
+    raise NotImplementedError
+
+  def get_points(self, num_points: int | None = None) -> Any:
+    points = np.vstack([x.arr for x in self.buckets.values()])
+    if num_points is None:
+      return points
+    return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)]
+
+  def load_points(self, points: list[list[float]]) -> None:
+    for point in points:
+      self.add_point(*point)
+
+
+class ParameterEstimator:
+  """ Base class for parameter estimators """
+  def reset(self) -> None:
+    raise NotImplementedError
+
+  def handle_log(self, t: int, which: str, msg: log.Event) -> None:
+    raise NotImplementedError
+
+  def get_msg(self, valid: bool, with_points: bool) -> log.Event:
+    raise NotImplementedError
+
+
+class Measurement:
+  x, y, z = (property(lambda self: self.xyz[0]), property(lambda self: self.xyz[1]), property(lambda self: self.xyz[2]))
+  x_std, y_std, z_std = (property(lambda self: self.xyz_std[0]), property(lambda self: self.xyz_std[1]), property(lambda self: self.xyz_std[2]))
+  roll, pitch, yaw = x, y, z
+  roll_std, pitch_std, yaw_std = x_std, y_std, z_std
+
+  def __init__(self, xyz: np.ndarray, xyz_std: np.ndarray):
+    self.xyz: np.ndarray = xyz
+    self.xyz_std: np.ndarray = xyz_std
+
+  @classmethod
+  def from_measurement_xyz(cls, measurement: log.DeviceMotion.XYZMeasurement) -> 'Measurement':
+    return cls(
+      xyz=np.array([measurement.x, measurement.y, measurement.z]),
+      xyz_std=np.array([measurement.xStd, measurement.yStd, measurement.zStd])
+    )
+
+
+class Pose:
+  def __init__(self, orientation: Measurement, velocity: Measurement, acceleration: Measurement, angular_velocity: Measurement):
+    self.orientation = orientation
+    self.velocity = velocity
+    self.acceleration = acceleration
+    self.angular_velocity = angular_velocity
+
+  @classmethod
+  def from_live_pose(cls, live_pose: log.DeviceMotion) -> 'Pose':
+    return Pose(
+      orientation=Measurement.from_measurement_xyz(live_pose.orientationNED),
+      velocity=Measurement.from_measurement_xyz(live_pose.velocityDevice),
+      acceleration=Measurement.from_measurement_xyz(live_pose.accelerationDevice),
+      angular_velocity=Measurement.from_measurement_xyz(live_pose.angularVelocityDevice)
+    )
+
+
+class PoseCalibrator:
+  def __init__(self):
+    self.calib_valid = False
+    self.calib_from_device = np.eye(3)
+
+  def _transform_calib_from_device(self, meas: Measurement):
+    new_xyz = self.calib_from_device @ meas.xyz
+    new_xyz_std = rotate_std(self.calib_from_device, meas.xyz_std)
+    return Measurement(new_xyz, new_xyz_std)
+
+  def _ned_from_calib(self, orientation: Measurement):
+    ned_from_device = rot_from_euler(orientation.xyz)
+    ned_from_calib = ned_from_device @ self.calib_from_device.T
+    ned_from_calib_euler_meas = Measurement(euler_from_rot(ned_from_calib), np.full(3, np.nan))
+    return ned_from_calib_euler_meas
+
+  def build_calibrated_pose(self, pose: Pose) -> Pose:
+    ned_from_calib_euler = self._ned_from_calib(pose.orientation)
+    angular_velocity_calib = self._transform_calib_from_device(pose.angular_velocity)
+    acceleration_calib = self._transform_calib_from_device(pose.acceleration)
+    velocity_calib = self._transform_calib_from_device(pose.velocity)
+
+    return Pose(ned_from_calib_euler, velocity_calib, acceleration_calib, angular_velocity_calib)
+
+  def feed_live_calib(self, live_calib: log.ExtrinsicsCalibration):
+    calib_rpy = get_calibrated_rpy(live_calib)
+    if calib_rpy is not None:
+      self.calib_from_device = rot_from_euler(calib_rpy).T
+      self.calib_valid = True
+    else:
+      if not self.calib_valid:
+        self.calib_from_device = np.eye(3)
+      self.calib_valid = False

iqpilot/selfdrive/locationd/lagd.py

@@ -0,0 +1,381 @@
+#!/usr/bin/env python3
+import numpy as np
+import capnp
+from collections import deque
+from functools import partial
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import car, log
+from iqpilot.common.params import Params
+from iqpilot.common.constants import CV
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose, fft_next_good_size, parabolic_peak_interp
+
+BLOCK_SIZE = 100
+BLOCK_NUM = 50
+BLOCK_NUM_NEEDED = 5
+MOVING_WINDOW_SEC = 60.0
+MIN_OKAY_WINDOW_SEC = 25.0
+MIN_RECOVERY_BUFFER_SEC = 2.0
+MIN_VEGO = 50.0 * CV.MPH_TO_MS
+MIN_ABS_YAW_RATE = 0.0
+MAX_YAW_RATE_SANITY_CHECK = 1.0
+MIN_NCC = 0.95
+MAX_LAG = 1.0
+MIN_LAG = 0.15
+MAX_LAG_STD = 0.1
+MAX_LAT_ACCEL = 2.0
+MAX_LAT_ACCEL_DIFF = 0.6
+MIN_LAT_ACCEL_RANGE = 0.5
+MIN_CONFIDENCE = 0.7
+CORR_BORDER_OFFSET = 5
+LAG_CANDIDATE_CORR_THRESHOLD = 0.9
+SMOOTH_K = 5
+SMOOTH_SIGMA = 1.0
+
+VERSION = 1  # bump this to invalidate old parameter caches
+
+
+def masked_symmetric_moving_average(x: np.ndarray, mask: np.ndarray, k: int, sigma: float) -> np.ndarray:
+  assert k >= 1 and k % 2 == 1, "k must be positive and odd"
+  pad = k // 2
+  i = np.arange(k) - pad
+  w = np.exp(-0.5 * (i / sigma) ** 2)
+  w /= w.sum()
+  xp = np.pad(x * mask, pad, mode="edge")
+  mp = np.pad(mask, pad, mode="edge")
+  num = np.convolve(xp, w, mode="valid")
+  den = np.convolve(mp, w, mode="valid")
+  return np.divide(num, den, out=np.full_like(num, np.nan, dtype=np.float64), where=den != 0)
+
+
+def masked_normalized_cross_correlation(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray, n: int):
+  """
+  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]
+
+  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: int):
+    self.times = deque[float]([0.0] * num_points, maxlen=num_points)
+    self.okay = deque[bool]([False] * num_points, maxlen=num_points)
+    self.desired = deque[float]([0.0] * num_points, maxlen=num_points)
+    self.actual = deque[float]([0.0] * num_points, 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: float, desired: float, actual: float, okay: bool):
+    self.times.append(t)
+    self.okay.append(okay)
+    self.desired.append(desired)
+    self.actual.append(actual)
+
+  def get(self) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
+    return np.array(self.times), np.array(self.desired), np.array(self.actual), np.array(self.okay)
+
+
+class BlockAverage:
+  def __init__(self, num_blocks: int, block_size: int, valid_blocks: int, initial_value: float):
+    self.num_blocks = num_blocks
+    self.block_size = block_size
+    self.block_idx = valid_blocks % num_blocks
+    self.idx = 0
+
+    self.values = np.tile(initial_value, (num_blocks, 1))
+    self.valid_blocks = valid_blocks
+
+  def update(self, value: float):
+    self.values[self.block_idx] = (self.idx * self.values[self.block_idx] + value) / (self.idx + 1)
+    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) -> tuple[float, float, float, float]:
+    valid_block_idx = [i for i in range(self.valid_blocks) if i != self.block_idx]
+    valid_and_current_idx = valid_block_idx + ([self.block_idx] if self.idx > 0 else [])
+
+    if len(valid_block_idx) > 0:
+      valid_mean = float(np.mean(self.values[valid_block_idx], axis=0).item())
+      valid_std = float(np.std(self.values[valid_block_idx], axis=0).item())
+    else:
+      valid_mean, valid_std = float('nan'), float('nan')
+
+    if len(valid_and_current_idx) > 0:
+      current_mean = float(np.mean(self.values[valid_and_current_idx], axis=0).item())
+      current_std = float(np.std(self.values[valid_and_current_idx], axis=0).item())
+    else:
+      current_mean, current_std = float('nan'), float('nan')
+
+    return valid_mean, valid_std, current_mean, current_std
+
+
+class LateralLagEstimator:
+  inputs = {"carControl", "carState", "controlsState", "extrinsicsCalibration", "deviceMotion"}
+
+  def __init__(self, CP: car.CarParams, dt: float,
+               block_count: int = BLOCK_NUM, min_valid_block_count: int = BLOCK_NUM_NEEDED, block_size: int = BLOCK_SIZE,
+               window_sec: float = MOVING_WINDOW_SEC, okay_window_sec: float = MIN_OKAY_WINDOW_SEC, min_recovery_buffer_sec: float = MIN_RECOVERY_BUFFER_SEC,
+               min_vego: float = MIN_VEGO, min_yr: float = MIN_ABS_YAW_RATE, min_ncc: float = MIN_NCC,
+               max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE):
+    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.min_confidence = min_confidence
+    self.max_lat_accel = max_lat_accel
+    self.max_lat_accel_diff = max_lat_accel_diff
+
+    self.t = 0.0
+    self.lat_active = False
+    self.steering_pressed = False
+    self.steering_saturated = False
+    self.desired_curvature = 0.0
+    self.v_ego = 0.0
+    self.yaw_rate = 0.0
+    self.yaw_rate_std = 0.0
+    self.pose_valid = False
+
+    self.last_lat_inactive_t = 0.0
+    self.last_steering_pressed_t = 0.0
+    self.last_steering_saturated_t = 0.0
+    self.last_pose_invalid_t = 0.0
+    self.last_estimate_t = 0.0
+
+    self.calibrator = PoseCalibrator()
+
+    self.reset(self.initial_lag, 0)
+
+  def reset(self, initial_lag: float, valid_blocks: int):
+    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: bool, debug: bool = False) -> capnp._DynamicStructBuilder:
+    msg = messaging.new_message('lateralDelay')
+
+    msg.valid = valid
+
+    lateralDelay = msg.lateralDelay
+
+    valid_mean_lag, valid_std, current_mean_lag, current_std = self.block_avg.get()
+    if self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std):
+      if valid_std > MAX_LAG_STD:
+        lateralDelay.status = log.LateralDelay.Status.invalid
+      else:
+        lateralDelay.status = log.LateralDelay.Status.estimated
+    else:
+      lateralDelay.status = log.LateralDelay.Status.unestimated
+
+    if lateralDelay.status == log.LateralDelay.Status.estimated:
+      lateralDelay.lateralDelay = min(MAX_LAG, max(MIN_LAG, valid_mean_lag))
+    else:
+      lateralDelay.lateralDelay = self.initial_lag
+
+    if not np.isnan(current_mean_lag) and not np.isnan(current_std):
+      lateralDelay.lateralDelayEstimate = current_mean_lag
+      lateralDelay.lateralDelayEstimateStd = current_std
+    else:
+      lateralDelay.lateralDelayEstimate = self.initial_lag
+      lateralDelay.lateralDelayEstimateStd = 0.0
+
+    lateralDelay.validBlocks = self.block_avg.valid_blocks
+    lateralDelay.calPerc = min(100 * (self.block_avg.valid_blocks * self.block_size + self.block_avg.idx) //
+                            (self.min_valid_block_count * self.block_size), 100)
+    if debug:
+      lateralDelay.points = self.block_avg.values.flatten().tolist()
+    lateralDelay.version = VERSION
+
+    return msg
+
+  def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader):
+    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 == "extrinsicsCalibration":
+      self.calibrator.feed_live_calib(msg)
+    elif which == "deviceMotion":
+      device_pose = Pose.from_live_pose(msg)
+      calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
+      self.yaw_rate = calibrated_pose.angular_velocity.yaw
+      self.yaw_rate_std = calibrated_pose.angular_velocity.yaw_std
+      self.pose_valid = msg.angularVelocityDevice.valid and msg.posenetOK and msg.inputsOK
+    self.t = t
+
+  def points_enough(self):
+    return self.points.num_points >= int(self.okay_window_sec / self.dt)
+
+  def points_valid(self):
+    return self.points.num_okay >= int(self.okay_window_sec / self.dt)
+
+  def update_points(self):
+    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
+    sensors_valid = self.pose_valid and np.abs(self.yaw_rate) < MAX_YAW_RATE_SANITY_CHECK and self.yaw_rate_std < MAX_YAW_RATE_SANITY_CHECK
+    la_valid = np.abs(la_actual_pose) <= self.max_lat_accel and np.abs(la_desired - la_actual_pose) <= self.max_lat_accel_diff
+    calib_valid = self.calibrator.calib_valid
+
+    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
+    if not sensors_valid or not la_valid:
+      self.last_pose_invalid_t = self.t
+
+    has_recovered = all( # wait for recovery after !lat_active, steering_pressed, steering_saturated, !sensors/la_valid
+      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, self.last_pose_invalid_t]
+    )
+    okay = self.lat_active and not self.steering_pressed and not self.steering_saturated and \
+           fast and turning and has_recovered and calib_valid and sensors_valid and la_valid
+
+    self.points.update(self.t, la_desired, la_actual_pose, okay)
+
+  def update_estimate(self):
+    if not self.points_enough():
+      return
+
+    times, desired, actual, okay = self.points.get()
+    # check if there are any new valid data points since the last update
+    is_valid = self.points_valid() and (actual.max() - actual.min() >= MIN_LAT_ACCEL_RANGE)
+    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)
+      is_valid = is_valid and not (new_values_start_idx == 0 or not np.any(okay[new_values_start_idx:]))
+
+    desired = masked_symmetric_moving_average(desired, okay, SMOOTH_K, SMOOTH_SIGMA)
+    actual = masked_symmetric_moving_average(actual, okay, SMOOTH_K, SMOOTH_SIGMA)
+
+    delay, corr, confidence = self.actuator_delay(desired, actual, okay, self.dt, MIN_LAG, MAX_LAG)
+    if corr < self.min_ncc or confidence < self.min_confidence or not is_valid:
+      return
+
+    self.block_avg.update(delay)
+    self.last_estimate_t = self.t
+
+  @staticmethod
+  def actuator_delay(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray,
+                     dt: float, min_lag: float, max_lag: float) -> tuple[float, float, float]:
+    assert len(expected_sig) == len(actual_sig)
+    min_lag_samples, max_lag_samples, one_sec_samples = int(round(min_lag / dt)), int(round(max_lag / dt)), int(round(1.0 / dt))
+    padded_size = fft_next_good_size(len(expected_sig) + max(max_lag_samples, one_sec_samples))
+
+    ncc = masked_normalized_cross_correlation(expected_sig, actual_sig, mask, padded_size)
+
+    # only consider lags from ranges:
+    roi = np.s_[len(expected_sig) - 1 + min_lag_samples: len(expected_sig) - 1 + max_lag_samples] # min_lag - max_lag range
+    threshold_roi = np.s_[len(expected_sig) - 1: len(expected_sig) - 1 + one_sec_samples] # 0 - 1 second range
+    confidence_roi = np.s_[threshold_roi.start - CORR_BORDER_OFFSET: threshold_roi.stop + CORR_BORDER_OFFSET] # threshold range +/- border
+    roi_ncc, confidence_roi_ncc, threshold_roi_ncc = ncc[roi], ncc[confidence_roi], ncc[threshold_roi]
+
+    max_corr_index = np.argmax(roi_ncc)
+    corr = roi_ncc[max_corr_index]
+    lag = parabolic_peak_interp(roi_ncc, max_corr_index) * dt + min_lag
+
+    # to estimate lag confidence, gather all high-correlation candidates and see how spread they are
+    # if e.g. 0.8 and 0.4 are both viable, this is an ambiguous case
+    ncc_thresh = (threshold_roi_ncc.max() - threshold_roi_ncc.min()) * LAG_CANDIDATE_CORR_THRESHOLD + threshold_roi_ncc.min()
+    good_lag_candidate_mask = confidence_roi_ncc >= ncc_thresh
+    good_lag_candidate_edges = np.diff(good_lag_candidate_mask.astype(int), prepend=0, append=0)
+    starts, ends = np.where(good_lag_candidate_edges == 1)[0], np.where(good_lag_candidate_edges == -1)[0] - 1
+    run_idx = np.searchsorted(starts, max_corr_index + CORR_BORDER_OFFSET, side='right') - 1
+    width = ends[run_idx] - starts[run_idx] + 1
+    confidence = np.clip(1 - width * dt, 0, 1)
+
+    return lag, corr, confidence
+
+
+def retrieve_initial_lag(params: Params, CP: car.CarParams):
+  last_lag_data = params.get("LiveDelay")
+  last_carparams_data = params.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.lateralDelay
+        if last_CP.carFingerprint != CP.carFingerprint:
+          raise Exception("Car model mismatch")
+
+        lag, valid_blocks, status, version = ld.lateralDelayEstimate, ld.validBlocks, ld.status, ld.version
+        assert valid_blocks <= BLOCK_NUM, "Invalid number of valid blocks"
+        assert status != log.LateralDelay.Status.invalid, "Lag estimate is invalid"
+        assert version == VERSION, f"Lag estimate is from a different version (got {version}, expected {VERSION})"
+        return lag, valid_blocks
+    except Exception as e:
+      cloudlog.error(f"Failed to retrieve initial lag: {e}")
+      params.remove("LiveDelay")
+
+  return None

iqpilot/selfdrive/locationd/locationd.py

@@ -0,0 +1,336 @@
+#!/usr/bin/env python3
+import os
+import time
+import capnp
+import numpy as np
+from enum import Enum
+from collections import defaultdict
+
+from iqpilot.cereal import log, messaging
+from iqpilot.cereal.services import SERVICE_LIST
+from iqpilot.common.transformations.orientation import rot_from_euler
+from iqpilot.common.realtime import config_realtime_process
+from iqpilot.common.params import Params
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
+from iqpilot.selfdrive.locationd.helpers import rotate_std
+from iqpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States
+from iqpilot.selfdrive.locationd.models.constants import ObservationKind, GENERATED_DIR
+
+ACCEL_SANITY_CHECK = 100.0  # m/s^2
+ROTATION_SANITY_CHECK = 10.0  # rad/s
+TRANS_SANITY_CHECK = 200.0  # m/s
+CALIB_RPY_SANITY_CHECK = 0.5  # rad (+- 30 deg)
+MIN_STD_SANITY_CHECK = 1e-5  # m or rad
+MAX_FILTER_REWIND_TIME = 0.8  # s
+MAX_SENSOR_TIME_DIFF = 0.1  # s
+YAWRATE_CROSS_ERR_CHECK_FACTOR = 30
+INPUT_INVALID_LIMIT = 2.0 # 1 (camodo) / 9 (sensor) bad input[s] ignored
+INPUT_INVALID_RECOVERY = 10.0 # ~10 secs to resume after exceeding allowed bad inputs by one
+POSENET_STD_INITIAL_VALUE = 10.0
+POSENET_STD_HIST_HALF = 20
+
+
+def calculate_invalid_input_decay(invalid_limit, recovery_time, frequency):
+  return (1 - 1 / (2 * invalid_limit)) ** (1 / (recovery_time * frequency))
+
+
+def init_xyz_measurement(measurement: capnp._DynamicStructBuilder, values: np.ndarray, stds: np.ndarray, valid: bool):
+  assert len(values) == len(stds) == 3
+  measurement.x, measurement.y, measurement.z = map(float, values)
+  measurement.xStd, measurement.yStd, measurement.zStd = map(float, stds)
+  measurement.valid = valid
+
+
+class HandleLogResult(Enum):
+  SUCCESS = 0
+  TIMING_INVALID = 1
+  INPUT_INVALID = 2
+  SENSOR_SOURCE_INVALID = 3
+
+
+class LocationEstimator:
+  def __init__(self, debug: bool):
+    self.kf = PoseKalman(GENERATED_DIR, MAX_FILTER_REWIND_TIME)
+
+    self.debug = debug
+
+    self.posenet_stds = np.array([POSENET_STD_INITIAL_VALUE] * (POSENET_STD_HIST_HALF * 2))
+    self.car_speed = 0.0
+    self.camodo_yawrate_distribution = np.array([0.0, 10.0])  # mean, std
+    self.device_from_calib = np.eye(3)
+
+    obs_kinds = [ObservationKind.PHONE_ACCEL, ObservationKind.PHONE_GYRO, ObservationKind.CAMERA_ODO_ROTATION, ObservationKind.CAMERA_ODO_TRANSLATION]
+    self.observations = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds}
+    self.observation_errors = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds}
+
+  def reset(self, t: float, x_initial: np.ndarray = PoseKalman.initial_x, P_initial: np.ndarray = PoseKalman.initial_P):
+    self.kf.init_state(x_initial, covs=P_initial, filter_time=t)
+
+  def _validate_sensor_source(self, source: log.SensorEventData.SensorSource):
+    # some segments have two IMUs, ignore the second one
+    return source != log.SensorEventData.SensorSource.bmx055
+
+  def _validate_sensor_time(self, sensor_time: float, t: float):
+    # ignore empty readings
+    if sensor_time == 0:
+      return False
+
+    # sensor time and log time should be close
+    sensor_time_invalid = abs(sensor_time - t) > MAX_SENSOR_TIME_DIFF
+    if sensor_time_invalid:
+      cloudlog.warning("Sensor reading ignored, sensor timestamp more than 100ms off from log time")
+    return not sensor_time_invalid
+
+  def _validate_timestamp(self, t: float):
+    kf_t = self.kf.t
+    invalid = not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME
+    if invalid:
+      cloudlog.warning("Observation timestamp is older than the max rewind threshold of the filter")
+    return not invalid
+
+  def _finite_check(self, t: float, new_x: np.ndarray, new_P: np.ndarray):
+    all_finite = np.isfinite(new_x).all() and np.isfinite(new_P).all()
+    if not all_finite:
+      cloudlog.error("Non-finite values detected, kalman reset")
+      self.reset(t)
+
+  def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> HandleLogResult:
+    new_x, new_P = None, None
+    if which == "accelerometer" and msg.which() == "acceleration":
+      sensor_time = msg.timestamp * 1e-9
+
+      if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time):
+        return HandleLogResult.TIMING_INVALID
+
+      if not self._validate_sensor_source(msg.source):
+        return HandleLogResult.SENSOR_SOURCE_INVALID
+
+      v = msg.acceleration.v
+      meas = np.array([-v[2], -v[1], -v[0]])
+      if np.linalg.norm(meas) >= ACCEL_SANITY_CHECK:
+        return HandleLogResult.INPUT_INVALID
+
+      acc_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_ACCEL, meas)
+      if acc_res is not None:
+        _, new_x, _, new_P, _, _, (acc_err,), _, _ = acc_res
+        self.observation_errors[ObservationKind.PHONE_ACCEL] = np.array(acc_err)
+        self.observations[ObservationKind.PHONE_ACCEL] = meas
+
+    elif which == "gyroscope" and msg.which() == "gyroUncalibrated":
+      sensor_time = msg.timestamp * 1e-9
+
+      if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time):
+        return HandleLogResult.TIMING_INVALID
+
+      if not self._validate_sensor_source(msg.source):
+        return HandleLogResult.SENSOR_SOURCE_INVALID
+
+      v = msg.gyroUncalibrated.v
+      meas = np.array([-v[2], -v[1], -v[0]])
+
+      gyro_bias = self.kf.x[States.GYRO_BIAS]
+      gyro_camodo_yawrate_err = np.abs((meas[2] - gyro_bias[2]) - self.camodo_yawrate_distribution[0])
+      gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * self.camodo_yawrate_distribution[1]
+      gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold
+
+      if np.linalg.norm(meas) >= ROTATION_SANITY_CHECK or not gyro_valid:
+        return HandleLogResult.INPUT_INVALID
+
+      gyro_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_GYRO, meas)
+      if gyro_res is not None:
+        _, new_x, _, new_P, _, _, (gyro_err,), _, _ = gyro_res
+        self.observation_errors[ObservationKind.PHONE_GYRO] = np.array(gyro_err)
+        self.observations[ObservationKind.PHONE_GYRO] = meas
+
+    elif which == "carState":
+      self.car_speed = abs(msg.vEgo)
+
+    elif which == "extrinsicsCalibration":
+      # Note that we use this message during calibration
+      calib = get_calibrated_rpy(msg)
+      if calib is None and len(msg.rpyCalib) > 0:
+        calib = np.array(msg.rpyCalib)
+
+      if calib is not None:
+        if calib.min() < -CALIB_RPY_SANITY_CHECK or calib.max() > CALIB_RPY_SANITY_CHECK:
+          return HandleLogResult.INPUT_INVALID
+
+        self.device_from_calib = rot_from_euler(calib)
+
+    elif which == "cameraOdometry":
+      if not self._validate_timestamp(t):
+        return HandleLogResult.TIMING_INVALID
+
+      rot_device = np.matmul(self.device_from_calib, np.array(msg.rot))
+      trans_device = np.matmul(self.device_from_calib, np.array(msg.trans))
+
+      if np.linalg.norm(rot_device) > ROTATION_SANITY_CHECK or np.linalg.norm(trans_device) > TRANS_SANITY_CHECK:
+        return HandleLogResult.INPUT_INVALID
+
+      rot_calib_std = np.array(msg.rotStd)
+      trans_calib_std = np.array(msg.transStd)
+
+      if rot_calib_std.min() <= MIN_STD_SANITY_CHECK or trans_calib_std.min() <= MIN_STD_SANITY_CHECK:
+        return HandleLogResult.INPUT_INVALID
+
+      if np.linalg.norm(rot_calib_std) > 10 * ROTATION_SANITY_CHECK or np.linalg.norm(trans_calib_std) > 10 * TRANS_SANITY_CHECK:
+        return HandleLogResult.INPUT_INVALID
+
+      self.posenet_stds = np.roll(self.posenet_stds, -1)
+      self.posenet_stds[-1] = trans_calib_std[0]
+
+      # Multiply by N to avoid to high certainty in kalman filter because of temporally correlated noise
+      rot_calib_std *= 10
+      trans_calib_std *= 2
+
+      rot_device_std = rotate_std(self.device_from_calib, rot_calib_std)
+      trans_device_std = rotate_std(self.device_from_calib, trans_calib_std)
+      rot_device_noise = rot_device_std ** 2
+      trans_device_noise = trans_device_std ** 2
+
+      cam_odo_rot_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_ROTATION, rot_device, np.array([np.diag(rot_device_noise)]))
+      cam_odo_trans_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_TRANSLATION, trans_device, np.array([np.diag(trans_device_noise)]))
+      self.camodo_yawrate_distribution =  np.array([rot_device[2], rot_device_std[2]])
+      if cam_odo_rot_res is not None:
+        _, new_x, _, new_P, _, _, (cam_odo_rot_err,), _, _ = cam_odo_rot_res
+        self.observation_errors[ObservationKind.CAMERA_ODO_ROTATION] = np.array(cam_odo_rot_err)
+        self.observations[ObservationKind.CAMERA_ODO_ROTATION] = rot_device
+      if cam_odo_trans_res is not None:
+        _, new_x, _, new_P, _, _, (cam_odo_trans_err,), _, _ = cam_odo_trans_res
+        self.observation_errors[ObservationKind.CAMERA_ODO_TRANSLATION] = np.array(cam_odo_trans_err)
+        self.observations[ObservationKind.CAMERA_ODO_TRANSLATION] = trans_device
+
+    if new_x is not None and new_P is not None:
+      self._finite_check(t, new_x, new_P)
+    return HandleLogResult.SUCCESS
+
+  def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_valid: bool):
+    state, cov = self.kf.x, self.kf.P
+    std = np.sqrt(np.diag(cov))
+
+    orientation_ned, orientation_ned_std = state[States.NED_ORIENTATION], std[States.NED_ORIENTATION]
+    velocity_device, velocity_device_std = state[States.DEVICE_VELOCITY], std[States.DEVICE_VELOCITY]
+    angular_velocity_device, angular_velocity_device_std = state[States.ANGULAR_VELOCITY], std[States.ANGULAR_VELOCITY]
+    acceleration_device, acceleration_device_std = state[States.ACCELERATION], std[States.ACCELERATION]
+
+    msg = messaging.new_message("deviceMotion")
+    msg.valid = filter_valid
+
+    deviceMotion = msg.deviceMotion
+    init_xyz_measurement(deviceMotion.orientationNED, orientation_ned, orientation_ned_std, filter_valid)
+    init_xyz_measurement(deviceMotion.velocityDevice, velocity_device, velocity_device_std, filter_valid)
+    init_xyz_measurement(deviceMotion.angularVelocityDevice, angular_velocity_device, angular_velocity_device_std, filter_valid)
+    init_xyz_measurement(deviceMotion.accelerationDevice, acceleration_device, acceleration_device_std, filter_valid)
+    if self.debug:
+      deviceMotion.debugFilterState.value = state.tolist()
+      deviceMotion.debugFilterState.std = std.tolist()
+      deviceMotion.debugFilterState.valid = filter_valid
+      deviceMotion.debugFilterState.observations = [
+        {'kind': k, 'value': self.observations[k].tolist(), 'error': self.observation_errors[k].tolist()}
+        for k in self.observations.keys()
+      ]
+
+    old_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST_HALF])
+    new_mean = np.mean(self.posenet_stds[POSENET_STD_HIST_HALF:])
+    std_spike = (new_mean / old_mean) > 4.0 and new_mean > 7.0
+
+    deviceMotion.inputsOK = inputs_valid
+    deviceMotion.posenetOK = not std_spike or self.car_speed <= 5.0
+    deviceMotion.sensorsOK = sensors_valid
+
+    return msg
+
+
+def sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, simulation):
+  cur_time = time.monotonic()
+  for which, msgs in [("accelerometer", acc_msgs), ("gyroscope", gyro_msgs)]:
+    if len(msgs) > 0:
+      sensor_valid[which] = msgs[-1].valid
+      sensor_recv_time[which] = cur_time
+
+    if not simulation:
+      sensor_alive[which] = (cur_time - sensor_recv_time[which]) < 0.1
+    else:
+      sensor_alive[which] = len(msgs) > 0
+
+  return all(sensor_alive.values()) and all(sensor_valid.values())
+
+
+def main():
+  config_realtime_process([0, 1, 2, 3], 5)
+
+  DEBUG = bool(int(os.getenv("DEBUG", "0")))
+  SIMULATION = bool(int(os.getenv("SIMULATION", "0")))
+
+  pm = messaging.PubMaster(['deviceMotion'])
+  sm = messaging.SubMaster(['carState', 'extrinsicsCalibration', 'cameraOdometry'], poll='cameraOdometry')
+  # separate sensor sockets for efficiency
+  sensor_sockets = [messaging.sub_sock(which, timeout=20) for which in ['accelerometer', 'gyroscope']]
+  sensor_alive, sensor_valid, sensor_recv_time = defaultdict(bool), defaultdict(bool), defaultdict(float)
+
+  params = Params()
+
+  estimator = LocationEstimator(DEBUG)
+
+  filter_initialized = False
+  critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"]
+  observation_input_invalid = defaultdict(int)
+
+  input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services}
+  input_invalid_threshold = {s: input_invalid_limit[s] - 0.5 for s in critcal_services}
+  input_invalid_decay = {s: calculate_invalid_input_decay(input_invalid_limit[s], INPUT_INVALID_RECOVERY, SERVICE_LIST[s].frequency) for s in critcal_services}
+
+  initial_pose_data = params.get("LocationFilterInitialState")
+  if initial_pose_data is not None:
+    with log.Event.from_bytes(initial_pose_data) as lp_msg:
+      filter_state = lp_msg.deviceMotion.debugFilterState
+      x_initial = np.array(filter_state.value, dtype=np.float64) if len(filter_state.value) != 0 else PoseKalman.initial_x
+      P_initial = np.diag(np.array(filter_state.std, dtype=np.float64)) if len(filter_state.std) != 0 else PoseKalman.initial_P
+      estimator.reset(None, x_initial, P_initial)
+
+  while True:
+    sm.update()
+
+    acc_msgs, gyro_msgs = (messaging.drain_sock(sock) for sock in sensor_sockets)
+
+    if filter_initialized:
+      msgs = []
+      for msg in acc_msgs + gyro_msgs:
+        t, valid, which, data = msg.logMonoTime, msg.valid, msg.which(), getattr(msg, msg.which())
+        msgs.append((t, valid, which, data))
+      for which, updated in sm.updated.items():
+        if not updated:
+          continue
+        t, valid, data = sm.logMonoTime[which], sm.valid[which], sm[which]
+        msgs.append((t, valid, which, data))
+
+      for log_mono_time, valid, which, msg in sorted(msgs, key=lambda x: x[0]):
+        if valid:
+          t = log_mono_time * 1e-9
+          res = estimator.handle_log(t, which, msg)
+          if which not in critcal_services:
+            continue
+
+          if res == HandleLogResult.TIMING_INVALID:
+            cloudlog.warning(f"Observation {which} ignored due to failed timing check")
+            observation_input_invalid[which] += 1
+          elif res == HandleLogResult.INPUT_INVALID:
+            cloudlog.warning(f"Observation {which} ignored due to failed sanity check")
+            observation_input_invalid[which] += 1
+          elif res == HandleLogResult.SUCCESS:
+            observation_input_invalid[which] *= input_invalid_decay[which]
+    else:
+      filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
+
+    if sm.updated["cameraOdometry"]:
+      critical_service_inputs_valid = all(observation_input_invalid[s] < input_invalid_threshold[s] for s in critcal_services)
+      inputs_valid = sm.all_valid() and critical_service_inputs_valid
+      sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
+
+      msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized)
+      pm.send("deviceMotion", msg)
+
+
+if __name__ == "__main__":
+  main()

iqpilot/selfdrive/locationd/models/.gitignore

@@ -0,0 +1 @@
+generated/

iqpilot/selfdrive/locationd/models/car_kf.py

@@ -0,0 +1,180 @@
+#!/usr/bin/env python3
+import math
+import sys
+from typing import Any
+
+import numpy as np
+
+from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from iqpilot.selfdrive.locationd.models.constants import ObservationKind
+from iqpilot.common.swaglog import cloudlog
+
+from rednose.helpers.kalmanfilter import KalmanFilter
+
+if __name__ == '__main__':  # Generating sympy
+  import sympy as sp
+  from rednose.helpers.ekf_sym import gen_code
+else:
+  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
+
+
+i = 0
+
+def _slice(n):
+  global i
+  s = slice(i, i + n)
+  i += n
+
+  return s
+
+
+class States:
+  # Vehicle model params
+  STIFFNESS = _slice(1)  # [-]
+  STEER_RATIO = _slice(1)  # [-]
+  ANGLE_OFFSET = _slice(1)  # [rad]
+  ANGLE_OFFSET_FAST = _slice(1)  # [rad]
+
+  VELOCITY = _slice(2)  # (x, y) [m/s]
+  YAW_RATE = _slice(1)  # [rad/s]
+  STEER_ANGLE = _slice(1)  # [rad]
+  ROAD_ROLL = _slice(1)  # [rad]
+
+
+class CarKalman(KalmanFilter):
+  name = 'car'
+
+  initial_x = np.array([
+    1.0,
+    15.0,
+    0.0,
+    0.0,
+
+    10.0, 0.0,
+    0.0,
+    0.0,
+    0.0
+  ])
+
+  # process noise
+  Q = np.diag([
+    (.05 / 100)**2,
+    .01**2,
+    math.radians(0.02)**2,
+    math.radians(0.25)**2,
+
+    .1**2, .01**2,
+    math.radians(0.1)**2,
+    math.radians(0.1)**2,
+    math.radians(1)**2,
+  ])
+  P_initial = Q.copy()
+
+  obs_noise: dict[int, Any] = {
+    ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
+    ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
+    ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
+    ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
+    ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
+    ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
+  }
+
+  global_vars = [
+    'mass',
+    'rotational_inertia',
+    'center_to_front',
+    'center_to_rear',
+    'stiffness_front',
+    'stiffness_rear',
+  ]
+
+  @staticmethod
+  def generate_code(generated_dir):
+    dim_state = CarKalman.initial_x.shape[0]
+    name = CarKalman.name
+
+    # Linearized single-track lateral dynamics, equations 7.211-7.213
+    # Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
+    # Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6
+
+    # globals
+    global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
+    m, j, aF, aR, cF_orig, cR_orig = global_vars
+
+    # make functions and jacobians with sympy
+    # state variables
+    state_sym = sp.MatrixSymbol('state', dim_state, 1)
+    state = sp.Matrix(state_sym)
+
+    # Vehicle model constants
+    sf = state[States.STIFFNESS, :][0, 0]
+
+    cF, cR = sf * cF_orig, sf * cR_orig
+    angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
+    angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
+    theta = state[States.ROAD_ROLL, :][0, 0]
+    sa = state[States.STEER_ANGLE, :][0, 0]
+
+    sR = state[States.STEER_RATIO, :][0, 0]
+    u, v = state[States.VELOCITY, :]
+    r = state[States.YAW_RATE, :][0, 0]
+
+    A = sp.Matrix(np.zeros((2, 2)))
+    A[0, 0] = -(cF + cR) / (m * u)
+    A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
+    A[1, 0] = -(cF * aF - cR * aR) / (j * u)
+    A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
+
+    B = sp.Matrix(np.zeros((2, 1)))
+    B[0, 0] = cF / m / sR
+    B[1, 0] = (cF * aF) / j / sR
+
+    C = sp.Matrix(np.zeros((2, 1)))
+    C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
+    C[1, 0] = 0
+
+    x = sp.Matrix([v, r])  # lateral velocity, yaw rate
+    x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
+
+    dt = sp.Symbol('dt')
+    state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+    state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
+    state_dot[States.YAW_RATE.start, 0] = x_dot[1]
+
+    # Basic descretization, 1st order integrator
+    # Can be pretty bad if dt is big
+    f_sym = state + dt * state_dot
+
+    #
+    # Observation functions
+    #
+    obs_eqs = [
+      [sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
+      [sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
+      [sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
+      [sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
+      [sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
+      [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
+      [sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
+      [sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
+    ]
+
+    gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
+
+  def __init__(self, generated_dir):
+    dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
+    self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
+                              dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
+
+  def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
+    self.filter.set_global("mass", mass)
+    self.filter.set_global("rotational_inertia", rotational_inertia)
+    self.filter.set_global("center_to_front", center_to_front)
+    self.filter.set_global("center_to_rear", center_to_rear)
+    self.filter.set_global("stiffness_front", stiffness_front)
+    self.filter.set_global("stiffness_rear", stiffness_rear)
+
+
+if __name__ == "__main__":
+  generated_dir = sys.argv[2]
+  CarKalman.generate_code(generated_dir)

iqpilot/selfdrive/locationd/models/constants.py

@@ -0,0 +1,92 @@
+import os
+
+GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
+
+class ObservationKind:
+  UNKNOWN = 0
+  NO_OBSERVATION = 1
+  GPS_NED = 2
+  ODOMETRIC_SPEED = 3
+  PHONE_GYRO = 4
+  GPS_VEL = 5
+  PSEUDORANGE_GPS = 6
+  PSEUDORANGE_RATE_GPS = 7
+  SPEED = 8
+  NO_ROT = 9
+  PHONE_ACCEL = 10
+  ORB_POINT = 11
+  ECEF_POS = 12
+  CAMERA_ODO_TRANSLATION = 13
+  CAMERA_ODO_ROTATION = 14
+  ORB_FEATURES = 15
+  MSCKF_TEST = 16
+  FEATURE_TRACK_TEST = 17
+  LANE_PT = 18
+  IMU_FRAME = 19
+  PSEUDORANGE_GLONASS = 20
+  PSEUDORANGE_RATE_GLONASS = 21
+  PSEUDORANGE = 22
+  PSEUDORANGE_RATE = 23
+  ECEF_VEL = 35
+  ECEF_ORIENTATION_FROM_GPS = 32
+  NO_ACCEL = 33
+  ORB_FEATURES_WIDE = 34
+
+  ROAD_FRAME_XY_SPEED = 24  # (x, y) [m/s]
+  ROAD_FRAME_YAW_RATE = 25  # [rad/s]
+  STEER_ANGLE = 26  # [rad]
+  ANGLE_OFFSET_FAST = 27  # [rad]
+  STIFFNESS = 28  # [-]
+  STEER_RATIO = 29  # [-]
+  ROAD_FRAME_X_SPEED = 30  # (x) [m/s]
+  ROAD_ROLL = 31  # [rad]
+
+  names = [
+    'Unknown',
+    'No observation',
+    'GPS NED',
+    'Odometric speed',
+    'Phone gyro',
+    'GPS velocity',
+    'GPS pseudorange',
+    'GPS pseudorange rate',
+    'Speed',
+    'No rotation',
+    'Phone acceleration',
+    'ORB point',
+    'ECEF pos',
+    'camera odometric translation',
+    'camera odometric rotation',
+    'ORB features',
+    'MSCKF test',
+    'Feature track test',
+    'Lane ecef point',
+    'imu frame eulers',
+    'GLONASS pseudorange',
+    'GLONASS pseudorange rate',
+    'pseudorange',
+    'pseudorange rate',
+
+    'Road Frame x,y speed',
+    'Road Frame yaw rate',
+    'Steer Angle',
+    'Fast Angle Offset',
+    'Stiffness',
+    'Steer Ratio',
+    'Road Frame x speed',
+    'Road Roll',
+    'ECEF orientation from GPS',
+    'NO accel',
+    'ORB features wide camera',
+    'ECEF_VEL',
+  ]
+
+  @classmethod
+  def to_string(cls, kind):
+    return cls.names[kind]
+
+
+SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
+           ObservationKind.PSEUDORANGE_RATE_GPS,
+           ObservationKind.PSEUDORANGE_GLONASS,
+           ObservationKind.PSEUDORANGE_RATE_GLONASS]

iqpilot/selfdrive/locationd/models/pose_kf.py

@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+
+import sys
+import numpy as np
+
+from iqpilot.selfdrive.locationd.models.constants import ObservationKind
+
+from rednose.helpers.kalmanfilter import KalmanFilter
+
+if __name__=="__main__":
+  import sympy as sp
+  from rednose.helpers.ekf_sym import gen_code
+  from rednose.helpers.sympy_helpers import euler_rotate, rot_to_euler
+else:
+  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
+
+EARTH_G = 9.81
+
+
+class States:
+  NED_ORIENTATION = slice(0, 3)  # roll, pitch, yaw in rad
+  DEVICE_VELOCITY = slice(3, 6)  # ned velocity in m/s
+  ANGULAR_VELOCITY = slice(6, 9)  # roll, pitch and yaw rates in rad/s
+  GYRO_BIAS = slice(9, 12)  # roll, pitch and yaw gyroscope biases in rad/s
+  ACCELERATION = slice(12, 15)  # acceleration in device frame in m/s**2
+  ACCEL_BIAS = slice(15, 18)  # Acceletometer bias in m/s**2
+
+
+class PoseKalman(KalmanFilter):
+  name = "pose"
+
+  # state
+  initial_x = np.array([0.0, 0.0, 0.0,
+                        0.0, 0.0, 0.0,
+                        0.0, 0.0, 0.0,
+                        0.0, 0.0, 0.0,
+                        0.0, 0.0, 0.0,
+                        0.0, 0.0, 0.0])
+  # state covariance
+  initial_P = np.diag([0.01**2, 0.01**2, 0.01**2,
+                       10**2, 10**2, 10**2,
+                       1**2, 1**2, 1**2,
+                       1**2, 1**2, 1**2,
+                       100**2, 100**2, 100**2,
+                       0.01**2, 0.01**2, 0.01**2])
+
+  # process noise
+  Q = np.diag([0.001**2, 0.001**2, 0.001**2,
+               0.01**2, 0.01**2, 0.01**2,
+               0.1**2, 0.1**2, 0.1**2,
+               (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
+               3**2, 3**2, 3**2,
+               0.005**2, 0.005**2, 0.005**2])
+
+  obs_noise = {ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]),
+               ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]),
+               ObservationKind.CAMERA_ODO_TRANSLATION: np.diag([0.5**2, 0.5**2, 0.5**2]),
+               ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2])}
+
+  @staticmethod
+  def generate_code(generated_dir):
+    name = PoseKalman.name
+    dim_state = PoseKalman.initial_x.shape[0]
+    dim_state_err = PoseKalman.initial_P.shape[0]
+
+    state_sym = sp.MatrixSymbol('state', dim_state, 1)
+    state = sp.Matrix(state_sym)
+    roll, pitch, yaw = state[States.NED_ORIENTATION, :]
+    velocity = state[States.DEVICE_VELOCITY, :]
+    angular_velocity = state[States.ANGULAR_VELOCITY, :]
+    vroll, vpitch, vyaw = angular_velocity
+    gyro_bias = state[States.GYRO_BIAS, :]
+    acceleration = state[States.ACCELERATION, :]
+    acc_bias = state[States.ACCEL_BIAS, :]
+
+    dt = sp.Symbol('dt')
+
+    ned_from_device = euler_rotate(roll, pitch, yaw)
+    device_from_ned = ned_from_device.T
+
+    state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+    state_dot[States.DEVICE_VELOCITY, :] = acceleration
+
+    f_sym = state + dt * state_dot
+    device_from_device_t1 = euler_rotate(dt*vroll, dt*vpitch, dt*vyaw)
+    ned_from_device_t1 = ned_from_device * device_from_device_t1
+    f_sym[States.NED_ORIENTATION, :] = rot_to_euler(ned_from_device_t1)
+
+    centripetal_acceleration = angular_velocity.cross(velocity)
+    gravity = sp.Matrix([0, 0, -EARTH_G])
+    h_gyro_sym = angular_velocity + gyro_bias
+    h_acc_sym = device_from_ned * gravity + acceleration + centripetal_acceleration + acc_bias
+    h_phone_rot_sym = angular_velocity
+    h_relative_motion_sym = velocity
+    obs_eqs = [
+      [h_gyro_sym, ObservationKind.PHONE_GYRO, None],
+      [h_acc_sym, ObservationKind.PHONE_ACCEL, None],
+      [h_relative_motion_sym, ObservationKind.CAMERA_ODO_TRANSLATION, None],
+      [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
+    ]
+    gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err)
+
+  def __init__(self, generated_dir, max_rewind_age):
+    dim_state, dim_state_err = PoseKalman.initial_x.shape[0], PoseKalman.initial_P.shape[0]
+    self.filter = EKF_sym_pyx(generated_dir, self.name, PoseKalman.Q, PoseKalman.initial_x, PoseKalman.initial_P,
+                              dim_state, dim_state_err, max_rewind_age=max_rewind_age)
+
+
+if __name__ == "__main__":
+  generated_dir = sys.argv[2]
+  PoseKalman.generate_code(generated_dir)

iqpilot/selfdrive/locationd/paramsd.py

@@ -0,0 +1,268 @@
+#!/usr/bin/env python3
+import numpy as np
+import capnp
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import car, log
+from iqpilot.common.issue_debug import log_issue_limited
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
+from iqpilot.selfdrive.locationd.models.constants import GENERATED_DIR
+from iqpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
+from iqpilot.common.swaglog import cloudlog
+
+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 = 16.0 # angle
+OFFSET_LOWERED_MAX = 14.0 # angle
+MIN_ACTIVE_SPEED = 1.0
+LOW_ACTIVE_SPEED = 10.0
+
+
+class VehicleParamsEstimator:
+  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 == 'deviceMotion':
+      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 == 'extrinsicsCalibration':
+      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)
+      self.kf.filter.reset_rewind()
+
+  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 vehicleParameters 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)
+
+    if not self.total_offset_valid:
+      log_issue_limited(
+        "paramsd_angle_offset_invalid",
+        "calibration",
+        f"paramsd angle offset invalid angleOffsetDeg={self.angle_offset:.2f} "
+        f"angleOffsetAverageDeg={self.avg_angle_offset:.2f} speed={self.observed_speed:.2f}",
+        interval_sec=2.0,
+      )
+
+    msg = messaging.new_message('vehicleParameters')
+
+    msg.valid = valid
+
+    vehicleParameters = msg.vehicleParameters
+    vehicleParameters.posenetValid = True
+    vehicleParameters.sensorValid = sensors_valid
+    vehicleParameters.steerRatio = float(x[States.STEER_RATIO].item())
+    vehicleParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
+    vehicleParameters.roll = float(self.roll)
+    vehicleParameters.angleOffsetAverageDeg = float(self.avg_angle_offset)
+    vehicleParameters.angleOffsetDeg = float(self.angle_offset)
+    vehicleParameters.steerRatioValid = self.min_sr <= vehicleParameters.steerRatio <= self.max_sr
+    vehicleParameters.stiffnessFactorValid = 0.2 <= vehicleParameters.stiffnessFactor <= 5.0
+    vehicleParameters.angleOffsetAverageValid = bool(self.avg_offset_valid)
+    vehicleParameters.angleOffsetValid = bool(self.total_offset_valid)
+    vehicleParameters.valid = all((
+      vehicleParameters.angleOffsetAverageValid,
+      vehicleParameters.angleOffsetValid ,
+      self.roll_valid,
+      roll_std < ROLL_STD_MAX,
+      vehicleParameters.stiffnessFactorValid,
+      vehicleParameters.steerRatioValid,
+    ))
+    vehicleParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
+    vehicleParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
+    vehicleParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
+    vehicleParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
+    if debug:
+      vehicleParameters.debugFilterState = log.VehicleParameters.FilterState.new_message()
+      vehicleParameters.debugFilterState.value = x.tolist()
+      vehicleParameters.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)
+def migrate_cached_vehicle_params_if_needed(params: Params):
+  last_parameters_data_old = params.get("LiveParameters")
+  last_parameters_data = params.get("LiveParametersV2")
+  if last_parameters_data_old is None or last_parameters_data is not None:
+    return
+
+  try:
+    last_parameters_msg = messaging.new_message('vehicleParameters')
+    last_parameters_msg.vehicleParameters.valid = True
+    last_parameters_msg.vehicleParameters.steerRatio = last_parameters_data_old['steerRatio']
+    last_parameters_msg.vehicleParameters.stiffnessFactor = last_parameters_data_old['stiffnessFactor']
+    last_parameters_msg.vehicleParameters.angleOffsetAverageDeg = last_parameters_data_old['angleOffsetAverageDeg']
+    params.put("LiveParametersV2", last_parameters_msg.to_bytes())
+  except Exception as e:
+    cloudlog.error(f"Failed to perform parameter migration: {e}")
+    params.remove("LiveParameters")
+
+
+def retrieve_initial_vehicle_params(params: Params, CP: car.CarParams, replay: bool, debug: bool):
+  last_parameters_data = params.get("LiveParametersV2")
+  last_carparams_data = params.get("CarParamsPrevRoute")
+
+  steer_ratio, stiffness_factor, angle_offset_deg, p_initial = CP.steerRatio, 1.0, 0.0, None
+
+  retrieve_success = False
+  if last_parameters_data is not None and last_carparams_data is not None:
+    try:
+      with log.Event.from_bytes(last_parameters_data) as last_lp_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP:
+        lp = last_lp_msg.vehicleParameters
+        # Check if car model matches
+        if last_CP.carFingerprint != CP.carFingerprint:
+          raise Exception("Car model mismatch")
+
+        # Check if starting values are sane
+        min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
+        steer_ratio_sane = min_sr <= lp.steerRatio <= max_sr
+        if not steer_ratio_sane:
+          raise Exception(f"Invalid starting values found {lp}")
+
+        initial_filter_std = np.array(lp.debugFilterState.std)
+        if debug and len(initial_filter_std) != 0:
+          p_initial = np.diag(initial_filter_std)
+
+        steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg
+        retrieve_success = True
+    except Exception as e:
+      cloudlog.error(f"Failed to retrieve initial values: {e}")
+      params.remove("LiveParametersV2")
+
+  if not replay:
+    # When driving in wet conditions the stiffness can go down, and then be too low on the next drive
+    # Without a way to detect this we have to reset the stiffness every drive
+    stiffness_factor = 1.0
+
+  if not retrieve_success:
+    cloudlog.info("Vehicle parameter estimator resetting to default values")
+
+  return steer_ratio, stiffness_factor, angle_offset_deg, p_initial

iqpilot/selfdrive/locationd/test/.gitignore

@@ -0,0 +1 @@
+out/

iqpilot/selfdrive/locationd/test/test_calibration_helpers.py

@@ -0,0 +1,61 @@
+import numpy as np
+import pytest
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import log
+
+from iqpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
+from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy, get_render_path_height
+from iqpilot.selfdrive.locationd.helpers import PoseCalibrator
+
+
+def build_live_calibration(status: log.ExtrinsicsCalibration.Status,
+                           rpy: tuple[float, float, float] = (0.0, 0.0, 0.0),
+                           height: tuple[float, ...] = (1.22,)):
+  msg = messaging.new_message("extrinsicsCalibration")
+  msg.extrinsicsCalibration.calStatus = status
+  msg.extrinsicsCalibration.rpyCalib = list(rpy)
+  msg.extrinsicsCalibration.height = list(height)
+  return msg.extrinsicsCalibration
+
+
+def test_get_calibrated_rpy_requires_calibrated_status():
+  live_calib = build_live_calibration(log.ExtrinsicsCalibration.Status.uncalibrated, rpy=(0.1, 0.2, 0.3))
+  assert get_calibrated_rpy(live_calib) is None
+
+  live_calib = build_live_calibration(log.ExtrinsicsCalibration.Status.calibrated, rpy=(0.1, 0.2, 0.3))
+  np.testing.assert_allclose(get_calibrated_rpy(live_calib), np.array([0.1, 0.2, 0.3], dtype=np.float32))
+
+
+def test_get_render_path_height_uses_default_until_calibrated():
+  live_calib = build_live_calibration(log.ExtrinsicsCalibration.Status.uncalibrated, height=(1.5,))
+  assert get_render_path_height(live_calib) == float(HEIGHT_INIT[0])
+
+  live_calib = build_live_calibration(log.ExtrinsicsCalibration.Status.calibrated, height=(1.5,))
+  assert get_render_path_height(live_calib) == 1.5
+
+
+def test_calibrated_values_are_used_even_when_identity_override_is_disabled():
+  live_calib = build_live_calibration(log.ExtrinsicsCalibration.Status.calibrated, rpy=(0.3, -0.2, 0.1), height=(1.6,))
+  np.testing.assert_allclose(get_calibrated_rpy(live_calib), np.array([0.3, -0.2, 0.1], dtype=np.float32))
+  assert get_render_path_height(live_calib) == pytest.approx(1.6)
+
+
+def test_pose_calibrator_holds_identity_until_calibrated():
+  calibrator = PoseCalibrator()
+
+  uncalibrated = build_live_calibration(log.ExtrinsicsCalibration.Status.uncalibrated, rpy=(0.1, 0.2, 0.3))
+  calibrator.feed_live_calib(uncalibrated)
+  np.testing.assert_allclose(calibrator.calib_from_device, np.eye(3))
+  assert not calibrator.calib_valid
+
+  calibrated = build_live_calibration(log.ExtrinsicsCalibration.Status.calibrated, rpy=(0.0, 0.1, 0.0))
+  calibrator.feed_live_calib(calibrated)
+  assert not np.allclose(calibrator.calib_from_device, np.eye(3))
+  assert calibrator.calib_valid
+
+  recalibrating = build_live_calibration(log.ExtrinsicsCalibration.Status.recalibrating, rpy=(0.2, 0.2, 0.2))
+  frozen_transform = calibrator.calib_from_device.copy()
+  calibrator.feed_live_calib(recalibrating)
+  np.testing.assert_allclose(calibrator.calib_from_device, frozen_transform)
+  assert not calibrator.calib_valid

iqpilot/selfdrive/locationd/test/test_calibrationd.py

@@ -0,0 +1,117 @@
+import random
+
+import numpy as np
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import log
+from iqpilot.common.params import Params
+from iqpilot.selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, \
+                                                         MAX_YAW_RATE_FILTER, SMOOTH_CYCLES, HEIGHT_INIT, MAX_ALLOWED_PITCH_SPREAD, MAX_ALLOWED_YAW_SPREAD
+
+
+def process_messages(c, cam_odo_calib, cycles,
+                     cam_odo_speed=MIN_SPEED_FILTER + 1,
+                     carstate_speed=MIN_SPEED_FILTER + 1,
+                     cam_odo_yr=0.0,
+                     cam_odo_speed_std=1e-3,
+                     cam_odo_height_std=1e-3):
+  old_rpy_weight_prev = 0.0
+  for _ in range(cycles):
+    assert (old_rpy_weight_prev - c.old_rpy_weight < 1/SMOOTH_CYCLES + 1e-3)
+    old_rpy_weight_prev = c.old_rpy_weight
+    c.handle_v_ego(carstate_speed)
+    c.handle_cam_odom([cam_odo_speed,
+                       np.sin(cam_odo_calib[2]) * cam_odo_speed,
+                       -np.sin(cam_odo_calib[1]) * cam_odo_speed],
+                        [0.0, 0.0, cam_odo_yr],
+                        [0.0, 0.0, 0.0],
+                        [cam_odo_speed_std, cam_odo_speed_std, cam_odo_speed_std],
+                        [0.0, 0.0, HEIGHT_INIT.item()],
+                        [cam_odo_height_std, cam_odo_height_std, cam_odo_height_std])
+
+class TestCalibrationd:
+  def test_read_saved_params(self):
+    msg = messaging.new_message('extrinsicsCalibration')
+    msg.extrinsicsCalibration.validBlocks = random.randint(1, 10)
+    msg.extrinsicsCalibration.rpyCalib = [0.0, 0.01, 0.01]
+    msg.extrinsicsCalibration.height = HEIGHT_INIT.tolist()
+    Params().put("CalibrationParams", msg.to_bytes())
+    c = Calibrator(param_put=True)
+
+    np.testing.assert_allclose(msg.extrinsicsCalibration.rpyCalib, c.rpy)
+    np.testing.assert_allclose(msg.extrinsicsCalibration.height, c.height)
+    assert msg.extrinsicsCalibration.validBlocks == c.valid_blocks
+
+
+  def test_calibration_basics(self):
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED)
+    assert c.valid_blocks == INPUTS_WANTED
+    np.testing.assert_allclose(c.rpy, np.zeros(3))
+    np.testing.assert_allclose(c.height, HEIGHT_INIT)
+    c.reset()
+
+
+  def test_calibration_low_speed_reject(self):
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, carstate_speed=MIN_SPEED_FILTER - 1)
+    assert c.valid_blocks == 0
+    np.testing.assert_allclose(c.rpy, np.zeros(3))
+    np.testing.assert_allclose(c.height, HEIGHT_INIT)
+
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED,
+                     cam_odo_speed=MIN_SPEED_FILTER - 10, carstate_speed=MIN_SPEED_FILTER + 5)
+    assert c.valid_blocks == INPUTS_WANTED
+    np.testing.assert_allclose(c.rpy[1:], np.zeros(2), atol=1e-6)
+    np.testing.assert_allclose(c.height, HEIGHT_INIT)
+
+
+  def test_calibration_yaw_rate_reject(self):
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_yr=MAX_YAW_RATE_FILTER)
+    assert c.valid_blocks == 0
+    np.testing.assert_allclose(c.rpy, np.zeros(3))
+    np.testing.assert_allclose(c.height, HEIGHT_INIT)
+
+
+  def test_calibration_speed_std_reject(self):
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed_std=1e3)
+    assert c.valid_blocks == 0
+    np.testing.assert_allclose(c.rpy, np.zeros(3))
+
+
+  def test_calibration_speed_std_height_reject(self):
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_height_std=1e3)
+    assert c.valid_blocks == INPUTS_WANTED
+    np.testing.assert_allclose(c.rpy, np.zeros(3))
+
+
+  def test_calibration_auto_reset(self):
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
+    assert c.valid_blocks == INPUTS_NEEDED
+    np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0], atol=1e-3)
+    process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*0.9, MAX_ALLOWED_YAW_SPREAD*0.9], BLOCK_SIZE + 10)
+    assert c.valid_blocks == INPUTS_NEEDED + 1
+    assert c.cal_status == log.ExtrinsicsCalibration.Status.calibrated
+
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
+    assert c.valid_blocks == INPUTS_NEEDED
+    np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
+    process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], BLOCK_SIZE + 10)
+    assert c.valid_blocks == 1
+    assert c.cal_status == log.ExtrinsicsCalibration.Status.recalibrating
+    np.testing.assert_allclose(c.rpy, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], atol=1e-2)
+
+    c = Calibrator(param_put=False)
+    process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
+    assert c.valid_blocks == INPUTS_NEEDED
+    np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
+    process_messages(c, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], BLOCK_SIZE + 10)
+    assert c.valid_blocks == 1
+    assert c.cal_status == log.ExtrinsicsCalibration.Status.recalibrating
+    np.testing.assert_allclose(c.rpy, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], atol=1e-2)

iqpilot/selfdrive/locationd/test/test_helpers.py

@@ -0,0 +1,43 @@
+import numpy as np
+import pytest
+
+from iqpilot.selfdrive.locationd.helpers import ParameterEstimator, PointBuckets, fft_next_good_size
+
+
+class ScalarBuckets(PointBuckets):
+  def add_point(self, x, y):
+    for bounds, bucket in self.buckets.items():
+      if bounds[0] <= x < bounds[1]:
+        bucket.append([x, y])
+        return
+    raise ValueError(x)
+
+
+def test_fft_next_good_size_small_input():
+  assert fft_next_good_size(6) == 6
+
+
+def test_point_buckets_base_requires_an_insertion_policy():
+  buckets = PointBuckets([(0, 1)], [1], 1, 2, 2)
+  with pytest.raises(NotImplementedError):
+    buckets.add_point(0.5, 1.0)
+
+
+def test_point_buckets_load_and_retrieve():
+  buckets = ScalarBuckets([(0, 1), (1, 2)], [1, 1], 2, 3, 2)
+  points = [[0.25, 10.0], [1.25, 20.0], [0.75, 30.0]]
+  buckets.load_points(points)
+  assert buckets.is_valid()
+  assert buckets.is_calculable()
+  np.testing.assert_allclose(buckets.get_points(), [[0.25, 10.0], [0.75, 30.0], [1.25, 20.0]])
+  assert buckets.get_points(2).shape == (2, 2)
+
+
+@pytest.mark.parametrize("method,args", [
+  ("reset", ()),
+  ("handle_log", (0, "carState", None)),
+  ("get_msg", (True, False)),
+])
+def test_parameter_estimator_requires_an_implementation(method, args):
+  with pytest.raises(NotImplementedError):
+    getattr(ParameterEstimator(), method)(*args)

iqpilot/selfdrive/locationd/test/test_lagd.py

@@ -0,0 +1,136 @@
+import random
+import numpy as np
+import time
+import pytest
+
+from iqpilot.cereal import messaging, log, car
+from iqpilot.selfdrive.locationd.lagd import LateralLagEstimator, retrieve_initial_lag, masked_normalized_cross_correlation, \
+                                               BLOCK_NUM_NEEDED, BLOCK_SIZE, MIN_OKAY_WINDOW_SEC, VERSION, MIN_LAG, MAX_LAG
+from iqpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams
+from iqpilot.selfdrive.locationd.test.test_locationd_scenarios import TEST_ROUTE
+from iqpilot.common.params import Params
+from iqpilot.tools.lib.logreader import LogReader
+
+MAX_ERR_FRAMES = 1
+DT = 0.05
+LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES = int(round(MIN_LAG / DT)), int(round(MAX_LAG / DT))
+
+
+def process_messages(estimator, lag_frames, n_frames, vego=25.0, rejection_threshold=0.0):
+  for i in range(n_frames):
+    t = i * estimator.dt
+    desired_la = np.cos(10 * t) * 0.3
+    actual_la = np.cos(10 * (t - lag_frames * estimator.dt)) * 0.3
+
+    # if sample is masked out, set it to desired value (no lag)
+    rejected = random.uniform(0, 1) < rejection_threshold
+    if rejected:
+      actual_la = desired_la
+
+    desired_cuvature = float(desired_la / (vego ** 2))
+    actual_yr = float(actual_la / vego)
+    msgs = [
+      (t, "carControl", car.CarControl(latActive=not rejected)),
+      (t, "carState", car.CarState(vEgo=vego, steeringPressed=False)),
+      (t, "controlsState", log.ControlsState(desiredCurvature=desired_cuvature)),
+      (t, "deviceMotion", log.DeviceMotion(angularVelocityDevice=log.DeviceMotion.XYZMeasurement(z=actual_yr, valid=True),
+                                   posenetOK=True, inputsOK=True)),
+      (t, "extrinsicsCalibration", log.ExtrinsicsCalibration(rpyCalib=[0, 0, 0], calStatus=log.ExtrinsicsCalibration.Status.calibrated)),
+    ]
+    for t, w, m in msgs:
+      estimator.handle_log(t, w, m)
+    estimator.update_points()
+    estimator.update_estimate()
+
+
+class TestLagd:
+  def test_read_saved_params(self):
+    params = Params()
+
+    lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
+    CP = next(m for m in lr if m.which() == "carParams").carParams
+
+    msg = messaging.new_message('lateralDelay')
+    msg.lateralDelay.lateralDelayEstimate = random.random()
+    msg.lateralDelay.validBlocks = random.randint(1, 10)
+    msg.lateralDelay.version = VERSION
+    params.put("LiveDelay", msg.to_bytes())
+    params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
+
+    saved_lag_params = retrieve_initial_lag(params, CP)
+    assert saved_lag_params is not None
+
+    lag, valid_blocks = saved_lag_params
+    assert lag == msg.lateralDelay.lateralDelayEstimate
+    assert valid_blocks == msg.lateralDelay.validBlocks
+
+  def test_ncc(self):
+    lag_frames = random.randint(1, 19)
+
+    desired_sig = np.sin(np.arange(0.0, 10.0, 0.1))
+    actual_sig = np.sin(np.arange(0.0, 10.0, 0.1) - lag_frames * 0.1)
+    mask = np.ones(len(desired_sig), dtype=bool)
+
+    corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20]
+    assert np.argmax(corr) == lag_frames
+
+    # add some noise
+    desired_sig += np.random.normal(0, 0.05, len(desired_sig))
+    actual_sig += np.random.normal(0, 0.05, len(actual_sig))
+    corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20]
+    assert np.argmax(corr)  in range(lag_frames - MAX_ERR_FRAMES, lag_frames + MAX_ERR_FRAMES + 1)
+
+    # mask out 40% of the values, and make them noise
+    mask = np.random.choice([True, False], size=len(desired_sig), p=[0.6, 0.4])
+    desired_sig[~mask] = np.random.normal(0, 1, size=np.sum(~mask))
+    actual_sig[~mask] = np.random.normal(0, 1, size=np.sum(~mask))
+    corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20]
+    assert np.argmax(corr) in range(lag_frames - MAX_ERR_FRAMES, lag_frames + MAX_ERR_FRAMES + 1)
+
+  def test_empty_estimator(self):
+    mocked_CP = car.CarParams(steerActuatorDelay=0.8)
+    estimator = LateralLagEstimator(mocked_CP, DT)
+    msg = estimator.get_msg(True)
+    assert msg.lateralDelay.status == 'unestimated'
+    assert np.allclose(msg.lateralDelay.lateralDelay, estimator.initial_lag)
+    assert np.allclose(msg.lateralDelay.lateralDelayEstimate, estimator.initial_lag)
+    assert msg.lateralDelay.validBlocks == 0
+    assert msg.lateralDelay.calPerc == 0
+
+  def test_estimator_basics(self, subtests):
+    for lag_frames in range(LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES - 1):
+      with subtests.test(msg=f"lag_frames={lag_frames}"):
+        mocked_CP = car.CarParams(steerActuatorDelay=0.8)
+        estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0)
+        process_messages(estimator, lag_frames, int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_NUM_NEEDED * BLOCK_SIZE)
+        msg = estimator.get_msg(True)
+        assert msg.lateralDelay.status == 'estimated'
+        assert np.allclose(msg.lateralDelay.lateralDelay, lag_frames * DT, atol=0.01)
+        assert np.allclose(msg.lateralDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01)
+        assert np.allclose(msg.lateralDelay.lateralDelayEstimateStd, 0.0, atol=0.01)
+        assert msg.lateralDelay.validBlocks == BLOCK_NUM_NEEDED
+        assert msg.lateralDelay.calPerc == 100
+
+  def test_estimator_masking(self):
+    mocked_CP, lag_frames = car.CarParams(steerActuatorDelay=0.8), random.randint(LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES - 1)
+    estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0, min_valid_block_count=1)
+    process_messages(estimator, lag_frames, (int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_SIZE) * 2, rejection_threshold=0.4)
+    msg = estimator.get_msg(True)
+    assert np.allclose(msg.lateralDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01)
+    assert np.allclose(msg.lateralDelay.lateralDelayEstimateStd, 0.0, atol=0.01)
+    assert msg.lateralDelay.calPerc == 100
+
+  @pytest.mark.timeout(60)
+  def test_estimator_performance(self):
+    mocked_CP = car.CarParams(steerActuatorDelay=0.8)
+    estimator = LateralLagEstimator(mocked_CP, DT)
+
+    ds = []
+    for _ in range(1000):
+      st = time.perf_counter()
+      estimator.update_points()
+      estimator.update_estimate()
+      d = time.perf_counter() - st
+      ds.append(d)
+
+    assert np.mean(ds) < DT

iqpilot/selfdrive/locationd/test/test_locationd_scenarios.py

@@ -0,0 +1,193 @@
+import numpy as np
+import pytest
+from collections import defaultdict
+from enum import Enum
+
+from iqpilot.tools.lib.logreader import LogReader
+from iqpilot.selfdrive.test.process_replay.migration import migrate_all
+from iqpilot.selfdrive.test.process_replay.process_replay import replay_process_with_name
+
+# TODO find a new segment to test
+TEST_ROUTE = "4019fff6e54cf1c7|00000123--4bc0d95ef6/5"
+GPS_MESSAGES = ['gpsLocationExternal', 'gpsLocation']
+SELECT_COMPARE_FIELDS = {
+  'yaw_rate': ['angularVelocityDevice', 'z'],
+  'roll': ['orientationNED', 'x'],
+  'inputs_flag': ['inputsOK'],
+  'sensors_flag': ['sensorsOK'],
+}
+JUNK_IDX = 100
+CONSISTENT_SPIKES_COUNT = 10
+
+
+class Scenario(Enum):
+  BASE = 'base'
+  GYRO_OFF = 'gyro_off'
+  GYRO_SPIKE_MIDWAY = 'gyro_spike_midway'
+  GYRO_CONSISTENT_SPIKES = 'gyro_consistent_spikes'
+  ACCEL_OFF = 'accel_off'
+  ACCEL_SPIKE_MIDWAY = 'accel_spike_midway'
+  ACCEL_CONSISTENT_SPIKES = 'accel_consistent_spikes'
+  SENSOR_TIMING_SPIKE_MIDWAY = 'timing_spikes'
+  SENSOR_TIMING_CONSISTENT_SPIKES = 'timing_consistent_spikes'
+
+
+def get_select_fields_data(logs):
+  def get_nested_keys(msg, keys):
+    val = None
+    for key in keys:
+      val = getattr(msg if val is None else val, key) if isinstance(key, str) else val[key]
+    return val
+  lp = [x.deviceMotion for x in logs if x.which() == 'deviceMotion']
+  data = defaultdict(list)
+  for msg in lp:
+    for key, fields in SELECT_COMPARE_FIELDS.items():
+      data[key].append(get_nested_keys(msg, fields))
+  for key in data:
+    data[key] = np.array(data[key][JUNK_IDX:], dtype=float)
+  return data
+
+
+def modify_logs_midway(logs, which, count, fn):
+  non_which = [x for x in logs if x.which() != which]
+  which = [x for x in logs if x.which() == which]
+  temps = which[len(which) // 2:len(which) // 2 + count]
+  for i, temp in enumerate(temps):
+    temp = temp.as_builder()
+    fn(temp)
+    which[len(which) // 2 + i] = temp.as_reader()
+  return sorted(non_which + which, key=lambda x: x.logMonoTime)
+
+
+def run_scenarios(scenario, logs):
+  if scenario == Scenario.BASE:
+    pass
+
+  elif scenario == Scenario.GYRO_OFF:
+    logs = sorted([x for x in logs if x.which() != 'gyroscope'], key=lambda x: x.logMonoTime)
+
+  elif scenario == Scenario.GYRO_SPIKE_MIDWAY or scenario == Scenario.GYRO_CONSISTENT_SPIKES:
+    def gyro_spike(msg):
+      msg.gyroscope.gyroUncalibrated.v[0] += 3.0
+    count = 1 if scenario == Scenario.GYRO_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT
+    logs = modify_logs_midway(logs, 'gyroscope', count, gyro_spike)
+
+  elif scenario == Scenario.ACCEL_OFF:
+    logs = sorted([x for x in logs if x.which() != 'accelerometer'], key=lambda x: x.logMonoTime)
+
+  elif scenario == Scenario.ACCEL_SPIKE_MIDWAY or scenario == Scenario.ACCEL_CONSISTENT_SPIKES:
+    def acc_spike(msg):
+      msg.accelerometer.acceleration.v[0] += 100.0
+    count = 1 if scenario == Scenario.ACCEL_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT
+    logs = modify_logs_midway(logs, 'accelerometer', count, acc_spike)
+
+  elif scenario == Scenario.SENSOR_TIMING_SPIKE_MIDWAY or scenario == Scenario.SENSOR_TIMING_CONSISTENT_SPIKES:
+    def timing_spike(msg):
+      msg.accelerometer.timestamp -= int(0.150 * 1e9)
+    count = 1 if scenario == Scenario.SENSOR_TIMING_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT
+    logs = modify_logs_midway(logs, 'accelerometer', count, timing_spike)
+
+  replayed_logs = replay_process_with_name(name='locationd', lr=logs)
+  return get_select_fields_data(logs), get_select_fields_data(replayed_logs)
+
+
+@pytest.mark.linux
+@pytest.mark.slow
+class TestLocationdScenarios:
+  """
+  Test locationd with different scenarios. In all these scenarios, we expect the following:
+    - locationd kalman filter should never go unstable (we care mostly about yaw_rate, roll, gpsOK, inputsOK, sensorsOK)
+    - faulty values should be ignored, with appropriate flags set
+  """
+
+  @classmethod
+  def setup_class(cls):
+    cls.logs = migrate_all(LogReader(TEST_ROUTE))
+
+  def test_base(self):
+    """
+    Test: unchanged log
+    Expected Result:
+      - yaw_rate: unchanged
+      - roll: unchanged
+    """
+    orig_data, replayed_data = run_scenarios(Scenario.BASE, self.logs)
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
+
+  def test_gyro_off(self):
+    """
+    Test: no gyroscope message for the entire segment
+    Expected Result:
+      - yaw_rate: 0
+      - roll: 0
+      - sensorsOK: False
+    """
+    _, replayed_data = run_scenarios(Scenario.GYRO_OFF, self.logs)
+    assert np.allclose(replayed_data['yaw_rate'], 0.0)
+    assert np.allclose(replayed_data['roll'], 0.0)
+    assert np.all(replayed_data['sensors_flag'] == 0.0)
+
+  def test_gyro_spike(self):
+    """
+    Test: a gyroscope spike in the middle of the segment
+    Expected Result:
+      - yaw_rate: unchanged
+      - roll: unchanged
+      - inputsOK: False for some time after the spike, True for the rest
+    """
+    orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs)
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
+    assert np.all(replayed_data['inputs_flag'] == orig_data['inputs_flag'])
+    assert np.all(replayed_data['sensors_flag'] == orig_data['sensors_flag'])
+
+  def test_consistent_gyro_spikes(self):
+    """
+    Test: consistent timing spikes for N gyroscope messages in the middle of the segment
+    Expected Result: inputsOK becomes False after N of bad measurements
+    """
+    orig_data, replayed_data = run_scenarios(Scenario.GYRO_CONSISTENT_SPIKES, self.logs)
+    assert np.diff(replayed_data['inputs_flag'])[501] == -1.0
+    assert np.diff(replayed_data['inputs_flag'])[708] == 1.0
+
+  def test_accel_off(self):
+    """
+    Test: no accelerometer message for the entire segment
+    Expected Result:
+      - yaw_rate: 0
+      - roll: 0
+      - sensorsOK: False
+    """
+    _, replayed_data = run_scenarios(Scenario.ACCEL_OFF, self.logs)
+    assert np.allclose(replayed_data['yaw_rate'], 0.0)
+    assert np.allclose(replayed_data['roll'], 0.0)
+    assert np.all(replayed_data['sensors_flag'] == 0.0)
+
+  def test_accel_spike(self):
+    """
+    ToDo:
+    Test: an accelerometer spike in the middle of the segment
+    Expected Result: Right now, the kalman filter is not robust to small spikes like it is to gyroscope spikes.
+    """
+    orig_data, replayed_data = run_scenarios(Scenario.ACCEL_SPIKE_MIDWAY, self.logs)
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
+
+  def test_single_timing_spike(self):
+    """
+    Test: timing of 150ms off for the single accelerometer message in the middle of the segment
+    Expected Result: the message is ignored, and inputsOK is False for that time
+    """
+    orig_data, replayed_data = run_scenarios(Scenario.SENSOR_TIMING_SPIKE_MIDWAY, self.logs)
+    assert np.all(replayed_data['inputs_flag'] == orig_data['inputs_flag'])
+    assert np.all(replayed_data['sensors_flag'] == orig_data['sensors_flag'])
+
+  def test_consistent_timing_spikes(self):
+    """
+    Test: consistent timing spikes for N accelerometer messages in the middle of the segment
+    Expected Result: inputsOK becomes False after N of bad measurements
+    """
+    orig_data, replayed_data = run_scenarios(Scenario.SENSOR_TIMING_CONSISTENT_SPIKES, self.logs)
+    assert np.diff(replayed_data['inputs_flag'])[501] == -1.0
+    assert np.diff(replayed_data['inputs_flag'])[707] == 1.0

iqpilot/selfdrive/locationd/test/test_paramsd.py

@@ -0,0 +1,71 @@
+import random
+import numpy as np
+
+from iqpilot.cereal import messaging
+from iqpilot.selfdrive.locationd.paramsd import retrieve_initial_vehicle_params, migrate_cached_vehicle_params_if_needed
+from iqpilot.selfdrive.locationd.models.car_kf import CarKalman
+from iqpilot.selfdrive.locationd.test.test_locationd_scenarios import TEST_ROUTE
+from iqpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams
+from iqpilot.common.params import Params
+from iqpilot.tools.lib.logreader import LogReader
+
+
+def get_random_live_parameters(CP):
+  msg = messaging.new_message("vehicleParameters")
+  msg.vehicleParameters.steerRatio = (random.random() + 0.5) * CP.steerRatio
+  msg.vehicleParameters.stiffnessFactor = random.random()
+  msg.vehicleParameters.angleOffsetAverageDeg = random.random()
+  msg.vehicleParameters.debugFilterState.std = [random.random() for _ in range(CarKalman.P_initial.shape[0])]
+  return msg
+
+
+class TestParamsd:
+  def test_read_saved_params(self):
+    params = Params()
+
+    lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
+    CP = next(m for m in lr if m.which() == "carParams").carParams
+
+    msg = get_random_live_parameters(CP)
+    params.put("LiveParametersV2", msg.to_bytes())
+    params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
+
+    migrate_cached_vehicle_params_if_needed(params) # this is not tested here but should not mess anything up or throw an error
+    sr, sf, offset, p_init = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True)
+    np.testing.assert_allclose(sr, msg.vehicleParameters.steerRatio)
+    np.testing.assert_allclose(sf, msg.vehicleParameters.stiffnessFactor)
+    np.testing.assert_allclose(offset, msg.vehicleParameters.angleOffsetAverageDeg)
+    np.testing.assert_equal(p_init.shape, CarKalman.P_initial.shape)
+    np.testing.assert_allclose(np.diagonal(p_init), msg.vehicleParameters.debugFilterState.std)
+
+  # TODO Remove this test after the support for old format is removed
+  def test_read_saved_params_old_format(self):
+    params = Params()
+
+    lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
+    CP = next(m for m in lr if m.which() == "carParams").carParams
+
+    msg = get_random_live_parameters(CP)
+    params.put("LiveParameters", {
+      "steerRatio": msg.vehicleParameters.steerRatio,
+      "stiffnessFactor": msg.vehicleParameters.stiffnessFactor,
+      "angleOffsetAverageDeg": msg.vehicleParameters.angleOffsetAverageDeg,
+    })
+    params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
+    params.remove("LiveParametersV2")
+
+    migrate_cached_vehicle_params_if_needed(params)
+    sr, sf, offset, _ = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True)
+    np.testing.assert_allclose(sr, msg.vehicleParameters.steerRatio)
+    np.testing.assert_allclose(sf, msg.vehicleParameters.stiffnessFactor)
+    np.testing.assert_allclose(offset, msg.vehicleParameters.angleOffsetAverageDeg)
+    assert params.get("LiveParametersV2") is not None
+
+  def test_read_saved_params_corrupted_old_format(self):
+    params = Params()
+    params.put("LiveParameters", {})
+    params.remove("LiveParametersV2")
+
+    migrate_cached_vehicle_params_if_needed(params)
+    assert params.get("LiveParameters") is None
+    assert params.get("LiveParametersV2") is None

iqpilot/selfdrive/locationd/test/test_torqued.py

@@ -0,0 +1,25 @@
+from iqpilot.cereal import car
+from iqpilot.selfdrive.locationd.torqued import TorqueEstimator
+
+
+def test_cal_percent():
+  est = TorqueEstimator(car.CarParams())
+  msg = est.get_msg()
+  assert msg.lateralTorqueParameters.calPerc == 0
+
+  for (low, high), min_pts in zip(est.filtered_points.buckets.keys(),
+                                  est.filtered_points.buckets_min_points.values(), strict=True):
+    for _ in range(int(min_pts)):
+      est.filtered_points.add_point((low + high) / 2.0, 0.0)
+
+  # enough bucket points, but not enough total points
+  msg = est.get_msg()
+  assert msg.lateralTorqueParameters.calPerc == (len(est.filtered_points) / est.min_points_total * 100 + 100) / 2
+
+  # add enough points to bucket with most capacity
+  key = list(est.filtered_points.buckets)[0]
+  for _ in range(est.min_points_total - len(est.filtered_points)):
+    est.filtered_points.add_point((key[0] + key[1]) / 2.0, 0.0)
+
+  msg = est.get_msg()
+  assert msg.lateralTorqueParameters.calPerc == 100

iqpilot/selfdrive/locationd/torqued.py

@@ -0,0 +1,240 @@
+#!/usr/bin/env python3
+import numpy as np
+from collections import deque, defaultdict
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import car, log
+from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.common.filter_simple import FirstOrderFilter
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose
+
+HISTORY = 5  # secs
+POINTS_PER_BUCKET = 1500
+MIN_POINTS_TOTAL = 4000
+MIN_POINTS_TOTAL_QLOG = 600
+FIT_POINTS_TOTAL = 2000
+FIT_POINTS_TOTAL_QLOG = 600
+MIN_VEL = 15  # m/s
+FRICTION_FACTOR = 1.5  # ~85% of data coverage
+FACTOR_SANITY = 0.3
+FACTOR_SANITY_QLOG = 0.5
+FRICTION_SANITY = 0.5
+FRICTION_SANITY_QLOG = 0.8
+STEER_MIN_THRESHOLD = 0.02
+MIN_FILTER_DECAY = 50
+MAX_FILTER_DECAY = 250
+LAT_ACC_THRESHOLD = 1
+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])
+MIN_ENGAGE_BUFFER = 2  # secs
+
+VERSION = 1  # bump this to invalidate old parameter caches
+
+
+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]])
+
+
+class TorqueBuckets(PointBuckets):
+  def add_point(self, x, y):
+    for bound_min, bound_max in self.x_bounds:
+      if (x >= bound_min) and (x < bound_max):
+        self.buckets[(bound_min, bound_max)].append([x, 1.0, y])
+        break
+
+
+class TorqueEstimator(ParameterEstimator):
+  def __init__(self, CP, decimated=False, track_all_points=False):
+    ParameterEstimator.__init__(self)
+    self.CP = CP
+    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:
+      self.min_bucket_points = MIN_BUCKET_POINTS / 10
+      self.min_points_total = MIN_POINTS_TOTAL_QLOG
+      self.fit_points = FIT_POINTS_TOTAL_QLOG
+      self.factor_sanity = FACTOR_SANITY_QLOG
+      self.friction_sanity = FRICTION_SANITY_QLOG
+
+    else:
+      self.min_bucket_points = MIN_BUCKET_POINTS
+      self.min_points_total = MIN_POINTS_TOTAL
+      self.fit_points = FIT_POINTS_TOTAL
+      self.factor_sanity = FACTOR_SANITY
+      self.friction_sanity = FRICTION_SANITY
+
+    self.offline_friction = 0.0
+    self.offline_latAccelFactor = 0.0
+    self.resets = 0.0
+    self.use_params = CP.lateralTuning.which() == 'torque'
+
+    if CP.lateralTuning.which() == 'torque':
+      self.offline_friction = CP.lateralTuning.torque.friction
+      self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor
+
+    self.calibrator = PoseCalibrator()
+
+    self.reset()
+
+    initial_params = {
+      'latAccelFactor': self.offline_latAccelFactor,
+      'latAccelOffset': 0.0,
+      'frictionCoefficient': self.offline_friction,
+      '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_friction = (1.0 - self.friction_sanity) * self.offline_friction
+    self.max_friction = (1.0 + self.friction_sanity) * self.offline_friction
+
+    # try to restore cached params
+    params = Params()
+    params_cache = params.get("CarParamsPrevRoute")
+    torque_cache = params.get("LiveTorqueParameters")
+    if params_cache is not None and torque_cache is not None:
+      try:
+        with log.Event.from_bytes(torque_cache) as log_evt:
+          cache_ltp = log_evt.lateralTorqueParameters
+        with car.CarParams.from_bytes(params_cache) as msg:
+          cache_CP = msg
+        if self.get_restore_key(cache_CP, cache_ltp.version) == self.get_restore_key(CP, VERSION):
+          if cache_ltp.valid:
+            initial_params = {
+              'latAccelFactor': cache_ltp.latAccelFactorFiltered,
+              'latAccelOffset': cache_ltp.latAccelOffsetFiltered,
+              'frictionCoefficient': cache_ltp.frictionCoefficientFiltered
+            }
+          initial_params['points'] = cache_ltp.points
+          self.decay = cache_ltp.decay
+          self.filtered_points.load_points(initial_params['points'])
+          cloudlog.info("restored torque params from cache")
+      except Exception:
+        cloudlog.exception("failed to restore cached torque params")
+        params.remove("LiveTorqueParameters")
+
+    self.filtered_params = {}
+    for param in initial_params:
+      self.filtered_params[param] = FirstOrderFilter(initial_params[param], self.decay, DT_MDL)
+
+  @staticmethod
+  def get_restore_key(CP, version):
+    a, b = 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)
+
+  def reset(self):
+    self.resets += 1.0
+    self.decay = MIN_FILTER_DECAY
+    self.raw_points = defaultdict(lambda: deque(maxlen=self.hist_len))
+    self.filtered_points = TorqueBuckets(x_bounds=STEER_BUCKET_BOUNDS,
+                                         min_points=self.min_bucket_points,
+                                         min_points_total=self.min_points_total,
+                                         points_per_bucket=POINTS_PER_BUCKET,
+                                         rowsize=3)
+    self.all_torque_points = []
+
+  def estimate_params(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 update_params(self, params):
+    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)
+
+  def handle_log(self, t, which, msg):
+    if which == "carControl":
+      self.raw_points["carControl_t"].append(t + self.lag)
+      self.raw_points["lat_active"].append(msg.latActive)
+    elif which == "carOutput":
+      self.raw_points["carOutput_t"].append(t + self.lag)
+      self.raw_points["steer_torque"].append(-msg.actuatorsOutput.torque)
+    elif which == "carState":
+      self.raw_points["carState_t"].append(t + self.lag)
+      # TODO: check if high aEgo affects resulting lateral accel
+      self.raw_points["vego"].append(msg.vEgo)
+      self.raw_points["steer_override"].append(msg.steeringPressed)
+    elif which == "extrinsicsCalibration":
+      self.calibrator.feed_live_calib(msg)
+    elif which == "lateralDelay":
+      self.lag = msg.lateralDelay
+    # calculate lateral accel from past steering torque
+    elif which == "deviceMotion":
+      if len(self.raw_points['steer_torque']) == self.hist_len:
+        device_pose = Pose.from_live_pose(msg)
+        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_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_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()
+        if all(lat_active) and not any(steer_override) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD):
+          if abs(lateral_acc) <= LAT_ACC_THRESHOLD:
+            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('lateralTorqueParameters')
+    msg.valid = valid
+    lateralTorqueParameters = msg.lateralTorqueParameters
+    lateralTorqueParameters.version = VERSION
+    lateralTorqueParameters.useParams = self.use_params
+
+    # 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()
+      lateralTorqueParameters.latAccelFactorRaw = float(latAccelFactor)
+      lateralTorqueParameters.latAccelOffsetRaw = float(latAccelOffset)
+      lateralTorqueParameters.frictionCoefficientRaw = float(frictionCoeff)
+
+      if self.filtered_points.is_valid():
+        if any(val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff]):
+          cloudlog.exception("Live torque parameters are invalid.")
+          lateralTorqueParameters.valid = False
+          self.reset()
+        else:
+          lateralTorqueParameters.valid = 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})
+
+    if with_points:
+      lateralTorqueParameters.points = self.filtered_points.get_points()[:, [0, 2]].tolist()
+
+    lateralTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x)
+    lateralTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x)
+    lateralTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x)
+    lateralTorqueParameters.totalBucketPoints = len(self.filtered_points)
+    lateralTorqueParameters.calPerc = self.filtered_points.get_valid_percent()
+    lateralTorqueParameters.decay = self.decay
+    lateralTorqueParameters.maxResets = self.resets
+    return msg