locationd: Fix Nav localization reliability (#22959)

* modify reset logic

* remove debug statements

* use ecef pos and vel covariances during reset

* reset orientations initialized to 0,0,GPSbearing

* refactor nav fix

* add fake gps observations to control ecef pos and ecef vel std

* replace fake_P with individual fake cov

* set gps mode flag

* add gps invalid flag names

* update refs

* more accurate gps accuracy check + update refs
old-commit-hash: 8b6a147583

selfdrive/locationd/locationd.cc

@@ -18,6 +18,7 @@ const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
 const double VALID_TIME_SINCE_RESET = 1.0; // s
 const double VALID_POS_STD = 50.0; // m
 const double MAX_RESET_TRACKER = 5.0;
+const double SANE_GPS_UNCERTAINTY = 1500.0; // m
 
 static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
   VectorXd res(floatlist.size());
@@ -130,16 +131,16 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
   Vector3d nans = Vector3d(NAN, NAN, NAN);
 
   // write measurements to msg
-  init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->last_gps_fix > 0);
-  init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->last_gps_fix > 0);
-  init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->last_gps_fix > 0);
-  init_measurement(fix.initVelocityNED(), ned_vel, nans, this->last_gps_fix > 0);
+  init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode);
+  init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode);
+  init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode);
+  init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode);
   init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
   init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
-  init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->last_gps_fix > 0);
-  init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->last_gps_fix > 0);
-  init_measurement(fix.initOrientationNED(), orientation_ned, nans, this->last_gps_fix > 0);
-  init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->last_gps_fix > 0);
+  init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode);
+  init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
+  init_measurement(fix.initOrientationNED(), orientation_ned, nans, this->gps_mode);
+  init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
   init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
   init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
   init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
@@ -243,27 +244,39 @@ void Localizer::handle_sensors(double current_time, const capnp::List<cereal::Se
   }
 }
 
+void Localizer::input_fake_gps_observations(double current_time) {
+  // This is done to make sure that the error estimate of the position does not blow up
+  // when the filter is in no-gps mode
+  // Steps : first predict -> observe current obs with reasonable STD
+  this->kf->predict(current_time);
+
+  VectorXd current_x = this->kf->get_x();  
+  VectorXd ecef_pos = current_x.segment<3>(0);
+  VectorXd ecef_vel = current_x.segment<3>(7);
+  MatrixXdr ecef_pos_R = this->kf->get_fake_gps_pos_cov();
+  MatrixXdr ecef_vel_R = this->kf->get_fake_gps_vel_cov();
+  
+  this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
+  this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
+}
+
 void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log) {
   // ignore the message if the fix is invalid
-  if (log.getFlags() % 2 == 0) {
-    return;
-  }
-
-  // Sanity checks
-  if ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)) {
-    return;
-  }
 
-  if ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)) {
-    return;
-  }
+  bool gps_invalid_flag = (log.getFlags() % 2 == 0);
+  bool gps_unreasonable = (Vector2d(log.getAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
+  bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
+  bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
+  bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
 
-  if (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK) {
+  if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane){
+    this->determine_gps_mode(current_time);
     return;
   }
 
   // Process message
   this->last_gps_fix = current_time;
+  this->gps_mode = true;
   Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
   this->converter = std::make_unique<LocalCoord>(geodetic);
 
@@ -290,11 +303,11 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
 
   if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) {
     LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
-    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos);
+    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
     this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
   } else if (gps_est_error > 100.0) {
     LOGE("Locationd vs ubloxLocation position difference too large, kalman reset");
-    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos);
+    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
   }
 
   this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
@@ -358,7 +371,8 @@ void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibra
 
 void Localizer::reset_kalman(double current_time) {
   VectorXd init_x = this->kf->get_initial_x();
-  this->reset_kalman(current_time, init_x.segment<4>(3), init_x.head(3));
+  MatrixXdr init_P = this->kf->get_initial_P();
+  this->reset_kalman(current_time, init_x, init_P);
 }
 
 void Localizer::finite_check(double current_time) {
@@ -390,13 +404,26 @@ void Localizer::update_reset_tracker() {
   }
 }
 
-void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos) {
+void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos, VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R) {
   // too nonlinear to init on completely wrong
-  VectorXd init_x = this->kf->get_initial_x();
+  VectorXd current_x = this->kf->get_x();
+  MatrixXdr current_P = this->kf->get_P();
   MatrixXdr init_P = this->kf->get_initial_P();
-  init_x.segment<4>(3) = init_orient;
-  init_x.head(3) = init_pos;
+  MatrixXdr reset_orientation_P = this->kf->get_reset_orientation_P();
+
+  current_x.segment<4>(3) = init_orient;
+  current_x.segment<3>(7) = init_vel;
+  current_x.head(3) = init_pos;
+
+  init_P.block<3,3>(0,0).diagonal() = init_pos_R.diagonal();
+  init_P.block<3,3>(3,3).diagonal() = reset_orientation_P.diagonal();
+  init_P.block<3,3>(6,6).diagonal() = init_vel_R.diagonal();
+  init_P.block<16,16>(9,9).diagonal() = current_P.block<16,16>(9,9).diagonal();
+  
+  this->reset_kalman(current_time, current_x, init_P);
+}
 
+void Localizer::reset_kalman(double current_time, VectorXd init_x, MatrixXdr init_P) {
   this->kf->init_state(init_x, init_P, current_time);
   this->last_reset_time = current_time;
   this->reset_tracker += 1.0;
@@ -447,6 +474,22 @@ bool Localizer::isGpsOK() {
   return this->kf->get_filter_time() - this->last_gps_fix < 1.0;
 }
 
+void Localizer::determine_gps_mode(double current_time) {
+  // 1. If the pos_std is greater than what's not acceptible and localizer is in gps-mode, reset to no-gps-mode
+  // 2. If the pos_std is greater than what's not acceptible and localizer is in no-gps-mode, fake obs
+  // 3. If the pos_std is smaller than what's not acceptible, let gps-mode be whatever it is
+  VectorXd current_pos_std = this->kf->get_P().block<3,3>(0,0).diagonal().array().sqrt();
+  if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){
+    if (this->gps_mode){
+      this->gps_mode = false;
+      this->reset_kalman(current_time);
+    }
+    else{
+      this->input_fake_gps_observations(current_time);
+    }
+  }
+}
+
 int Localizer::locationd_thread() {
   const std::initializer_list<const char *> service_list =
       { "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" };

selfdrive/locationd/locationd.h

@@ -27,11 +27,13 @@ public:
   int locationd_thread();
 
   void reset_kalman(double current_time = NAN);
-  void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos);
+  void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos, Eigen::VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R);
+  void reset_kalman(double current_time, Eigen::VectorXd init_x, MatrixXdr init_P);
   void finite_check(double current_time = NAN);
   void time_check(double current_time = NAN);
   void update_reset_tracker();
   bool isGpsOK();
+  void determine_gps_mode(double current_time);
 
   kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
     bool inputsOK, bool sensorsOK, bool gpsOK);
@@ -49,6 +51,8 @@ public:
   void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
   void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log);
 
+  void input_fake_gps_observations(double current_time);
+
 private:
   std::unique_ptr<LiveKalman> kf;
 
@@ -67,4 +71,5 @@ private:
   double last_gps_fix = 0;
   double reset_tracker = 0.0;
   bool device_fell = false;
+  bool gps_mode = false;
 };

selfdrive/locationd/models/live_kf.cc

@@ -33,6 +33,9 @@ LiveKalman::LiveKalman() {
 
   this->initial_x = live_initial_x;
   this->initial_P = live_initial_P_diag.asDiagonal();
+  this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal();
+  this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal();
+  this->reset_orientation_P = live_reset_orientation_diag.asDiagonal();
   this->Q = live_Q_diag.asDiagonal();
   for (auto& pair : live_obs_noise_diag) {
     this->obs_noise[pair.first] = pair.second.asDiagonal();
@@ -87,6 +90,10 @@ std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, std:
   return r;
 }
 
+void LiveKalman::predict(double t) {
+  this->filter->predict(t);
+}
+
 Eigen::VectorXd LiveKalman::get_initial_x() {
   return this->initial_x;
 }
@@ -95,6 +102,18 @@ MatrixXdr LiveKalman::get_initial_P() {
   return this->initial_P;
 }
 
+MatrixXdr LiveKalman::get_fake_gps_pos_cov() {
+  return this->fake_gps_pos_cov;
+}
+
+MatrixXdr LiveKalman::get_fake_gps_vel_cov() {
+  return this->fake_gps_vel_cov;
+}
+
+MatrixXdr LiveKalman::get_reset_orientation_P() {
+  return this->reset_orientation_P;
+}
+
 MatrixXdr LiveKalman::H(VectorXd in) {
   assert(in.size() == 6);
   Matrix<double, 3, 6, Eigen::RowMajor> res;

selfdrive/locationd/models/live_kf.h

@@ -36,9 +36,13 @@ public:
   std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
   std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
   std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
+  void predict(double t);
 
   Eigen::VectorXd get_initial_x();
   MatrixXdr get_initial_P();
+  MatrixXdr get_fake_gps_pos_cov();
+  MatrixXdr get_fake_gps_vel_cov();
+  MatrixXdr get_reset_orientation_P();
 
   MatrixXdr H(Eigen::VectorXd in);
 
@@ -52,6 +56,9 @@ private:
 
   Eigen::VectorXd initial_x;
   MatrixXdr initial_P;
+  MatrixXdr fake_gps_pos_cov;
+  MatrixXdr fake_gps_vel_cov;
+  MatrixXdr reset_orientation_P;
   MatrixXdr Q;  // process noise
   std::unordered_map<int, MatrixXdr> obs_noise;
 };

selfdrive/locationd/models/live_kf.py

@@ -57,8 +57,8 @@ class LiveKalman():
                         0, 0, 0])
 
   # state covariance
-  initial_P_diag = np.array([1e3**2, 1e3**2, 1e3**2,
-                             0.5**2, 0.5**2, 0.5**2,
+  initial_P_diag = np.array([10**2, 10**2, 10**2,
+                             0.01**2, 0.01**2, 0.01**2,
                              10**2, 10**2, 10**2,
                              1**2, 1**2, 1**2,
                              1**2, 1**2, 1**2,
@@ -67,6 +67,13 @@ class LiveKalman():
                              0.01**2, 0.01**2, 0.01**2,
                              0.01**2, 0.01**2, 0.01**2])
 
+  # state covariance when resetting midway in a segment
+  reset_orientation_diag = np.array([1**2, 1**2, 1**2])
+
+  # fake observation covariance, to ensure the uncertainty estimate of the filter is under control
+  fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
+  fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
+
   # process noise
   Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
                      0.001**2, 0.001**2, 0.001**2,
@@ -241,6 +248,9 @@ class LiveKalman():
 
     live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n"
     live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n"
+    live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n"
+    live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n"
+    live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n"
     live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n"
     live_kf_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {\n"
     for kind, noise in LiveKalman.obs_noise_diag.items():

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-b5eb02181c84285ecc89c4cfe0292ca866354985
+eb82d8fc821da3488dffe85f191211ee1fad5904