      }
  
      // Gyro Uncalibrated
      if (sensor_reading.getSensor() == SENSOR_GYRO_UNCALIBRATED && sensor_reading.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
        auto v = sensor_reading.getGyroUncalibrated().getV();
        this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { Vector3d(-v[2], -v[1], -v[0]) });
      }
  
      // Accelerometer
      if (sensor_reading.getSensor() == SENSOR_ACCELEROMETER && sensor_reading.getType() == SENSOR_TYPE_ACCELEROMETER) {
        auto v = sensor_reading.getAcceleration().getV();
 
</s> remove       this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { Vector3d(-v[2], -v[1], -v[0]) });
 </s> add       this->acc_counter++;
      if (this->acc_counter % SENSOR_DECIMATION == 0) {
        this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { Vector3d(-v[2], -v[1], -v[0]) });
      } </s> remove   this->kf->predict_and_observe(current_time, OBSERVATION_ODOMETRIC_SPEED, { (VectorXd(1) << log.getVEgoRaw()).finished() });
  this->car_speed = std::abs(log.getVEgo());
  if (this->car_speed < 1e-3) {
    this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
 </s> add   this->speed_counter++;

  if (this->speed_counter % SENSOR_DECIMATION == 0) {
    this->kf->predict_and_observe(current_time, OBSERVATION_ODOMETRIC_SPEED, { (VectorXd(1) << log.getVEgoRaw()).finished() });
    this->car_speed = std::abs(log.getVEgo());
    if (this->car_speed < 1e-3) {
      this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
    } </s> remove   VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
  VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
 </s> add   if (this->cam_counter % VISION_DECIMATION == 0) {
    VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
    VectorXd rot_device_std = (this->device_from_calib * floatlist2vector(log.getRotStd())) * 10.0;
    this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
      { (VectorXd(rot_device.rows() + rot_device_std.rows()) << rot_device, rot_device_std).finished() }); </s> remove   this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
    { (VectorXd(rot_device.rows() + rot_device_std.rows()) << rot_device, rot_device_std).finished() });
  this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
    { (VectorXd(trans_device.rows() + trans_device_std.rows()) << trans_device, trans_device_std).finished() });
 </s> add     trans_device_std *= 10.0;
    this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
      { (VectorXd(trans_device.rows() + trans_device_std.rows()) << trans_device, trans_device_std).finished() });
  } </s> remove 
static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) {
  // To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix
  return ((rot_matrix *  cov_in) * rot_matrix.transpose());
}

static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) {
  // Stds cannot be rotated like values, only covariances can be rotated
  return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt();
}

 </s> add 
      this->gyro_counter++;
      if (this->gyro_counter % SENSOR_DECIMATION == 0) {
        auto v = sensor_reading.getGyroUncalibrated().getV();
        this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { Vector3d(-v[2], -v[1], -v[0]) });
      }
      }
  
      // Gyro Uncalibrated
      if (sensor_reading.getSensor() == SENSOR_GYRO_UNCALIBRATED && sensor_reading.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
      this->gyro_counter++;
      if (this->gyro_counter % SENSOR_DECIMATION == 0) {
        auto v = sensor_reading.getGyroUncalibrated().getV();
        this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { Vector3d(-v[2], -v[1], -v[0]) });
      }
      this->gyro_counter++;
      if (this->gyro_counter % SENSOR_DECIMATION == 0) {
        auto v = sensor_reading.getGyroUncalibrated().getV();
        this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { Vector3d(-v[2], -v[1], -v[0]) });
      }
      }
  
      // Accelerometer
      if (sensor_reading.getSensor() == SENSOR_ACCELEROMETER && sensor_reading.getType() == SENSOR_TYPE_ACCELEROMETER) {
        auto v = sensor_reading.getAcceleration().getV();
keep keep keep keep replace replace keep keep keep keep keep
 Revert "Locationd 100hz (#20759)" (#20797)

* Revert "Locationd 100hz (#20759)"

This reverts commit 333313cf0e6819767d8dcc4a55bea70f0354579f.

* new ref 
selfdrive/locationd/locationd.cc