locationd: Add camera-IMU cross-checks (#29727)

* camera-gyro cross checks, but one way

* increase factor to account for gyro noise (potholes, bad roads etc

* increase factor to reduce FP with device taps, bad roads, etc

* factor to 30

* add inputsok to sensoir data invalid alert

* bugfix

* move the sensors check

* add localizer catchall alert

* update refcommit

* remove permanent alert

* revert sensorDataInvalid alert change, split into new PR
old-commit-hash: dbada885ac

selfdrive/controls/controlsd.py

@@ -381,6 +381,8 @@ class Controls:
     if not (self.sm['liveParameters'].sensorValid or self.sm['liveLocationKalman'].sensorsOK) and not NOSENSOR:
       if self.sm.frame > 5 / DT_CTRL:  # Give locationd some time to receive all the inputs
         self.events.add(EventName.sensorDataInvalid)
+    if not self.sm['liveLocationKalman'].inputsOK and not NOSENSOR:
+      self.events.add(EventName.localizerMalfunction)
     if not self.sm['liveLocationKalman'].posenetOK:
       self.events.add(EventName.posenetInvalid)
     if not self.sm['liveLocationKalman'].deviceStable:
@@ -420,8 +422,6 @@ class Controls:
 
       if self.sm['modelV2'].frameDropPerc > 20:
         self.events.add(EventName.modeldLagging)
-      if self.sm['liveLocationKalman'].excessiveResets:
-        self.events.add(EventName.localizerMalfunction)
 
   def data_sample(self):
     """Receive data from sockets and update carState"""

selfdrive/controls/lib/events.py

@@ -576,11 +576,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
     ET.PERMANENT: NormalPermanentAlert("GPS Malfunction", "Likely Hardware Issue"),
   },
 
-  # When the GPS position and localizer diverge the localizer is reset to the
-  # current GPS position. This alert is thrown when the localizer is reset
-  # more often than expected.
   EventName.localizerMalfunction: {
-    # ET.PERMANENT: NormalPermanentAlert("Sensor Malfunction", "Hardware Malfunction"),
+    ET.NO_ENTRY: NoEntryAlert("Localizer Malfunction"),
+    ET.SOFT_DISABLE: soft_disable_alert("Localizer Malfunction"),
   },
 
   # ********** events that affect controls state transitions **********

selfdrive/locationd/locationd.cc

@@ -21,9 +21,11 @@ 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
-const double INPUT_INVALID_THRESHOLD = 5.0; // same as reset tracker
+const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker
-const double DECAY = 0.99995; // same as reset tracker
+const double RESET_TRACKER_DECAY = 0.99995;
+const double DECAY = 0.9993; // ~10 secs to resume after a bad input
 const double MAX_FILTER_REWIND_TIME = 0.8; // s
+const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30;
 
 // TODO: GPS sensor time offsets are empirically calculated
 // They should be replaced with synced time from a real clock
@@ -256,7 +258,13 @@ void Localizer::handle_sensor(double current_time, const cereal::SensorEventData
   if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
     auto v = log.getGyroUncalibrated().getV();
     auto meas = Vector3d(-v[2], -v[1], -v[0]);
-    if (meas.norm() < ROTATION_SANITY_CHECK) {
+
+    VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START);
+    float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]);
+    float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1];
+    bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold;
+
+    if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) {
       this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
       this->observation_values_invalid["gyroscope"] *= DECAY;
     } else {
@@ -483,6 +491,7 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry
   this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
     { trans_device }, { trans_device_cov });
   this->observation_values_invalid["cameraOdometry"] *= DECAY;
+  this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]);
 }
 
 void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {
@@ -538,7 +547,7 @@ void Localizer::time_check(double current_time) {
 void Localizer::update_reset_tracker() {
   // reset tracker is tuned to trigger when over 1reset/10s over 2min period
   if (this->is_gps_ok()) {
-    this->reset_tracker *= DECAY;
+    this->reset_tracker *= RESET_TRACKER_DECAY;
   } else {
     this->reset_tracker = 0.0;
   }

selfdrive/locationd/locationd.h

@@ -94,6 +94,7 @@ private:
   float gps_variance_factor;
   float gps_vertical_variance_factor;
   double gps_time_offset;
+  Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std
 
   void configure_gnss_source(const LocalizerGnssSource &source);
 };

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-649c512f95d9ad80e7c8c4d852cfa5f468a28fd5
+89e571c56fb6c053177cd28c3e0d688c641b46d5