From 369cba05849459f67dbe17aa00c4437d32c909fb Mon Sep 17 00:00:00 2001 From: Kurt Nistelberger Date: Wed, 14 Dec 2022 18:02:41 -0800 Subject: [PATCH] address PR comments --- selfdrive/locationd/laikad_helpers.py | 4 ++-- selfdrive/locationd/locationd.cc | 21 ++++++++++++++------- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/selfdrive/locationd/laikad_helpers.py b/selfdrive/locationd/laikad_helpers.py index e5593f3606..61f09732c8 100644 --- a/selfdrive/locationd/laikad_helpers.py +++ b/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], diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 5ee5636878..c40c88f416 100755 --- a/selfdrive/locationd/locationd.cc +++ b/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::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_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy() * 10.0, 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(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_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_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 });