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 <harald.the.engineer@gmail.com>

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<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
   VectorXd res(floatlist.size());
@@ -183,7 +189,10 @@ void Localizer::handle_sensors(double current_time, const capnp::List<cereal::Se
     // Gyro Uncalibrated
     if (sensor_reading.getSensor() == SENSOR_GYRO_UNCALIBRATED && sensor_reading.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
       auto v = sensor_reading.getGyroUncalibrated().getV();
-      this->kf->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::List<cereal::Se
       // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
       this->device_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<LocalCoord>(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;

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