mawei/carrotpilot
1
0
Fork 0
mirror of https://github.com/ajouatom/openpilot.git synced 2026-06-08 02:54:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

locationd: cam odo delay compensation (#37543)

Browse Source
This commit is contained in:
ajouatom
2026-03-20 13:33:39 +09:00
parent b3b3b66e95
commit b15ecfbce3
3 changed files with 13 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
rednose_repo/rednose/helpers/kalmanfilter.py
View File

@@ -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)

93
selfdrive/locationd/locationd.py
View File

@@ -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()

4
selfdrive/locationd/models/pose_kf.py
View File

@@ -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])}