locationd: cam odo delay compensation (#37543)

* Delay compensation for camera odomtry

* Frame skip definition

* CAM_ODO_POSE_DELAY const

* Remove import

* Use timestampEof

* CAM_ODO_STD_MULT

* locationd processing_time=0.01

* Update angular velocity Q

* Try 075

* Acc obs std 0.75

* Adjust Cam odo std mults

* More tweaking

* Smoothing in lld tests

* Comment

* Remove import

* Revert gyro bias P update

* Tweak to 0.75

selfdrive/locationd/locationd.py

@@ -28,6 +28,9 @@ INPUT_INVALID_LIMIT = 2.0 # 1 (camodo) / 9 (sensor) bad input[s] ignored
 INPUT_INVALID_RECOVERY = 10.0 # ~10 secs to resume after exceeding allowed bad inputs by one
 POSENET_STD_INITIAL_VALUE = 10.0
 POSENET_STD_HIST_HALF = 20
+CAM_ODO_POSE_DELAY = 0.1 # dependent on the vision model context frames and temporal frequency (current model is 5 fps with 2 context frames)
+CAM_ODO_ROT_STD_MULT = 10
+CAM_ODO_TRANS_STD_MULT = 4
 
 
 def calculate_invalid_input_decay(invalid_limit, recovery_time, frequency):
@@ -155,6 +158,8 @@ class LocationEstimator:
         self.device_from_calib = rot_from_euler(calib)
 
     elif which == "cameraOdometry":
+      # camera odometry is delayed depending on the model context frames and temporal frequency
+      t = msg.timestampEof * 1e-9 - CAM_ODO_POSE_DELAY
       if not self._validate_timestamp(t):
         return HandleLogResult.TIMING_INVALID
 
@@ -177,8 +182,8 @@ class LocationEstimator:
       self.posenet_stds[-1] = trans_calib_std[0]
 
       # Multiply by N to avoid to high certainty in kalman filter because of temporally correlated noise
-      rot_calib_std *= 10
-      trans_calib_std *= 2
+      rot_calib_std *= CAM_ODO_ROT_STD_MULT
+      trans_calib_std *= CAM_ODO_TRANS_STD_MULT
 
       rot_device_std = rotate_std(self.device_from_calib, rot_calib_std)
       trans_device_std = rotate_std(self.device_from_calib, trans_calib_std)

selfdrive/locationd/models/pose_kf.py

@@ -47,13 +47,13 @@ class PoseKalman(KalmanFilter):
   # process noise
   Q = np.diag([0.001**2, 0.001**2, 0.001**2,
                0.01**2, 0.01**2, 0.01**2,
-               0.1**2, 0.1**2, 0.1**2,
+               0.085**2, 0.085**2, 0.085**2,
                (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
                3**2, 3**2, 3**2,
                0.005**2, 0.005**2, 0.005**2])
 
   obs_noise = {ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]),
-               ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]),
+               ObservationKind.PHONE_ACCEL: np.diag([0.75**2, 0.75**2, 0.75**2]),
                ObservationKind.CAMERA_ODO_TRANSLATION: np.diag([0.5**2, 0.5**2, 0.5**2]),
                ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2])}
 

selfdrive/locationd/test/test_locationd_scenarios.py

@@ -3,6 +3,7 @@ from collections import defaultdict
 from enum import Enum
 
 from openpilot.tools.lib.logreader import LogReader
+from openpilot.selfdrive.locationd.lagd import masked_symmetric_moving_average
 from openpilot.selfdrive.test.process_replay.migration import migrate_all
 from openpilot.selfdrive.test.process_replay.process_replay import replay_process_with_name
 
@@ -15,6 +16,7 @@ SELECT_COMPARE_FIELDS = {
   'inputs_flag': ['inputsOK'],
   'sensors_flag': ['sensorsOK'],
 }
+SMOOTH_FIELDS = ['yaw_rate', 'roll']
 JUNK_IDX = 100
 CONSISTENT_SPIKES_COUNT = 10
 
@@ -32,6 +34,8 @@ class Scenario(Enum):
 
 
 def get_select_fields_data(logs):
+  def sig_smooth(signal):
+    return masked_symmetric_moving_average(signal, np.ones_like(signal), 5, 1.0)
   def get_nested_keys(msg, keys):
     val = None
     for key in keys:
@@ -44,6 +48,8 @@ def get_select_fields_data(logs):
       data[key].append(get_nested_keys(msg, fields))
   for key in data:
     data[key] = np.array(data[key][JUNK_IDX:], dtype=float)
+    if key in SMOOTH_FIELDS:
+      data[key] = sig_smooth(data[key])
   return data
 
 
@@ -110,7 +116,7 @@ class TestLocationdScenarios:
     """
     orig_data, replayed_data = run_scenarios(Scenario.BASE, 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.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.35))
 
   def test_gyro_off(self):
     """
@@ -135,7 +141,7 @@ 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.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.35))
     assert np.all(replayed_data['inputs_flag'] == orig_data['inputs_flag'])
     assert np.all(replayed_data['sensors_flag'] == orig_data['sensors_flag'])
 
@@ -169,7 +175,7 @@ class TestLocationdScenarios:
     """
     orig_data, replayed_data = run_scenarios(Scenario.ACCEL_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.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.35))
 
   def test_single_timing_spike(self):
     """

selfdrive/test/process_replay/process_replay.py

@@ -509,6 +509,7 @@ CONFIGS = [
     ignore=["logMonoTime"],
     should_recv_callback=MessageBasedRcvCallback("cameraOdometry"),
     tolerance=NUMPY_TOLERANCE,
+    processing_time=0.01,
   ),
   ProcessConfig(
     proc_name="paramsd",