From b15ecfbce36ea9c1519c236d2e7b73f53e40e6e4 Mon Sep 17 00:00:00 2001 From: ajouatom Date: Fri, 20 Mar 2026 13:33:39 +0900 Subject: [PATCH] locationd: cam odo delay compensation (#37543) --- rednose_repo/rednose/helpers/kalmanfilter.py | 2 +- selfdrive/locationd/locationd.py | 93 +++----------------- selfdrive/locationd/models/pose_kf.py | 4 +- 3 files changed, 13 insertions(+), 86 deletions(-) diff --git a/rednose_repo/rednose/helpers/kalmanfilter.py b/rednose_repo/rednose/helpers/kalmanfilter.py index af2c9f300..de0e3dcc9 100644 --- a/rednose_repo/rednose/helpers/kalmanfilter.py +++ b/rednose_repo/rednose/helpers/kalmanfilter.py @@ -49,4 +49,4 @@ class KalmanFilter: if R is None: R = self.get_R(kind, len(data)) - self.filter.predict_and_update_batch(t, kind, data, R) + return self.filter.predict_and_update_batch(t, kind, data, R) diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index d20ea5a86..a34a584ff 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) @@ -285,52 +290,11 @@ def main(): P_initial = np.diag(np.array(filter_state.std, dtype=np.float64)) if len(filter_state.std) != 0 else PoseKalman.initial_P estimator.reset(None, x_initial, P_initial) - last_fail_print_t = 0.0 - last_input_fail_print_t = 0.0 - show_debug = True while True: sm.update() acc_msgs, gyro_msgs = (messaging.drain_sock(sock) for sock in sensor_sockets) - ok_alive = sm.all_alive() - ok_freq = sm.all_freq_ok() - ok_valid = sm.all_valid() - ok_sm = ok_alive and ok_freq and ok_valid - ok_sensor = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) - - if not filter_initialized: - filter_initialized = ok_sm and ok_sensor - if show_debug and (not filter_initialized): - now = time.monotonic() - if now - last_fail_print_t > 1.0: - last_fail_print_t = now - print(f"\n[locationd init waiting] frame={sm.frame} " - f"sm={ok_sm} alive={ok_alive} freq={ok_freq} valid={ok_valid} sensor={ok_sensor}") - - for s in sm.services: - recv_age_ms = (now - sm.recv_time[s]) * 1000.0 if sm.recv_time[s] > 0 else -1.0 - print( - f" SM {s:16s} " - f"seen={sm.seen[s]} " - f"updated={sm.updated[s]} " - f"alive={sm.alive[s]} " - f"freq_ok={sm.freq_ok[s]} " - f"valid={sm.valid[s]} " - f"recv_age_ms={recv_age_ms:7.1f} " - f"logMonoTime={sm.logMonoTime[s]}" - ) - - for s in ['accelerometer', 'gyroscope']: - recv_age_ms = (now - sensor_recv_time[s]) * 1000.0 if sensor_recv_time[s] > 0 else -1.0 - print( - f" SNS {s:16s} " - f"alive={sensor_alive[s]} " - f"valid={sensor_valid[s]} " - f"recv_age_ms={recv_age_ms:7.1f} " - f"drain_cnt={len(acc_msgs) if s == 'accelerometer' else len(gyro_msgs)}" - ) - if filter_initialized: msgs = [] for msg in acc_msgs + gyro_msgs: @@ -357,54 +321,17 @@ def main(): observation_input_invalid[which] += 1 elif res == HandleLogResult.SUCCESS: observation_input_invalid[which] *= input_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[s] for s in critcal_services) inputs_valid = sm.all_valid() and critical_service_inputs_valid sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) - now = time.monotonic() - if show_debug and ((not ok_sm) or (not sensors_valid) or (not critical_service_inputs_valid) or (not inputs_valid)) and (now - last_input_fail_print_t > 1.0): - last_input_fail_print_t = now - - print(f"\n[locationd output invalid] frame={sm.frame} " - f"sm={ok_sm} alive={ok_alive} freq={ok_freq} valid={ok_valid} " - f"sensors_valid={sensors_valid} critical_inputs_valid={critical_service_inputs_valid} " - f"inputs_valid={inputs_valid} filter_initialized={filter_initialized}") - - for s in sm.services: - recv_age_ms = (now - sm.recv_time[s]) * 1000.0 if sm.recv_time[s] > 0 else -1.0 - print( - f" SM {s:16s} " - f"seen={sm.seen[s]} " - f"updated={sm.updated[s]} " - f"alive={sm.alive[s]} " - f"freq_ok={sm.freq_ok[s]} " - f"valid={sm.valid[s]} " - f"recv_age_ms={recv_age_ms:7.1f} " - f"logMonoTime={sm.logMonoTime[s]}" - ) - - for s in ['accelerometer', 'gyroscope']: - recv_age_ms = (now - sensor_recv_time[s]) * 1000.0 if sensor_recv_time[s] > 0 else -1.0 - print( - f" SNS {s:16s} " - f"alive={sensor_alive[s]} " - f"valid={sensor_valid[s]} " - f"recv_age_ms={recv_age_ms:7.1f} " - f"drain_cnt={len(acc_msgs) if s == 'accelerometer' else len(gyro_msgs)}" - ) - - for s in critcal_services: - print( - f" CRIT {s:16s} " - f"invalid_score={observation_input_invalid[s]:8.3f} " - f"threshold={input_invalid_threshold[s]:8.3f} " - f"limit={input_invalid_limit[s]:8.3f}" - ) - msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized) pm.send("livePose", msg) + if __name__ == "__main__": main() diff --git a/selfdrive/locationd/models/pose_kf.py b/selfdrive/locationd/models/pose_kf.py index 020e51ad6..a8ff80c71 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])}