From 1c1437579638939f2741dbd89ba84240c1c6f051 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Thu, 19 Mar 2026 19:35:10 -0700 Subject: [PATCH] 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 | 9 +++++++-- selfdrive/locationd/models/pose_kf.py | 4 ++-- selfdrive/locationd/test/test_locationd_scenarios.py | 12 +++++++++--- selfdrive/test/process_replay/process_replay.py | 1 + 4 files changed, 19 insertions(+), 7 deletions(-) diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index f6a0935ed9..a34a584ff3 100755 --- a/selfdrive/locationd/locationd.py +++ b/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) diff --git a/selfdrive/locationd/models/pose_kf.py b/selfdrive/locationd/models/pose_kf.py index 020e51ad6e..a8ff80c713 100755 --- a/selfdrive/locationd/models/pose_kf.py +++ b/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])} diff --git a/selfdrive/locationd/test/test_locationd_scenarios.py b/selfdrive/locationd/test/test_locationd_scenarios.py index 0ea7ac183f..69f2ca2821 100644 --- a/selfdrive/locationd/test/test_locationd_scenarios.py +++ b/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): """ diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index d168a7e800..5e9b2e742c 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/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",