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: 8b6a14758329eb977beb5cba73403dd113495aeb

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():