locationd: frequency based bad observation resiliance and recovery (#34476)

* Improve it

* Fix static

* Fix test_consistent_timing_spikes test

* Fix tests

* Comment

* Remove crap

selfdrive/locationd/locationd.py

@@ -24,14 +24,16 @@ MIN_STD_SANITY_CHECK = 1e-5  # m or rad
 MAX_FILTER_REWIND_TIME = 0.8  # s
 MAX_SENSOR_TIME_DIFF = 0.1  # s
 YAWRATE_CROSS_ERR_CHECK_FACTOR = 30
-INPUT_INVALID_THRESHOLD = 0.5  # 0 bad inputs ignored
+INPUT_INVALID_LIMIT = 2.0 # 1 (camodo) / 9 (sensor) bad input[s] ignored
-TIMING_INVALID_THRESHOLD = 2.5  # 2 bad timings ignored
+INPUT_INVALID_RECOVERY = 10.0 # ~10 secs to resume after exceeding allowed bad inputs by one
-INPUT_INVALID_DECAY = 0.9993  # ~10 secs to resume after exceeding allowed bad inputs by one (at 100hz)
-TIMING_INVALID_DECAY = 0.9990  # ~2 secs to resume after exceeding allowed bad timings by one (at 100hz)
 POSENET_STD_INITIAL_VALUE = 10.0
 POSENET_STD_HIST_HALF = 20
 
 
+def calculate_invalid_input_decay(invalid_limit, recovery_time, frequency):
+  return (1 - 1 / (2 * invalid_limit)) ** (1 / (recovery_time * frequency))
+
+
 def init_xyz_measurement(measurement: capnp._DynamicStructBuilder, values: np.ndarray, stds: np.ndarray, valid: bool):
   assert len(values) == len(stds) == 3
   measurement.x, measurement.y, measurement.z = map(float, values)
@@ -269,11 +271,11 @@ def main():
 
   filter_initialized = False
   critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"]
-  observation_timing_invalid = defaultdict(int)
   observation_input_invalid = defaultdict(int)
 
-  input_invalid_decay = {s: INPUT_INVALID_DECAY ** (100. / SERVICE_LIST[s].frequency) for s in critcal_services}
+  input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services}
-  timing_invalid_decay = {s: TIMING_INVALID_DECAY ** (100. / SERVICE_LIST[s].frequency) for s in critcal_services}
+  input_invalid_threshold = {s: input_invalid_limit[s] - 0.5 for s in critcal_services}
+  input_invalid_decay = {s: calculate_invalid_input_decay(input_invalid_limit[s], INPUT_INVALID_RECOVERY, SERVICE_LIST[s].frequency) for s in critcal_services}
 
   initial_pose = params.get("LocationFilterInitialState")
   if initial_pose is not None:
@@ -306,19 +308,20 @@ def main():
             continue
 
           if res == HandleLogResult.TIMING_INVALID:
-            observation_timing_invalid[which] += 1
+            print(f"Observation {which} ignored due to failed timing check")
+            observation_input_invalid[which] += 1
+            print(observation_input_invalid[which])
           elif res == HandleLogResult.INPUT_INVALID:
+            print(f"Observation {which} ignored due to failed sanity check")
             observation_input_invalid[which] += 1
           else:
             observation_input_invalid[which] *= input_invalid_decay[which]
-            observation_timing_invalid[which] *= timing_invalid_decay[which]
     else:
       filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
 
     if sm.updated["cameraOdometry"]:
-      critical_service_inputs_valid = all(observation_input_invalid[s] < INPUT_INVALID_THRESHOLD for s in critcal_services)
+      critical_service_inputs_valid = all(observation_input_invalid[s] < input_invalid_threshold[s] for s in critcal_services)
-      critical_service_timing_valid = all(observation_timing_invalid[s] < TIMING_INVALID_THRESHOLD for s in critcal_services)
+      inputs_valid = sm.all_valid() and critical_service_inputs_valid
-      inputs_valid = sm.all_valid() and critical_service_inputs_valid and critical_service_timing_valid
       sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
 
       msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized)

selfdrive/locationd/test/test_locationd.py

@@ -1,56 +0,0 @@
-import capnp
-
-import cereal.messaging as messaging
-from openpilot.common.params import Params
-
-from openpilot.system.manager.process_config import managed_processes
-
-
-class TestLocationdProc:
-  LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration',
-              'accelerometer', 'gyroscope', 'magnetometer']
-
-  def setup_method(self):
-    self.pm = messaging.PubMaster(self.LLD_MSGS)
-
-    self.params = Params()
-    self.params.put_bool("UbloxAvailable", True)
-    managed_processes['locationd'].prepare()
-    managed_processes['locationd'].start()
-
-  def teardown_method(self):
-    managed_processes['locationd'].stop()
-
-  def get_msg(self, name, t):
-    try:
-      msg = messaging.new_message(name)
-    except capnp.lib.capnp.KjException:
-      msg = messaging.new_message(name, 0)
-
-    if name == "gpsLocationExternal":
-      msg.gpsLocationExternal.flags = 1
-      msg.gpsLocationExternal.hasFix = True
-      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 = float(self.lat)
-      msg.gpsLocationExternal.longitude = float(self.lon)
-      msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
-      msg.gpsLocationExternal.altitude = float(self.alt)
-    #if name == "gnssMeasurements":
-    #  msg.gnssMeasurements.measTime = t
-    #  msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
-    #  msg.gnssMeasurements.positionECEF.std = [0,0,0]
-    #  msg.gnssMeasurements.positionECEF.valid = True
-    #  msg.gnssMeasurements.velocityECEF.value = []
-    #  msg.gnssMeasurements.velocityECEF.std = [0,0,0]
-    #  msg.gnssMeasurements.velocityECEF.valid = True
-    elif name == 'cameraOdometry':
-      msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
-      msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
-      msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
-      msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
-    msg.logMonoTime = t
-    msg.valid = True
-    return msg

selfdrive/locationd/test/test_locationd_scenarios.py

@@ -23,8 +23,10 @@ class Scenario(Enum):
   BASE = 'base'
   GYRO_OFF = 'gyro_off'
   GYRO_SPIKE_MIDWAY = 'gyro_spike_midway'
+  GYRO_CONSISTENT_SPIKES = 'gyro_consistent_spikes'
   ACCEL_OFF = 'accel_off'
   ACCEL_SPIKE_MIDWAY = 'accel_spike_midway'
+  ACCEL_CONSISTENT_SPIKES = 'accel_consistent_spikes'
   SENSOR_TIMING_SPIKE_MIDWAY = 'timing_spikes'
   SENSOR_TIMING_CONSISTENT_SPIKES = 'timing_consistent_spikes'
 
@@ -63,18 +65,20 @@ def run_scenarios(scenario, logs):
   elif scenario == Scenario.GYRO_OFF:
     logs = sorted([x for x in logs if x.which() != 'gyroscope'], key=lambda x: x.logMonoTime)
 
-  elif scenario == Scenario.GYRO_SPIKE_MIDWAY:
+  elif scenario == Scenario.GYRO_SPIKE_MIDWAY or scenario == Scenario.GYRO_CONSISTENT_SPIKES:
     def gyro_spike(msg):
       msg.gyroscope.gyroUncalibrated.v[0] += 3.0
-    logs = modify_logs_midway(logs, 'gyroscope', 1, gyro_spike)
+    count = 1 if scenario == Scenario.GYRO_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT
+    logs = modify_logs_midway(logs, 'gyroscope', count, gyro_spike)
 
   elif scenario == Scenario.ACCEL_OFF:
     logs = sorted([x for x in logs if x.which() != 'accelerometer'], key=lambda x: x.logMonoTime)
 
-  elif scenario == Scenario.ACCEL_SPIKE_MIDWAY:
+  elif scenario == Scenario.ACCEL_SPIKE_MIDWAY or scenario == Scenario.ACCEL_CONSISTENT_SPIKES:
     def acc_spike(msg):
-      msg.accelerometer.acceleration.v[0] += 10.0
+      msg.accelerometer.acceleration.v[0] += 100.0
-    logs = modify_logs_midway(logs, 'accelerometer', 1, acc_spike)
+    count = 1 if scenario == Scenario.ACCEL_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT
+    logs = modify_logs_midway(logs, 'accelerometer', count, acc_spike)
 
   elif scenario == Scenario.SENSOR_TIMING_SPIKE_MIDWAY or scenario == Scenario.SENSOR_TIMING_CONSISTENT_SPIKES:
     def timing_spike(msg):
@@ -121,7 +125,7 @@ class TestLocationdScenarios:
     assert np.allclose(replayed_data['roll'], 0.0)
     assert np.all(replayed_data['sensors_flag'] == 0.0)
 
-  def test_gyro_spikes(self):
+  def test_gyro_spike(self):
     """
     Test: a gyroscope spike in the middle of the segment
     Expected Result:
@@ -132,8 +136,17 @@ class TestLocationdScenarios:
     orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs)
     assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
     assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
-    assert np.diff(replayed_data['inputs_flag'])[499] == -1.0
+    assert np.all(replayed_data['inputs_flag'] == orig_data['inputs_flag'])
-    assert np.diff(replayed_data['inputs_flag'])[704] == 1.0
+    assert np.all(replayed_data['sensors_flag'] == orig_data['sensors_flag'])
+
+  def test_consistent_gyro_spikes(self):
+    """
+    Test: consistent timing spikes for N gyroscope messages in the middle of the segment
+    Expected Result: inputsOK becomes False after N of bad measurements
+    """
+    orig_data, replayed_data = run_scenarios(Scenario.GYRO_CONSISTENT_SPIKES, self.logs)
+    assert np.diff(replayed_data['inputs_flag'])[501] == -1.0
+    assert np.diff(replayed_data['inputs_flag'])[708] == 1.0
 
   def test_accel_off(self):
     """
@@ -148,7 +161,7 @@ class TestLocationdScenarios:
     assert np.allclose(replayed_data['roll'], 0.0)
     assert np.all(replayed_data['sensors_flag'] == 0.0)
 
-  def test_accel_spikes(self):
+  def test_accel_spike(self):
     """
     ToDo:
     Test: an accelerometer spike in the middle of the segment
@@ -173,5 +186,5 @@ class TestLocationdScenarios:
     Expected Result: inputsOK becomes False after N of bad measurements
     """
     orig_data, replayed_data = run_scenarios(Scenario.SENSOR_TIMING_CONSISTENT_SPIKES, self.logs)
-    assert np.diff(replayed_data['inputs_flag'])[500] == -1.0
+    assert np.diff(replayed_data['inputs_flag'])[501] == -1.0
-    assert np.diff(replayed_data['inputs_flag'])[787] == 1.0
+    assert np.diff(replayed_data['inputs_flag'])[707] == 1.0