From d537897e9a3827051abc263be970ff657a370bcb Mon Sep 17 00:00:00 2001 From: Vivek Aithal Date: Wed, 24 Nov 2021 15:24:25 -0800 Subject: [PATCH] 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/locationd.cc | 97 +++++++++++++++++------- selfdrive/locationd/locationd.h | 7 +- selfdrive/locationd/models/live_kf.cc | 19 +++++ selfdrive/locationd/models/live_kf.h | 7 ++ selfdrive/locationd/models/live_kf.py | 14 +++- selfdrive/test/process_replay/ref_commit | 2 +- 6 files changed, 115 insertions(+), 31 deletions(-) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 8bdc30a8d2..0056687e51 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -18,6 +18,7 @@ const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad const double VALID_TIME_SINCE_RESET = 1.0; // s const double VALID_POS_STD = 50.0; // m const double MAX_RESET_TRACKER = 5.0; +const double SANE_GPS_UNCERTAINTY = 1500.0; // m static VectorXd floatlist2vector(const capnp::List::Reader& floatlist) { VectorXd res(floatlist.size()); @@ -130,16 +131,16 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { Vector3d nans = Vector3d(NAN, NAN, NAN); // write measurements to msg - init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->last_gps_fix > 0); - init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->last_gps_fix > 0); - init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->last_gps_fix > 0); - init_measurement(fix.initVelocityNED(), ned_vel, nans, this->last_gps_fix > 0); + init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode); + init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode); + init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode); + init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode); init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true); init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true); - init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->last_gps_fix > 0); - init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->last_gps_fix > 0); - init_measurement(fix.initOrientationNED(), orientation_ned, nans, this->last_gps_fix > 0); - init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->last_gps_fix > 0); + init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode); + init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode); + init_measurement(fix.initOrientationNED(), orientation_ned, nans, this->gps_mode); + init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode); init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true); init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated); init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated); @@ -243,27 +244,39 @@ void Localizer::handle_sensors(double current_time, const capnp::List observe current obs with reasonable STD + this->kf->predict(current_time); + + VectorXd current_x = this->kf->get_x(); + VectorXd ecef_pos = current_x.segment<3>(0); + VectorXd ecef_vel = current_x.segment<3>(7); + MatrixXdr ecef_pos_R = this->kf->get_fake_gps_pos_cov(); + MatrixXdr ecef_vel_R = this->kf->get_fake_gps_vel_cov(); + + this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); + this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); +} + void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log) { // ignore the message if the fix is invalid - if (log.getFlags() % 2 == 0) { - return; - } - - // Sanity checks - if ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)) { - return; - } - if ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)) { - return; - } + bool gps_invalid_flag = (log.getFlags() % 2 == 0); + bool gps_unreasonable = (Vector2d(log.getAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY); + bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)); + bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)); + bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK); - if (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK) { + if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane){ + this->determine_gps_mode(current_time); return; } // Process message this->last_gps_fix = current_time; + this->gps_mode = true; Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; this->converter = std::make_unique(geodetic); @@ -290,11 +303,11 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) { LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset"); - this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); } else if (gps_est_error > 100.0) { LOGE("Locationd vs ubloxLocation position difference too large, kalman reset"); - this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); } this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); @@ -358,7 +371,8 @@ void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibra void Localizer::reset_kalman(double current_time) { VectorXd init_x = this->kf->get_initial_x(); - this->reset_kalman(current_time, init_x.segment<4>(3), init_x.head(3)); + MatrixXdr init_P = this->kf->get_initial_P(); + this->reset_kalman(current_time, init_x, init_P); } void Localizer::finite_check(double current_time) { @@ -390,13 +404,26 @@ void Localizer::update_reset_tracker() { } } -void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos) { +void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos, VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R) { // too nonlinear to init on completely wrong - VectorXd init_x = this->kf->get_initial_x(); + VectorXd current_x = this->kf->get_x(); + MatrixXdr current_P = this->kf->get_P(); MatrixXdr init_P = this->kf->get_initial_P(); - init_x.segment<4>(3) = init_orient; - init_x.head(3) = init_pos; + MatrixXdr reset_orientation_P = this->kf->get_reset_orientation_P(); + + current_x.segment<4>(3) = init_orient; + current_x.segment<3>(7) = init_vel; + current_x.head(3) = init_pos; + + init_P.block<3,3>(0,0).diagonal() = init_pos_R.diagonal(); + init_P.block<3,3>(3,3).diagonal() = reset_orientation_P.diagonal(); + init_P.block<3,3>(6,6).diagonal() = init_vel_R.diagonal(); + init_P.block<16,16>(9,9).diagonal() = current_P.block<16,16>(9,9).diagonal(); + + this->reset_kalman(current_time, current_x, init_P); +} +void Localizer::reset_kalman(double current_time, VectorXd init_x, MatrixXdr init_P) { this->kf->init_state(init_x, init_P, current_time); this->last_reset_time = current_time; this->reset_tracker += 1.0; @@ -447,6 +474,22 @@ bool Localizer::isGpsOK() { return this->kf->get_filter_time() - this->last_gps_fix < 1.0; } +void Localizer::determine_gps_mode(double current_time) { + // 1. If the pos_std is greater than what's not acceptible and localizer is in gps-mode, reset to no-gps-mode + // 2. If the pos_std is greater than what's not acceptible and localizer is in no-gps-mode, fake obs + // 3. If the pos_std is smaller than what's not acceptible, let gps-mode be whatever it is + VectorXd current_pos_std = this->kf->get_P().block<3,3>(0,0).diagonal().array().sqrt(); + if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){ + if (this->gps_mode){ + this->gps_mode = false; + this->reset_kalman(current_time); + } + else{ + this->input_fake_gps_observations(current_time); + } + } +} + int Localizer::locationd_thread() { const std::initializer_list service_list = { "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" }; diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index 60fed112cf..9bc864bf6d 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -27,11 +27,13 @@ public: int locationd_thread(); void reset_kalman(double current_time = NAN); - void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos); + void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos, Eigen::VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R); + void reset_kalman(double current_time, Eigen::VectorXd init_x, MatrixXdr init_P); void finite_check(double current_time = NAN); void time_check(double current_time = NAN); void update_reset_tracker(); bool isGpsOK(); + void determine_gps_mode(double current_time); kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, bool inputsOK, bool sensorsOK, bool gpsOK); @@ -49,6 +51,8 @@ public: void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log); void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log); + void input_fake_gps_observations(double current_time); + private: std::unique_ptr kf; @@ -67,4 +71,5 @@ private: double last_gps_fix = 0; double reset_tracker = 0.0; bool device_fell = false; + bool gps_mode = false; }; diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc index 541c0f7730..914f272fd3 100755 --- a/selfdrive/locationd/models/live_kf.cc +++ b/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 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 res; diff --git a/selfdrive/locationd/models/live_kf.h b/selfdrive/locationd/models/live_kf.h index 4cd4756c9f..06ec3854cb 100755 --- a/selfdrive/locationd/models/live_kf.h +++ b/selfdrive/locationd/models/live_kf.h @@ -36,9 +36,13 @@ public: std::optional predict_and_update_odo_speed(std::vector speed, double t, int kind); std::optional predict_and_update_odo_trans(std::vector trans, double t, int kind); std::optional predict_and_update_odo_rot(std::vector 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 obs_noise; }; diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py index 75415a2845..93aab2774f 100755 --- a/selfdrive/locationd/models/live_kf.py +++ b/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> live_obs_noise_diag = {\n" for kind, noise in LiveKalman.obs_noise_diag.items(): diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1014685633..b179c09e16 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -b5eb02181c84285ecc89c4cfe0292ca866354985 \ No newline at end of file +eb82d8fc821da3488dffe85f191211ee1fad5904 \ No newline at end of file