add fall filter and less FP on posenet (#1971)

* add fall filter and less FP on posenet

* add alert

* list

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

cereal

@@ -1 +1 @@
-Subproject commit 10a25c8b98ad295308e6825cef7cf42dff6f4396
+Subproject commit 61d7488e55fcaa3760d9ccc9d6589b30cbe4a6c1

selfdrive/controls/controlsd.py

@@ -216,6 +216,8 @@ class Controls:
       self.events.add(EventName.vehicleModelInvalid)
     if not self.sm['liveLocationKalman'].posenetOK:
       self.events.add(EventName.posenetInvalid)
+    if not self.sm['liveLocationKalman'].deviceStable:
+      self.events.add(EventName.deviceFalling)
     if not self.sm['frame'].recoverState < 2:
       # counter>=2 is active
       self.events.add(EventName.focusRecoverActive)

selfdrive/controls/lib/events.py

@@ -655,6 +655,11 @@ EVENTS = {
     ET.NO_ENTRY: NoEntryAlert("Vision Model Output Uncertain"),
   },
 
+  EventName.deviceFalling: {
+    ET.SOFT_DISABLE: SoftDisableAlert("Device Fell Off Mount"),
+    ET.NO_ENTRY: NoEntryAlert("Device Fell Off Mount"),
+  },
+
   EventName.lowMemory: {
     ET.SOFT_DISABLE: SoftDisableAlert("Low Memory: Reboot Your Device"),
     ET.PERMANENT: Alert(

selfdrive/locationd/locationd.py

@@ -69,6 +69,7 @@ class Localizer():
 
     self.unix_timestamp_millis = 0
     self.last_gps_fix = 0
+    self.device_fell = False
 
   @staticmethod
   def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov):
@@ -142,21 +143,12 @@ class Localizer():
 
   def liveLocationMsg(self):
     fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P)
-
+    # experimentally found these values, no false positives in 20k minutes of driving
-    #if abs(self.posenet_speed - self.car_speed) > max(0.4 * self.car_speed, 5.0):
-    #  self.posenet_invalid_count += 1
-    #else:
-    #  self.posenet_invalid_count = 0
-    #fix.posenetOK = self.posenet_invalid_count < 4
-
-    # experimentally found these values
     old_mean, new_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST//2]), np.mean(self.posenet_stds[POSENET_STD_HIST//2:])
-    std_spike = new_mean/old_mean > 4 and new_mean > 5
+    std_spike = new_mean/old_mean > 4 and new_mean > 7
 
-    if std_spike and self.car_speed > 5:
+    fix.posenetOK = not (std_spike and self.car_speed > 5)
-      fix.posenetOK = False
+    fix.deviceStable = not self.device_fell
-    else:
-      fix.posenetOK = True
 
     #fix.gpsWeek = self.time.week
     #fix.gpsTimeOfWeek = self.time.tow
@@ -251,6 +243,10 @@ class Localizer():
 
       # Accelerometer
       if sensor_reading.sensor == 1 and sensor_reading.type == 1:
+        # check if device fell, estimate 10 for g
+        # 40g is a good filter for falling detection, no false positives in 20k minutes of driving
+        self.device_fell = abs(sensor_reading.acceleration.v[0] - 10) > 40
+
         self.acc_counter += 1
         if self.acc_counter % SENSOR_DECIMATION == 0:
           v = sensor_reading.acceleration.v