IQ.Pilot Release Commit @ bec7652

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