address PR comments

selfdrive/locationd/laikad_helpers.py

@@ -44,7 +44,7 @@ def pr_residual(measurements, posfix_functions, signal='C1C'):
 
 
 def get_prr_sympy_func():
-  # implemting this without sympy.Matrix gives a 2x speedup at generation
+  # implementing this without sympy.Matrix gives a 2x speedup at generation
 
   # knowns, receiver position, satellite position, satellite velocity
   ep_x, ep_y, ep_z = sympy.Symbol('ep_x'), sympy.Symbol('ep_y'), sympy.Symbol('ep_z')
@@ -89,7 +89,7 @@ def prr_residual(measurements, est_pos, no_weight=False, signal='D1C'):
         continue
 
       sat_pos = meas.sat_pos_final if meas.corrected else meas.sat_pos
-      weight = 1 if no_weight or meas.observables[signal] == 0 else (1 / meas.observables[signal])
+      weight = 1 if no_weight or meas.observables_std[signal] == 0 else (1 / meas.observables_std[signal])
 
       val, *gradient = loss_func(est_pos[0], est_pos[1], est_pos[2],
                                  sat_pos[0], sat_pos[1], sat_pos[2],

selfdrive/locationd/locationd.cc

@@ -27,6 +27,13 @@ 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 = 200.0
+const float  GPS_POS_STD_RESET_THRESHOLD = 5.0
+const float  GPS_VEL_STD_RESET_THRESHOLD = 0.5
+const float  GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 5.0
 
 static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
   VectorXd res(floatlist.size());
@@ -316,8 +323,8 @@ 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(10.0 * log.getAccuracy(),2) + std::pow(10.0 * log.getVerticalAccuracy(),2)).asDiagonal();
+  MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(GPS_MUL_FACTOR * log.getAccuracy(),2) + std::pow(GPS_MUL_FACTOR * log.getVerticalAccuracy(),2)).asDiagonal();
-  MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy() * 10.0, 2)).asDiagonal();
+  MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(GPS_MUL_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();
@@ -368,14 +375,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(ecef_pos_std*10, 2)).asDiagonal();
+  MatrixXdr ecef_pos_R = Vector3d::Constant(pow(GPS_MUL_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(ecef_vel_std*10, 2)).asDiagonal();
+  MatrixXdr ecef_vel_R = Vector3d::Constant(pow(GPS_MUL_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();
 
@@ -398,17 +405,17 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements:
   }
   VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
 
-  if (ecef_pos_std > 50. || ecef_vel_std > 5.) {
+  if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
     this->gps_valid = false;
     this->determine_gps_mode(current_time);
     return;
   }
 
-  if ((gps_est_error > 200. && ecef_pos_std < 5.) || this->last_gps_msg == 0) {
+  if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
     // always reset on first gps message and if the location is off but the accuracy is high
     LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset");
     this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
-  } else if (ecef_vel_std < .5 && orientation_error.norm() > 5.0) {
+  } else if (ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD && orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD) {
     LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset");
     this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
     this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });