Misc locationd improvements (#1714)

* I like this more

* rewind less

* bump rednose

* falling off windshield detectopr

* adjust thresholds

* this is a soft disable now

* move that

* process replay fixes

* update refs

Co-authored-by: Comma Device <device@comma.ai>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 8e2d344135

rednose_repo

@@ -1 +1 @@
-Subproject commit 307c771fcaa5e301ccb18d3ab9fdedddd675c569
+Subproject commit 2e556b8219185708ed974a4b6502796607d7ce0d

selfdrive/controls/lib/events.py

@@ -530,15 +530,6 @@ EVENTS = {
                               duration_hud_alert=0.),
   },
 
-  EventName.posenetInvalid: {
-    ET.WARNING: Alert(
-      "TAKE CONTROL",
-      "Vision Model Output Uncertain",
-      AlertStatus.userPrompt, AlertSize.mid,
-      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.),
-    ET.NO_ENTRY: NoEntryAlert("Vision Model Output Uncertain"),
-  },
-
   EventName.focusRecoverActive: {
     ET.WARNING: Alert(
       "TAKE CONTROL",
@@ -659,6 +650,11 @@ EVENTS = {
     ET.NO_ENTRY : NoEntryAlert("Driving model lagging"),
   },
 
+  EventName.posenetInvalid: {
+    ET.SOFT_DISABLE: SoftDisableAlert("Vision Model Output Uncertain"),
+    ET.NO_ENTRY: NoEntryAlert("Vision Model Output Uncertain"),
+  },
+
   EventName.lowMemory: {
     ET.SOFT_DISABLE: SoftDisableAlert("Low Memory: Reboot Your Device"),
     ET.PERMANENT: Alert(

selfdrive/locationd/locationd.py

@@ -1,7 +1,6 @@
 #!/usr/bin/env python3
 import numpy as np
 import sympy as sp
-
 import cereal.messaging as messaging
 import common.transformations.coordinates as coord
 from common.transformations.orientation import ecef_euler_from_ned, \
@@ -23,6 +22,7 @@ from rednose.helpers.sympy_helpers import euler_rotate
 
 VISION_DECIMATION = 2
 SENSOR_DECIMATION = 10
+POSENET_STD_HIST = 40
 
 
 def to_float(arr):
@@ -52,7 +52,7 @@ class Localizer():
 
     self.kf = LiveKalman(GENERATED_DIR)
     self.reset_kalman()
-    self.max_age = .2  # seconds
+    self.max_age = .1  # seconds
     self.disabled_logs = disabled_logs
     self.calib = np.zeros(3)
     self.device_from_calib = np.eye(3)
@@ -63,6 +63,7 @@ class Localizer():
     self.posenet_invalid_count = 0
     self.posenet_speed = 0
     self.car_speed = 0
+    self.posenet_stds = 10*np.ones((POSENET_STD_HIST))
 
     self.converter = coord.LocalCoord.from_ecef(self.kf.x[States.ECEF_POS])
 
@@ -113,8 +114,8 @@ class Localizer():
 
     fix = messaging.log.LiveLocationKalman.new_message()
     fix.positionGeodetic.value = to_float(fix_pos_geo)
-    #fix.positionGeodetic.std = to_float(fix_pos_geo_std)
+    fix.positionGeodetic.std = to_float(np.nan*np.zeros(3))
-    #fix.positionGeodetic.valid = True
+    fix.positionGeodetic.valid = True
     fix.positionECEF.value = to_float(fix_ecef)
     fix.positionECEF.std = to_float(fix_ecef_std)
     fix.positionECEF.valid = True
@@ -122,8 +123,8 @@ class Localizer():
     fix.velocityECEF.std = to_float(vel_ecef_std)
     fix.velocityECEF.valid = True
     fix.velocityNED.value = to_float(ned_vel)
-    #fix.velocityNED.std = to_float(ned_vel_std)
+    fix.velocityNED.std = to_float(np.nan*np.zeros(3))
-    #fix.velocityNED.valid = True
+    fix.velocityNED.valid = True
     fix.velocityDevice.value = to_float(vel_device)
     fix.velocityDevice.std = to_float(vel_device_std)
     fix.velocityDevice.valid = True
@@ -135,11 +136,11 @@ class Localizer():
     fix.orientationECEF.std = to_float(orientation_ecef_std)
     fix.orientationECEF.valid = True
     fix.calibratedOrientationECEF.value = to_float(calibrated_orientation_ecef)
-    #fix.calibratedOrientationECEF.std = to_float(calibrated_orientation_ecef_std)
+    fix.calibratedOrientationECEF.std = to_float(np.nan*np.zeros(3))
-    #fix.calibratedOrientationECEF.valid = True
+    fix.calibratedOrientationECEF.valid = True
     fix.orientationNED.value = to_float(orientation_ned)
-    #fix.orientationNED.std = to_float(orientation_ned_std)
+    fix.orientationNED.std = to_float(np.nan*np.zeros(3))
-    #fix.orientationNED.valid = True
+    fix.orientationNED.valid = True
     fix.angularVelocityDevice.value = to_float(predicted_state[States.ANGULAR_VELOCITY])
     fix.angularVelocityDevice.std = to_float(predicted_std[States.ANGULAR_VELOCITY_ERR])
     fix.angularVelocityDevice.valid = True
@@ -155,14 +156,23 @@ class Localizer():
     fix.accelerationCalibrated.valid = True
     return fix
 
-  def liveLocationMsg(self, time):
+  def liveLocationMsg(self):
     fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P)
 
-    if abs(self.posenet_speed - self.car_speed) > max(0.4 * self.car_speed, 5.0):
+    #if abs(self.posenet_speed - self.car_speed) > max(0.4 * self.car_speed, 5.0):
-      self.posenet_invalid_count += 1
+    #  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
+
+    if std_spike and self.car_speed > 5:
+      fix.posenetOK = False
     else:
-      self.posenet_invalid_count = 0
+      fix.posenetOK = True
-    fix.posenetOK = self.posenet_invalid_count < 4
 
     #fix.gpsWeek = self.time.week
     #fix.gpsTimeOfWeek = self.time.tow
@@ -178,15 +188,10 @@ class Localizer():
 
   def update_kalman(self, time, kind, meas, R=None):
     try:
-      self.kf.predict_and_observe(time, kind, meas, R=R)
+      self.kf.predict_and_observe(time, kind, meas, R)
     except KalmanError:
       cloudlog.error("Error in predict and observe, kalman reset")
       self.reset_kalman()
-    #idx = bisect_right([x[0] for x in self.observation_buffer], time)
-    #self.observation_buffer.insert(idx, (time, kind, meas))
-    #while len(self.observation_buffer) > 0 and self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
-    #  else:
-    #    self.observation_buffer.pop(0)
 
   def handle_gps(self, current_time, log):
     # ignore the message if the fix is invalid
@@ -244,6 +249,8 @@ class Localizer():
       trans_device = self.device_from_calib.dot(log.trans)
       trans_device_std = self.device_from_calib.dot(log.transStd)
       self.posenet_speed = np.linalg.norm(trans_device)
+      self.posenet_stds[:-1] = self.posenet_stds[1:]
+      self.posenet_stds[-1] = trans_device_std[0]
       self.update_kalman(current_time,
                          ObservationKind.CAMERA_ODO_TRANSLATION,
                          np.concatenate([trans_device, 10*trans_device_std]))
@@ -322,7 +329,7 @@ def locationd_thread(sm, pm, disabled_logs=None):
       msg = messaging.new_message('liveLocationKalman')
       msg.logMonoTime = t
 
-      msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9)
+      msg.liveLocationKalman = localizer.liveLocationMsg()
       msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid()
       msg.liveLocationKalman.sensorsOK = sm.alive['sensorEvents'] and sm.valid['sensorEvents']
 

selfdrive/locationd/models/live_kf.py

@@ -206,7 +206,7 @@ class LiveKalman():
                       ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.diag([.2**2, .2**2, .2**2, .2**2])}
 
     # init filter
-    self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)
+    self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err, max_rewind_age=0.2)
 
   @property
   def x(self):

selfdrive/test/process_replay/compare_logs.py

@@ -55,11 +55,12 @@ def remove_ignored_fields(msg, ignore):
 def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=None):
   if ignore_fields is None:
     ignore_fields = []
-
   if ignore_msgs is None:
     ignore_msgs = []
+
   log1, log2 = [list(filter(lambda m: m.which() not in ignore_msgs, log)) for log in (log1, log2)]
-  assert len(log1) == len(log2), "logs are not same length: " + str(len(log1)) + " VS " + str(len(log2))
+  if len(log1) != len(log2):
+    raise Exception(f"logs are not same length: {len(log1)} VS {len(log2)}")
 
   diff = []
   for msg1, msg2 in tqdm(zip(log1, log2)):
@@ -77,8 +78,9 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non
       tolerance = EPSILON if tolerance is None else tolerance
       dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields)
 
-      # Dictiffer only supports relative tolerance, we also want to check for absolute
+      # Dictdiffer only supports relative tolerance, we also want to check for absolute
       def outside_tolerance(diff):
+        if diff[0] == "change":
           a, b = diff[2]
           if isinstance(a, numbers.Number) and isinstance(b, numbers.Number):
             return abs(a - b) > max(tolerance, tolerance * max(abs(a), abs(b)))

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-f3261f36402ad9bc3003feb9de61a04a634f30e1
+d8944d6eb13c30c2fd5bcc1af5dbad07472c42c0

selfdrive/test/process_replay/test_processes.py

@@ -71,8 +71,10 @@ def test_process(cfg, lr, cmp_log_fn, ignore_fields=None, ignore_msgs=None):
       segment = cmp_log_fn.split("/")[-1].split("_")[0]
       raise Exception("Route never enabled: %s" % segment)
 
+  try:
     return compare_logs(cmp_log_msgs, log_msgs, ignore_fields+cfg.ignore, ignore_msgs, cfg.tolerance)
-
+  except Exception as e:
+    return str(e)
 
 def format_diff(results, ref_commit):
   diff1, diff2 = "", ""