    init_x.segment<4>(3) = init_orient;
    init_x.head(3) = init_pos;
  
    this->kf->init_state(init_x, init_P, current_time);
  }
  
  void Localizer::handle_msg_bytes(const char *data, const size_t size) {
    AlignedBuffer aligned_buf;
  
    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size));
 
</s> remove   VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
  VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
 </s> add   this->cam_counter++; </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   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   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       auto v = sensor_reading.getGyroUncalibrated().getV();
      this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { Vector3d(-v[2], -v[1], -v[0]) });
 </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]) });
      }
  this->gyro_counter = 0;
  this->acc_counter = 0;
  this->speed_counter = 0;
  this->cam_counter = 0;
    init_x.segment<4>(3) = init_orient;
    init_x.head(3) = init_pos;
  
    this->kf->init_state(init_x, init_P, current_time);
  this->gyro_counter = 0;
  this->acc_counter = 0;
  this->speed_counter = 0;
  this->cam_counter = 0;
  }
  
  void Localizer::handle_msg_bytes(const char *data, const size_t size) {
    AlignedBuffer aligned_buf;
  
    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size));
keep keep keep add keep keep keep keep keep keep
 Revert "Locationd 100hz (#20759)" (#20797)

* Revert "Locationd 100hz (#20759)"

This reverts commit 333313cf0e6819767d8dcc4a55bea70f0354579f.

* new ref 
selfdrive/locationd/locationd.cc