From 3778592f5049e53b06d1fd6b11e08f59c6fee42a Mon Sep 17 00:00:00 2001 From: Vivek Aithal Date: Fri, 17 Sep 2021 22:33:46 -0700 Subject: [PATCH] locationd: modify factor for measurement STDEV in gps update (#22262) * modify factor for measurement STDEV in gps update * add new ref_commit --- selfdrive/locationd/locationd.cc | 6 +++--- selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index e49718af7a..2e58f1fb7f 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -261,9 +261,9 @@ 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(3.0 * log.getVerticalAccuracy(), 2)).asDiagonal(); - MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy(), 2)).asDiagonal(); - + 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(); + this->unix_timestamp_millis = log.getTimestamp(); double gps_est_error = (this->kf->get_x().head(3) - ecef_pos).norm(); diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 59e6fb6035..e14f2b7b80 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -c66c5414f85cd98ad2dd7a83989e05990851da74 \ No newline at end of file +b575f81975c405417b502479ba15614bc1b3ab9d \ No newline at end of file