Locationd 100 Hz (#20816)

* fix std transform

* 100Hz

* new ref

* no more decimation

* clean up confusing maths

* static typing

* Revert "static typing"

This reverts commit 23d87337de648e629fbd35dd8c04a740bbefca47.

* 100Hz costs more

* move normalization into core

* add quat idxs

* add big eps

* this is not safe in the filter

* more sensible

* updates to rednose

* not tested

* normalize in python too

* update rednose

* nan check

* check for infs too

* all should be finite

* update ref

* rednose pr now in master

Co-authored-by: Harald Schafer <harald.the.engineer@gmail.com>

rednose_repo

@@ -1 +1 @@
-Subproject commit f069de89f6ccd277a557fb14590f8cb0fb069a8a
+Subproject commit 7fff192e234cd91f50deed12adb3616d96f12d3e

selfdrive/locationd/locationd.cc

@@ -30,6 +30,17 @@ 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();
@@ -64,11 +75,12 @@ 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);
-  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();
+  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_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 = ((this->calib_from_device * vel_angular_err) * this->calib_from_device.transpose()).diagonal().array().sqrt();
+  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();
@@ -88,7 +100,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 = ((this->calib_from_device * vel_device_cov) * this->calib_from_device.transpose()).diagonal().array().sqrt();
+  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);
   //orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
@@ -170,12 +182,9 @@ 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) {
-      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) {
@@ -185,13 +194,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->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]) });
     }
   }
 }
-}
 
 void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log) {
   // ignore the message if the fix is invalid
@@ -239,37 +245,34 @@ 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->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->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) {
-  this->cam_counter++;
-
-  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() });
-
   VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
-    VectorXd trans_device_std = this->device_from_calib * floatlist2vector(log.getTransStd());
+
+  VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
+  VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
 
   this->posenet_stds.pop_front();
-    this->posenet_stds.push_back(trans_device_std[0]);
+  this->posenet_stds.push_back(trans_calib_std[0]);
 
-    trans_device_std *= 10.0;
+  // 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);
+
+  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() });
 }
-}
 
 void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {
   if (log.getRpyCalib().size() > 0) {
@@ -285,6 +288,14 @@ void Localizer::reset_kalman(double current_time) {
   this->reset_kalman(current_time, init_x.segment<4>(3), init_x.head(3));
 }
 
+void Localizer::finite_check(double current_time) {
+  bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all();
+  if (!all_finite){
+    LOGE("Non-finite values detected, kalman reset");
+    this->reset_kalman(current_time);
+  }
+}
+
 void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos) {
   // too nonlinear to init on completely wrong
   VectorXd init_x = this->kf->get_initial_x();
@@ -293,11 +304,6 @@ 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) {
@@ -322,6 +328,7 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
   } else if (log.isLiveCalibration()) {
     this->handle_live_calib(t, log.getLiveCalibration());
   }
+  this->finite_check();
 }
 
 kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,

selfdrive/locationd/locationd.h

@@ -17,9 +17,6 @@
 
 #include "models/live_kf.h"
 
-#define VISION_DECIMATION 2
-#define SENSOR_DECIMATION 10
-
 #define POSENET_STD_HIST_HALF 20
 
 class Localizer {
@@ -30,6 +27,7 @@ public:
 
   void reset_kalman(double current_time = NAN);
   void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos);
+  void finite_check(double current_time = NAN);
 
   kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
     bool inputsOK, bool sensorsOK, bool gpsOK);
@@ -61,9 +59,4 @@ 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/models/live_kf.cc

@@ -39,8 +39,9 @@ LiveKalman::LiveKalman() {
   }
 
   // init filter
-  this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x), get_mapmat(initial_P),
-    this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(), std::vector<std::string>(), 0.2);
+  this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
+    get_mapmat(initial_P),  this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
+    std::vector<int>{3}, std::vector<std::string>(), 0.2);
 }
 
 void LiveKalman::init_state(VectorXd& state, VectorXd& covs_diag, double filter_time) {
@@ -92,7 +93,6 @@ std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, std:
     r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
     break;
   }
-  this->filter->normalize_state(STATE_ECEF_ORIENTATION_START, STATE_ECEF_ORIENTATION_END);
   return r;
 }
 

selfdrive/locationd/models/live_kf.py

@@ -55,8 +55,8 @@ class LiveKalman():
 
   # state covariance
   initial_P_diag = np.array([1e16, 1e16, 1e16,
-                             1e6, 1e6, 1e6,
-                             1e4, 1e4, 1e4,
+                             10**2, 10**2, 10**2,
+                             10**2, 10**2, 10**2,
                              1**2, 1**2, 1**2,
                              0.05**2, 0.05**2, 0.05**2,
                              0.02**2,

selfdrive/locationd/models/loc_kf.py

@@ -382,9 +382,11 @@ class LocKalman():
       self.computer = LstSqComputer(generated_dir, N)
       self.max_tracks = max_tracks
 
+    self.quaternion_idxs = [3,] + [(self.dim_main + i*self.dim_augment + 3)for i in range(self.N)]
+
     # init filter
     self.filter = EKF_sym(generated_dir, name, Q, x_initial, P_initial, self.dim_main, self.dim_main_err,
-                          N, self.dim_augment, self.dim_augment_err, self.maha_test_kinds)
+                          N, self.dim_augment, self.dim_augment_err, self.maha_test_kinds, self.quaternion_idxs)
 
   @property
   def x(self):
@@ -453,11 +455,6 @@ class LocKalman():
     # Should not continue if the quats behave this weirdly
     if not 0.1 < quat_norm < 10:
       raise RuntimeError("Sir! The filter's gone all wobbly!")
-    self.filter.normalize_state(3, 7)
-    for i in range(self.N):
-      d1 = self.dim_main
-      d3 = self.dim_augment
-      self.filter.normalize_state(d1 + d3 * i + 3, d1 + d3 * i + 7)
     return r
 
   def get_R(self, kind, n):

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 = 10
-VISION_DECIMATION = 2
+SENSOR_DECIMATION = 1
+VISION_DECIMATION = 1
 
 LIBLOCATIOND_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), '../liblocationd.so'))
 

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-f1515b6efca6de7305d411355f286d2c6b9fb19b
+fcfadaaad7f452fdbb69c1057058327b0d2004bf

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": 3.5,
+  "./locationd": 9.1,
   "selfdrive.controls.plannerd": 20.0,
   "./_ui": 15.0,
   "selfdrive.locationd.paramsd": 9.1,