Locationd qcom retune (#27739)

old-commit-hash: eca3838237

selfdrive/locationd/locationd.cc

@@ -27,7 +27,6 @@ const double MAX_FILTER_REWIND_TIME = 0.8; // s
 // They should be replaced with synced time from a real clock
 const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s
 const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
-const float  GPS_MUL_FACTOR = 10.0;
 const float  GPS_POS_STD_THRESHOLD = 50.0;
 const float  GPS_VEL_STD_THRESHOLD = 5.0;
 const float  GPS_POS_ERROR_RESET_THRESHOLD = 300.0;
@@ -326,8 +325,14 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
 
   VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
   VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
-  MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(GPS_MUL_FACTOR * log.getAccuracy(),2) + std::pow(GPS_MUL_FACTOR * log.getVerticalAccuracy(),2)).asDiagonal();
+  float ecef_pos_std;
-  MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(GPS_MUL_FACTOR * log.getSpeedAccuracy(), 2)).asDiagonal();
+  if (ublox_available) {
+    ecef_pos_std = std::sqrt(std::pow(log.getAccuracy(), 2) + std::pow(log.getVerticalAccuracy(), 2));
+  } else {
+    ecef_pos_std = std::sqrt(3 * std::pow(log.getVerticalAccuracy(), 2));
+  }
+  MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
+  MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
 
   this->unix_timestamp_millis = log.getUnixTimestampMillis();
   double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
@@ -377,14 +382,14 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements:
 
   // indexed at 0 cause all std values are the same MAE
   auto ecef_pos_std = log.getPositionECEF().getStd()[0];
-  MatrixXdr ecef_pos_R = Vector3d::Constant(pow(GPS_MUL_FACTOR*ecef_pos_std, 2)).asDiagonal();
+  MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal();
 
   auto ecef_vel_v = log.getVelocityECEF().getValue();
   VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]);
 
   // indexed at 0 cause all std values are the same MAE
   auto ecef_vel_std = log.getVelocityECEF().getStd()[0];
-  MatrixXdr ecef_vel_R = Vector3d::Constant(pow(GPS_MUL_FACTOR*ecef_vel_std, 2)).asDiagonal();
+  MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal();
 
   double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
 
@@ -661,8 +666,10 @@ int Localizer::locationd_thread() {
   const char* gps_location_socket;
   if (ublox_available) {
     gps_location_socket = "gpsLocationExternal";
+    this->gps_std_factor = 10.0;
   } else {
     gps_location_socket = "gpsLocation";
+    this->gps_std_factor = 2.0;
   }
   const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
                                                           "carState", "carParams", "accelerometer", "gyroscope"};

selfdrive/locationd/locationd.h

@@ -84,4 +84,5 @@ private:
   std::map<std::string, double> observation_values_invalid;
   bool standstill = true;
   int32_t orientation_reset_count = 0;
+  float gps_std_factor;
 };

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-1da098f096cee9f2871dafd2788dc9ddac85eeab
+1bb3f665191e1b75c1b786f60e76d51b274f98ae