  # Baseline CPU usage by process
  PROCS = {
    "selfdrive.controls.controlsd": 50.0,
    "./loggerd": 45.0,
    "./locationd": 9.1,
    "selfdrive.controls.plannerd": 20.0,
    "./_ui": 15.0,
    "selfdrive.locationd.paramsd": 9.1,
    "./camerad": 7.07,
    "./_sensord": 6.17,
 
</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   // Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise
  trans_calib_std *= 10.0;
  rot_calib_std *= 10.0;
  VectorXd rot_device_std = rotate_std(this->device_from_calib, rot_calib_std);
  VectorXd trans_device_std = rotate_std(this->device_from_calib, trans_calib_std);
 </s> add     this->posenet_stds.pop_front();
    this->posenet_stds.push_back(trans_device_std[0]); </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  </s> remove   this->posenet_stds.pop_front();
  this->posenet_stds.push_back(trans_calib_std[0]);
 </s> add     VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
    VectorXd trans_device_std = this->device_from_calib * floatlist2vector(log.getTransStd()); </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) });
    }
  "./locationd": 3.5,
  # Baseline CPU usage by process
  PROCS = {
    "selfdrive.controls.controlsd": 50.0,
    "./loggerd": 45.0,
  "./locationd": 3.5,
    "selfdrive.controls.plannerd": 20.0,
    "./_ui": 15.0,
    "selfdrive.locationd.paramsd": 9.1,
    "./camerad": 7.07,
    "./_sensord": 6.17,
keep keep keep keep replace keep keep keep keep keep
 Revert "Locationd 100hz (#20759)" (#20797)

* Revert "Locationd 100hz (#20759)"

This reverts commit 333313cf0e6819767d8dcc4a55bea70f0354579f.

* new ref 
selfdrive/test/test_onroad.py