spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

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
pull/23008/head^2
Vivek Aithal 2021-11-24 15:24:25 -08:00 committed by GitHub
parent 5bcb4c0358
commit 8b6a147583
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 115 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
selfdrive/locationd/locationd.cc
View File

@ -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<float, capnp::Kind::PRIMITIVE>::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<cereal::Se
}
}
void Localizer::input_fake_gps_observations(double current_time) {
// This is done to make sure that the error estimate of the position does not blow up
// when the filter is in no-gps mode
// Steps : first predict -> 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;
}
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 ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)) {
return;
}
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<LocalCoord>(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<const char *> service_list =
{ "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" };

7
selfdrive/locationd/locationd.h
View File

@ -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<capnp::byte> 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<LiveKalman> kf;
@ -67,4 +71,5 @@ private:
double last_gps_fix = 0;
double reset_tracker = 0.0;
bool device_fell = false;
bool gps_mode = false;
};

19
selfdrive/locationd/models/live_kf.cc
View File

@ -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<Estimate> 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<double, 3, 6, Eigen::RowMajor> res;

7
selfdrive/locationd/models/live_kf.h
View File

@ -36,9 +36,13 @@ public:
std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> 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<int, MatrixXdr> obs_noise;
};

14
selfdrive/locationd/models/live_kf.py
View File

@ -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<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {\n"
for kind, noise in LiveKalman.obs_noise_diag.items():

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
b5eb02181c84285ecc89c4cfe0292ca866354985
eb82d8fc821da3488dffe85f191211ee1fad5904