Revert "Locationd 100hz (#20759)" (#20797)

* Revert "Locationd 100hz (#20759)"

This reverts commit 333313cf0e.

* new ref

selfdrive/locationd/locationd.cc

@@ -27,17 +27,6 @@ static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder me
   meas.setValid(valid);
 }
 
-
-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();
-}
-
 Localizer::Localizer() {
   this->kf = std::make_unique<LiveKalman>();
   this->reset_kalman();
@@ -72,12 +61,11 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
   VectorXd calibrated_orientation_ecef = rot2euler(this->calib_from_device * device_from_ecef);
 
   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 = ((this->calib_from_device * predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START)) * this->calib_from_device.transpose()).diagonal().array().sqrt();
-  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);
+  MatrixXdr vel_angular_err = 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 ang_vel_calib_std = ((this->calib_from_device * vel_angular_err) * this->calib_from_device.transpose()).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();
@@ -97,7 +85,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
   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 vel_calib_std = ((this->calib_from_device * vel_device_cov) * this->calib_from_device.transpose()).diagonal().array().sqrt();
 
   VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
   //orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
@@ -179,8 +167,11 @@ void Localizer::handle_sensors(double current_time, const capnp::List<cereal::Se
 
     // Gyro Uncalibrated
     if (sensor_reading.getSensor() == SENSOR_GYRO_UNCALIBRATED && sensor_reading.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
-      auto v = sensor_reading.getGyroUncalibrated().getV();
+      this->gyro_counter++;
-      this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { Vector3d(-v[2], -v[1], -v[0]) });
+      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
@@ -191,7 +182,10 @@ void Localizer::handle_sensors(double current_time, const capnp::List<cereal::Se
       // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
       this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
 
-      this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { Vector3d(-v[2], -v[1], -v[0]) });
+      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]) });
+      }
     }
   }
 }
@@ -242,33 +236,36 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
 }
 
 void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) {
-  this->kf->predict_and_observe(current_time, OBSERVATION_ODOMETRIC_SPEED, { (VectorXd(1) << log.getVEgoRaw()).finished() });
+  this->speed_counter++;
-  this->car_speed = std::abs(log.getVEgo());
+
-  if (this->car_speed < 1e-3) {
+  if (this->speed_counter % SENSOR_DECIMATION == 0) {
-    this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.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) });
+    }
   }
 }
 
 void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) {
-  VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
+  this->cam_counter++;
-  VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
 
-  VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
+  if (this->cam_counter % VISION_DECIMATION == 0) {
-  VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
+    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() });
 
-  this->posenet_stds.pop_front();
+    VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
-  this->posenet_stds.push_back(trans_calib_std[0]);
+    VectorXd trans_device_std = this->device_from_calib * floatlist2vector(log.getTransStd());
 
-  // Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise
+    this->posenet_stds.pop_front();
-  trans_calib_std *= 10.0;
+    this->posenet_stds.push_back(trans_device_std[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);
 
-  this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
+    trans_device_std *= 10.0;
-    { (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,
-  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() });
-    { (VectorXd(trans_device.rows() + trans_device_std.rows()) << trans_device, trans_device_std).finished() });
+  }
 }
 
 void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {
@@ -293,6 +290,11 @@ void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd
   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) {

selfdrive/locationd/locationd.h

@@ -17,6 +17,9 @@
 
 #include "models/live_kf.h"
 
+#define VISION_DECIMATION 2
+#define SENSOR_DECIMATION 10
+
 #define POSENET_STD_HIST_HALF 20
 
 class Localizer {
@@ -58,4 +61,9 @@ private:
   int64_t unix_timestamp_millis = 0;
   double last_gps_fix = 0;
   bool device_fell = false;
+
+  int gyro_counter = 0;
+  int acc_counter = 0;
+  int speed_counter = 0;
+  int cam_counter = 0;
 };

selfdrive/locationd/test/test_locationd.py

@@ -12,8 +12,8 @@ from common.params import Params
 
 from selfdrive.manager.process_config import managed_processes
 
-SENSOR_DECIMATION = 1
+SENSOR_DECIMATION = 10
-VISION_DECIMATION = 1
+VISION_DECIMATION = 2
 
 LIBLOCATIOND_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), '../liblocationd.so'))
 

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-8bc705247a6f316da0a74d28d393101e8dcfcf57
+e53498342bd6ba53e59976d284fc635368370e3d

selfdrive/test/test_onroad.py

@@ -20,7 +20,7 @@ from tools.lib.logreader import LogReader
 PROCS = {
   "selfdrive.controls.controlsd": 50.0,
   "./loggerd": 45.0,
-  "./locationd": 9.1,
+  "./locationd": 3.5,
   "selfdrive.controls.plannerd": 20.0,
   "./_ui": 15.0,
   "selfdrive.locationd.paramsd": 9.1,