mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 16:23:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

locationd: Fix Nav localization reliability (#22959)

Browse Source 
* 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
This commit is contained in:
Vivek Aithal
2021-11-24 15:24:25 -08:00
committed by GitHub
parent ec6eb5b582
commit d537897e9a
6 changed files with 115 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd.cc
+70 -27
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" };