Deprecate liveLocationKalman (#33405)

* Remove usages of llk

* Deprecate it

* Add scenarios back

* Fix orientationNED valid

* Regenerate

* Increase tolerances and update segment

* Remove calibration

cereal/log.capnp

@@ -2331,7 +2331,6 @@ struct Event {
     onroadEvents @68: List(Car.CarEvent);
     carParams @69: Car.CarParams;
     driverMonitoringState @71: DriverMonitoringState;
-    liveLocationKalman @72 :LiveLocationKalman;
     livePose @129 :LivePose;
     modelV2 @75 :ModelDataV2;
     drivingModelData @128 :DrivingModelData;
@@ -2404,7 +2403,7 @@ struct Event {
     model @9 :Legacy.ModelData; # TODO: rename modelV2 and mark this as deprecated
     liveMpcDEPRECATED @36 :LiveMpcData;
     liveLongitudinalMpcDEPRECATED @37 :LiveLongitudinalMpcData;
-    liveLocationKalmanDEPRECATED @51 :Legacy.LiveLocationData;
+    liveLocationKalmanLegacyDEPRECATED @51 :Legacy.LiveLocationData;
     orbslamCorrectionDEPRECATED @45 :Legacy.OrbslamCorrection;
     liveUIDEPRECATED @14 :Legacy.LiveUI;
     sensorEventDEPRECATED @4 :SensorEventData;
@@ -2441,5 +2440,6 @@ struct Event {
     lateralPlanDEPRECATED @64 :LateralPlan;
     navModelDEPRECATED @104 :NavModelData;
     uiPlanDEPRECATED @106 :UiPlan;
+    liveLocationKalmanDEPRECATED @72 :LiveLocationKalman;
   }
 }

cereal/services.py

@@ -48,7 +48,6 @@ _services: dict[str, tuple] = {
   "clocks": (True, 0.1, 1),
   "ubloxRaw": (True, 20.),
   "livePose": (True, 20., 4),
-  "liveLocationKalman": (True, 20.),
   "liveParameters": (True, 20., 5),
   "cameraOdometry": (True, 20., 10),
   "thumbnail": (True, 0.2, 1),

selfdrive/locationd/locationd.cc

@@ -17,9 +17,6 @@ const double TRANS_SANITY_CHECK = 200.0;  // m/s
 const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg)
 const double ALTITUDE_SANITY_CHECK = 10000; // m
 const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
-const double VALID_TIME_SINCE_RESET = 1.0; // s
-const double VALID_POS_STD = 50.0; // m
-const double MAX_RESET_TRACKER = 5.0;
 const double SANE_GPS_UNCERTAINTY = 1500.0; // m
 const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker
 const double RESET_TRACKER_DECAY = 0.99995;
@@ -68,13 +65,7 @@ static Quaterniond vector2quat(const VectorXd& vec) {
   return Quaterniond(vec(0), vec(1), vec(2), vec(3));
 }
 
-static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) {
-  meas.setValue(kj::arrayPtr(val.data(), val.size()));
-  meas.setStd(kj::arrayPtr(std.data(), std.size()));
-  meas.setValid(valid);
-}
-
-static void init_xyz_measurement(cereal::LivePose::XYZMeasurement::Builder meas, const VectorXf& val, const VectorXf& std, bool valid) {
+static void init_xyz_measurement(cereal::LivePose::XYZMeasurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) {
   meas.setX(val[0]); meas.setY(val[1]); meas.setZ(val[2]);
   meas.setXStd(std[0]); meas.setYStd(std[1]); meas.setZStd(std[2]);
   meas.setValid(valid);
@@ -107,31 +98,7 @@ Localizer::Localizer(LocalizerGnssSource gnss_source) {
   this->configure_gnss_source(gnss_source);
 }
 
-void Localizer::build_live_pose(cereal::LivePose::Builder& livePose, cereal::LiveLocationKalman::Reader& liveLocation) {
-  // Just copy the values from liveLocation to livePose for now
-  VectorXf orientation_ned = float32list2vector(liveLocation.getOrientationNED().getValue()), orientation_ned_std = float32list2vector(liveLocation.getOrientationNED().getStd());
-  init_xyz_measurement(livePose.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode);
-  VectorXf velocity_device = float32list2vector(liveLocation.getVelocityDevice().getValue()), velocity_device_std = float32list2vector(liveLocation.getVelocityDevice().getStd());
-  init_xyz_measurement(livePose.initVelocityDevice(), velocity_device, velocity_device_std, true);
-  VectorXf acceleration_device = float32list2vector(liveLocation.getAccelerationDevice().getValue()), acceleration_device_std = float32list2vector(liveLocation.getAccelerationDevice().getStd());
-  init_xyz_measurement(livePose.initAccelerationDevice(), acceleration_device, acceleration_device_std, true);
-  VectorXf ang_velocity_device = float32list2vector(liveLocation.getAngularVelocityDevice().getValue()), ang_velocity_device_std = float32list2vector(liveLocation.getAngularVelocityDevice().getStd());
-  init_xyz_measurement(livePose.initAngularVelocityDevice(), ang_velocity_device, ang_velocity_device_std, true);
-
-  if (DEBUG) {
-    VectorXd filter_state = float64list2vector(liveLocation.getFilterState().getValue()), filter_state_std = float64list2vector(liveLocation.getFilterState().getStd());
-    cereal::LivePose::FilterState::Builder filter_state_builder = livePose.initFilterState();
-    filter_state_builder.setValue(kj::arrayPtr(filter_state.data(), filter_state.size()));
-    filter_state_builder.setStd(kj::arrayPtr(filter_state_std.data(), filter_state_std.size()));
-    filter_state_builder.setValid(true);
-  }
-
-  livePose.setInputsOK(liveLocation.getInputsOK());
-  livePose.setPosenetOK(liveLocation.getPosenetOK());
-  livePose.setSensorsOK(liveLocation.getSensorsOK());
-}
-
-void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
+void Localizer::build_live_pose(cereal::LivePose::Builder& fix) {
   VectorXd predicted_state = this->kf->get_x();
   MatrixXdr predicted_cov = this->kf->get_P();
   VectorXd predicted_std = predicted_cov.diagonal().array().sqrt();
@@ -141,20 +108,12 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
   VectorXd fix_ecef_std = predicted_std.segment<STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START);
   VectorXd vel_ecef = predicted_state.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
   VectorXd vel_ecef_std = predicted_std.segment<STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START);
-  VectorXd fix_pos_geo_vec = this->get_position_geodetic();
   VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
   VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
   MatrixXdr orientation_ecef_cov = predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
   MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose();
-  VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose());
-
-  VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
-  MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START);
-  VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt();
-  VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
 
   MatrixXdr vel_angular_cov = predicted_cov.block<STATE_ANGULAR_VELOCITY_ERR_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START);
-  VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt();
 
   VectorXd vel_device = device_from_ecef * vel_ecef;
   VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose();
@@ -173,12 +132,8 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
   MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose();
   VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt();
 
-  VectorXd vel_calib = this->calib_from_device * vel_device;
-  VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt();
-
   VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
   VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt();
-  VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
   VectorXd nextfix_ecef = fix_ecef + vel_ecef;
   VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
 
@@ -188,26 +143,16 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
   VectorXd angVelocityDevice = predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
   VectorXd angVelocityDeviceErr = predicted_std.segment<STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START);
 
-  Vector3d nans = Vector3d(NAN, NAN, NAN);
-
-  // TODO fill in NED and Calibrated stds
   // write measurements to msg
-  init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode);
-  init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode);
-  init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode);
-  init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode);
-  init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
-  init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
-  init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode);
-  init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
-  init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode);
-  init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
-  init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
-  init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
-  init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
-  init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated);
+  init_xyz_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
+  init_xyz_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
+  init_xyz_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode);
+  init_xyz_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
   if (DEBUG) {
-    init_measurement(fix.initFilterState(), predicted_state, predicted_std, true);
+    cereal::LivePose::FilterState::Builder filter_state_builder = fix.initFilterState();
+    filter_state_builder.setValue(kj::arrayPtr(predicted_state.data(), predicted_state.size()));
+    filter_state_builder.setStd(kj::arrayPtr(predicted_std.data(), predicted_std.size()));
+    filter_state_builder.setValid(true);
   }
 
   double old_mean = 0.0, new_mean = 0.0;
@@ -224,26 +169,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
   new_mean /= POSENET_STD_HIST_HALF;
   // experimentally found these values, no false positives in 20k minutes of driving
   bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0);
-
   fix.setPosenetOK(!(std_spike && this->car_speed > 5.0));
-  fix.setDeviceStable(!this->device_fell);
-  fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER);
-  fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff);
-  this->device_fell = false;
-
-  //fix.setGpsWeek(this->time.week);
-  //fix.setGpsTimeOfWeek(this->time.tow);
-  fix.setUnixTimestampMillis(this->unix_timestamp_millis);
-
-  double time_since_reset = this->kf->get_filter_time() - this->last_reset_time;
-  fix.setTimeSinceReset(time_since_reset);
-  if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) {
-    fix.setStatus(cereal::LiveLocationKalman::Status::VALID);
-  } else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) {
-    fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED);
-  } else {
-    fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED);
-  }
 }
 
 VectorXd Localizer::get_position_geodetic() {
@@ -651,26 +577,12 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
   this->update_reset_tracker();
 }
 
-void Localizer::build_location_message(
-  MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid) {
-  cereal::Event::Builder evt = msg_builder.initEvent();
-  evt.setValid(msgValid);
-  cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
-  this->build_live_location(liveLoc);
-  liveLoc.setSensorsOK(sensorsOK);
-  liveLoc.setGpsOK(gpsOK);
-  liveLoc.setInputsOK(inputsOK);
-}
-
 void Localizer::build_pose_message(
-  MessageBuilder& msg_builder, MessageBuilder& location_msg_builder, bool inputsOK, bool sensorsOK, bool msgValid) {
+  MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool msgValid) {
   cereal::Event::Builder evt = msg_builder.initEvent();
   evt.setValid(msgValid);
   cereal::LivePose::Builder livePose = evt.initLivePose();
-
-  cereal::LiveLocationKalman::Reader location_msg = location_msg_builder.getRoot<cereal::Event>().getLiveLocationKalman().asReader();
-  this->build_live_pose(livePose, location_msg);
-
+  this->build_live_pose(livePose);
   livePose.setSensorsOK(sensorsOK);
   livePose.setInputsOK(inputsOK);
 }
@@ -744,7 +656,7 @@ int Localizer::locationd_thread() {
                                                           "carState", "accelerometer", "gyroscope"};
 
   SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
-  PubMaster pm({"liveLocationKalman", "livePose"});
+  PubMaster pm({"livePose"});
 
   uint64_t cnt = 0;
   bool filterInitialized = false;
@@ -778,12 +690,8 @@ int Localizer::locationd_thread() {
         this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time);
       }
 
-      MessageBuilder location_msg_builder, pose_msg_builder;
-      this->build_location_message(location_msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
-      this->build_pose_message(pose_msg_builder, location_msg_builder, inputsOK, sensorsOK, filterInitialized);
-
-      kj::ArrayPtr<capnp::byte> location_bytes = location_msg_builder.toBytes();
-      pm.send("liveLocationKalman", location_bytes.begin(), location_bytes.size());
+      MessageBuilder pose_msg_builder;
+      this->build_pose_message(pose_msg_builder, inputsOK, sensorsOK, filterInitialized);
 
       kj::ArrayPtr<capnp::byte> pose_bytes = pose_msg_builder.toBytes();
       pm.send("livePose", pose_bytes.begin(), pose_bytes.size());

selfdrive/locationd/locationd.h

@@ -45,12 +45,9 @@ public:
   bool are_inputs_ok();
   void observation_timings_invalid_reset();
 
-  void build_location_message(
-    MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
   void build_pose_message(
-  MessageBuilder& msg_builder, MessageBuilder& location_msg_builder, bool inputsOK, bool sensorsOK, bool msgValid);
-  void build_live_location(cereal::LiveLocationKalman::Builder& fix);
-  void build_live_pose(cereal::LivePose::Builder& livePose, cereal::LiveLocationKalman::Reader& liveLocation);
+    MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool msgValid);
+  void build_live_pose(cereal::LivePose::Builder& fix);
 
   Eigen::VectorXd get_position_geodetic();
   Eigen::VectorXd get_state();
@@ -83,7 +80,6 @@ private:
 
   int64_t unix_timestamp_millis = 0;
   double reset_tracker = 0.0;
-  bool device_fell = false;
   bool gps_mode = false;
   double first_valid_log_time = NAN;
   double ttff = NAN;

selfdrive/locationd/test/test_locationd_scenarios.py

@@ -7,12 +7,12 @@ from openpilot.tools.lib.logreader import LogReader
 from openpilot.selfdrive.test.process_replay.migration import migrate_all
 from openpilot.selfdrive.test.process_replay.process_replay import replay_process_with_name
 
-TEST_ROUTE = "ff2bd20623fcaeaa|2023-09-05--10-14-54/4"
+# TODO find a new segment to test
+TEST_ROUTE = "4019fff6e54cf1c7|00000123--4bc0d95ef6/5"
 GPS_MESSAGES = ['gpsLocationExternal', 'gpsLocation']
 SELECT_COMPARE_FIELDS = {
-  'yaw_rate': ['angularVelocityCalibrated', 'value', 2],
-  'roll': ['orientationNED', 'value', 0],
-  'gps_flag': ['gpsOK'],
+  'yaw_rate': ['angularVelocityDevice', 'z'],
+  'roll': ['orientationNED', 'x'],
   'inputs_flag': ['inputsOK'],
   'sensors_flag': ['sensorsOK'],
 }
@@ -37,9 +37,9 @@ def get_select_fields_data(logs):
     for key in keys:
       val = getattr(msg if val is None else val, key) if isinstance(key, str) else val[key]
     return val
-  llk = [x.liveLocationKalman for x in logs if x.which() == 'liveLocationKalman']
+  lp = [x.livePose for x in logs if x.which() == 'livePose']
   data = defaultdict(list)
-  for msg in llk:
+  for msg in lp:
     for key, fields in SELECT_COMPARE_FIELDS.items():
       data[key].append(get_nested_keys(msg, fields))
   for key in data:
@@ -116,8 +116,8 @@ class TestLocationdScenarios:
       - 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.2))
-    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
 
   def test_gps_off(self):
     """
@@ -128,9 +128,8 @@ class TestLocationdScenarios:
       - gpsOK: False
     """
     orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF, self.logs)
-    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))
-    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))
-    assert np.all(replayed_data['gps_flag'] == 0.0)
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
 
   def test_gps_off_midway(self):
     """
@@ -138,12 +137,10 @@ class TestLocationdScenarios:
     Expected Result:
       - yaw_rate: unchanged
       - roll:
-      - gpsOK: True for the first half, False for the second half
     """
     orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF_MIDWAY, self.logs)
-    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))
-    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))
-    assert np.diff(replayed_data['gps_flag'])[512] == -1.0
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
 
   def test_gps_on_midway(self):
     """
@@ -151,12 +148,10 @@ class TestLocationdScenarios:
     Expected Result:
       - yaw_rate: unchanged
       - roll:
-      - gpsOK: False for the first half, True for the second half
     """
     orig_data, replayed_data = run_scenarios(Scenario.GPS_ON_MIDWAY, self.logs)
-    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
     assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(1.5))
-    assert np.diff(replayed_data['gps_flag'])[505] == 1.0
 
   def test_gps_tunnel(self):
     """
@@ -164,13 +159,10 @@ class TestLocationdScenarios:
     Expected Result:
       - yaw_rate: unchanged
       - roll:
-      - gpsOK: False for the middle section, True for the rest
     """
     orig_data, replayed_data = run_scenarios(Scenario.GPS_TUNNEL, self.logs)
-    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))
-    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))
-    assert np.diff(replayed_data['gps_flag'])[213] == -1.0
-    assert np.diff(replayed_data['gps_flag'])[805] == 1.0
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
 
   def test_gyro_off(self):
     """
@@ -194,10 +186,10 @@ class TestLocationdScenarios:
       - 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.2))
-    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))
-    assert np.diff(replayed_data['inputs_flag'])[500] == -1.0
-    assert np.diff(replayed_data['inputs_flag'])[694] == 1.0
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
+    assert np.diff(replayed_data['inputs_flag'])[499] == -1.0
+    assert np.diff(replayed_data['inputs_flag'])[696] == 1.0
 
   def test_accel_off(self):
     """
@@ -219,5 +211,5 @@ class TestLocationdScenarios:
     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.2))
-    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))
+    assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))

selfdrive/test/process_replay/process_replay.py

@@ -547,7 +547,7 @@ CONFIGS = [
       "cameraOdometry", "accelerometer", "gyroscope", "gpsLocationExternal",
       "liveCalibration", "carState", "gpsLocation"
     ],
-    subs=["liveLocationKalman", "livePose"],
+    subs=["livePose"],
     ignore=["logMonoTime"],
     config_callback=locationd_config_pubsub_callback,
     tolerance=NUMPY_TOLERANCE,

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-fa4965a27dee4449ad8b255f9f7674d69327b6f7
+51a1a4716fdd6d58fc69c586aaa62c5c25a3d93e

selfdrive/test/process_replay/test_processes.py

@@ -40,23 +40,23 @@ source_segments = [
 ]
 
 segments = [
-  ("BODY", "regen34ECCE11CA1|2024-07-29--22-55-10--0"),
-  ("HYUNDAI", "regenC713CE6FA82|2024-07-29--22-56-31--0"),
-  ("HYUNDAI2", "regenD81F3A374A7|2024-07-29--22-58-45--0"),
-  ("TOYOTA", "regenE6D76723DC2|2024-07-29--23-00-08--0"),
-  ("TOYOTA2", "regen198859A572C|2024-07-29--23-01-31--0"),
-  ("TOYOTA3", "regenDF1EB621A66|2024-07-29--23-03-49--0"),
-  ("HONDA", "regen0FE7C4758B5|2024-07-29--23-05-14--0"),
-  ("HONDA2", "regen510A1F60E60|2024-07-29--23-06-39--0"),
-  ("CHRYSLER", "regenDACF082E83B|2024-07-29--23-08-01--0"),
-  ("RAM", "regen8BFB7E62F52|2024-07-29--23-10-14--0"),
-  ("SUBARU", "regen4EE2D45369E|2024-07-29--23-12-31--0"),
-  ("GM", "regenB38D92E6A4D|2024-07-29--23-13-54--0"),
-  ("GM2", "regenC5488470F1A|2024-07-29--23-16-09--0"),
-  ("NISSAN", "regenE5400EB4689|2024-07-29--23-17-30--0"),
-  ("VOLKSWAGEN", "regenD0B5635A8B9|2024-07-29--23-18-54--0"),
-  ("MAZDA", "regen57F8511F082|2024-07-29--23-21-09--0"),
-  ("FORD", "regen5708620AA2E|2024-07-29--23-23-20--0"),
+  ("BODY", "regenA67A128BCD8|2024-08-30--02-36-22--0"),
+  ("HYUNDAI", "regen9CBD921E93E|2024-08-30--02-38-51--0"),
+  ("HYUNDAI2", "regen12E0C4EA1A7|2024-08-30--02-42-40--0"),
+  ("TOYOTA", "regen1CA7A48E6F7|2024-08-30--02-45-08--0"),
+  ("TOYOTA2", "regen6E484EDAB96|2024-08-30--02-47-37--0"),
+  ("TOYOTA3", "regen4CE950B0267|2024-08-30--02-51-30--0"),
+  ("HONDA", "regenC8F0D6ADC5C|2024-08-30--02-54-01--0"),
+  ("HONDA2", "regen4B38A7428CD|2024-08-30--02-56-31--0"),
+  ("CHRYSLER", "regenF3DBBA9E8DF|2024-08-30--02-59-03--0"),
+  ("RAM", "regenDB02684E00A|2024-08-30--03-02-54--0"),
+  ("SUBARU", "regenAA1FF48CF1F|2024-08-30--03-06-45--0"),
+  ("GM", "regen720F2BA4CF6|2024-08-30--03-09-15--0"),
+  ("GM2", "regen9ADBECBCD1C|2024-08-30--03-13-04--0"),
+  ("NISSAN", "regen58464878D07|2024-08-30--03-15-31--0"),
+  ("VOLKSWAGEN", "regenED976DEB757|2024-08-30--03-18-02--0"),
+  ("MAZDA", "regenACF84CCF482|2024-08-30--03-21-55--0"),
+  ("FORD", "regen6ECC59A6307|2024-08-30--03-25-42--0"),
 ]
 
 # dashcamOnly makes don't need to be tested until a full port is done
@@ -86,7 +86,7 @@ def run_test_process(data):
 
 def get_log_data(segment):
   r, n = segment.rsplit("--", 1)
-  with FileReader(get_url(r, n, "rlog.bz2")) as f:
+  with FileReader(get_url(r, n, "rlog.zst")) as f:
     return (segment, f.read())
 
 
@@ -199,7 +199,7 @@ if __name__ == "__main__":
 
         cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{cur_commit}.zst")
         if args.update_refs:  # reference logs will not exist if routes were just regenerated
-          ref_log_path = get_url(*segment.rsplit("--", 1,), "rlog.bz2")
+          ref_log_path = get_url(*segment.rsplit("--", 1,), "rlog.zst")
         else:
           ref_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{ref_commit}.zst")
           ref_log_path = ref_log_fn if os.path.exists(ref_log_fn) else BASE_URL + os.path.basename(ref_log_fn)

selfdrive/test/test_onroad.py

@@ -91,7 +91,7 @@ TIMINGS = {
   "driverCameraState": [2.5, 0.35],
   "modelV2": [2.5, 0.35],
   "driverStateV2": [2.5, 0.40],
-  "liveLocationKalman": [2.5, 0.35],
+  "livePose": [2.5, 0.35],
   "wideRoadCameraState": [1.5, 0.35],
 }