From c62bb106407f50b785570f6b01736839e2b42514 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 6 May 2021 22:39:50 +0200 Subject: [PATCH] Locationd input sanity checks (#20845) * Add first sanity checks on inputs * more gps checks * make sure test values pass sanity checks * this localizer supports cannonball runs Co-authored-by: Harald Schafer --- selfdrive/locationd/locationd.cc | 50 ++++++++++++++++++++-- selfdrive/locationd/test/test_locationd.py | 14 +++--- 2 files changed, 54 insertions(+), 10 deletions(-) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index d43f78261f..0e742cb6e9 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -7,6 +7,12 @@ using namespace EKFS; using namespace Eigen; ExitHandler do_exit; +const double ACCEL_SANITY_CHECK = 100.0; // m/s^2 +const double ROTATION_SANITY_CHECK = 10.0; // rad/s +const double TRANS_SANITY_CHECK = 200.0; // m/s +const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg) +const double ALTITUDE_SANITY_CHECK = 10000; // m +const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad static VectorXd floatlist2vector(const capnp::List::Reader& floatlist) { VectorXd res(floatlist.size()); @@ -183,7 +189,10 @@ void Localizer::handle_sensors(double current_time, const capnp::Listkf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { Vector3d(-v[2], -v[1], -v[0]) }); + auto meas = Vector3d(-v[2], -v[1], -v[0]); + if (meas.norm() < ROTATION_SANITY_CHECK) { + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas }); + } } // Accelerometer @@ -194,7 +203,10 @@ void Localizer::handle_sensors(double current_time, const capnp::Listdevice_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0; - this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { Vector3d(-v[2], -v[1], -v[0]) }); + auto meas = Vector3d(-v[2], -v[1], -v[0]); + if (meas.norm() < ACCEL_SANITY_CHECK) { + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas }); + } } } } @@ -205,8 +217,21 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R return; } - this->last_gps_fix = current_time; + // 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; + } + + if (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK){ + return; + } + // Process message + this->last_gps_fix = current_time; Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; this->converter = std::make_unique(geodetic); @@ -255,9 +280,21 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); + if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) { + return; + } + VectorXd rot_calib_std = floatlist2vector(log.getRotStd()); VectorXd trans_calib_std = floatlist2vector(log.getTransStd()); + if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)){ + return; + } + + if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) { + return; + } + this->posenet_stds.pop_front(); this->posenet_stds.push_back(trans_calib_std[0]); @@ -275,7 +312,12 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { if (log.getRpyCalib().size() > 0) { - this->calib = floatlist2vector(log.getRpyCalib()); + auto calib = floatlist2vector(log.getRpyCalib()); + if ((calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)){ + return; + } + + this->calib = calib; this->device_from_calib = euler2rot(this->calib); this->calib_from_device = this->device_from_calib.transpose(); this->calibrated = log.getCalStatus() == 1; diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index ac8aa6496a..27852c9bf6 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -45,7 +45,7 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t def test_liblocalizer(self): msg = messaging.new_message('liveCalibration') msg.liveCalibration.validBlocks = random.randint(1, 10) - msg.liveCalibration.rpyCalib = [random.random() for _ in range(3)] + msg.liveCalibration.rpyCalib = [random.random() / 10 for _ in range(3)] self.localizer_handle_msg(msg) liveloc = self.localizer_get_msg() @@ -84,17 +84,17 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t for _ in range(20 * VISION_DECIMATION): # size of hist_old msg = messaging.new_message('cameraOdometry') msg.cameraOdometry.rot = [0.0, 0.0, 0.0] - msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0] + msg.cameraOdometry.rotStd = [0.1, 0.1, 0.1] msg.cameraOdometry.trans = [0.0, 0.0, 0.0] - msg.cameraOdometry.transStd = [2.0, 0.0, 0.0] + msg.cameraOdometry.transStd = [2.0, 0.1, 0.1] self.localizer_handle_msg(msg) for _ in range(20 * VISION_DECIMATION): # size of hist_new msg = messaging.new_message('cameraOdometry') msg.cameraOdometry.rot = [0.0, 0.0, 0.0] - msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0] + msg.cameraOdometry.rotStd = [1.0, 1.0, 1.0] msg.cameraOdometry.trans = [0.0, 0.0, 0.0] - msg.cameraOdometry.transStd = [8.1, 0.0, 0.0] # more than 4 times larger + msg.cameraOdometry.transStd = [10.1, 0.1, 0.1] # more than 4 times larger self.localizer_handle_msg(msg) ret = self.localizer_get_msg() @@ -136,7 +136,9 @@ class TestLocationdProc(unittest.TestCase): msg = messaging.new_message('gpsLocationExternal') msg.logMonoTime = 0 msg.gpsLocationExternal.flags = 1 - msg.gpsLocationExternal.verticalAccuracy = 0.0 + msg.gpsLocationExternal.verticalAccuracy = 1.0 + msg.gpsLocationExternal.speedAccuracy = 1.0 + msg.gpsLocationExternal.bearingAccuracyDeg = 1.0 msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0] msg.gpsLocationExternal.latitude = lat msg.gpsLocationExternal.longitude = lon