|
|
|
@ -50,7 +50,7 @@ MOVING_CORR_WINDOW = 30 |
|
|
|
|
OVERLAP_FACTOR = 0.25 |
|
|
|
|
|
|
|
|
|
class LagEstimator(ParameterEstimator): |
|
|
|
|
def __init__(self, CP, dt, min_hist_len_sec, max_hist_len_sec, max_lag_hist_len_sec, moving_corr_window, overlap_factor): |
|
|
|
|
def __init__(self, CP, dt, min_hist_len_sec, max_lag_hist_len_sec, moving_corr_window, overlap_factor): |
|
|
|
|
self.dt = dt |
|
|
|
|
self.min_hist_len = int(min_hist_len_sec / self.dt) |
|
|
|
|
self.window_len = int(moving_corr_window / self.dt) |
|
|
|
@ -61,8 +61,8 @@ class LagEstimator(ParameterEstimator): |
|
|
|
|
self.steering_pressed = False |
|
|
|
|
self.v_ego = 0.0 |
|
|
|
|
self.lags = deque(maxlen= int(max_lag_hist_len_sec / (moving_corr_window * overlap_factor))) |
|
|
|
|
self.curvature = deque(maxlen=int(max_hist_len_sec / self.dt)) |
|
|
|
|
self.desired_curvature = deque(maxlen=int(max_hist_len_sec / self.dt)) |
|
|
|
|
self.curvature = deque(maxlen=int(moving_corr_window / self.dt)) |
|
|
|
|
self.desired_curvature = deque(maxlen=int(moving_corr_window / self.dt)) |
|
|
|
|
self.frame = 0 |
|
|
|
|
|
|
|
|
|
def correlation_lags(self, sig_len, dt): |
|
|
|
@ -102,7 +102,7 @@ class LagEstimator(ParameterEstimator): |
|
|
|
|
if self.frame % int(self.window_len * OVERLAP_FACTOR) == 0: |
|
|
|
|
_, curvature = zip(*self.curvature) |
|
|
|
|
_, desired_curvature = zip(*self.desired_curvature) |
|
|
|
|
delay_curvature, _ = self.actuator_delay(curvature[-self.window_len:], desired_curvature[-self.window_len:], self.dt) |
|
|
|
|
delay_curvature, _ = self.actuator_delay(curvature, desired_curvature, self.dt) |
|
|
|
|
if delay_curvature != 0.0: |
|
|
|
|
self.lags.append(delay_curvature) |
|
|
|
|
# FIXME: this is fragile and ugly, refactor this |
|
|
|
@ -332,7 +332,7 @@ def main(demo=False): |
|
|
|
|
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) |
|
|
|
|
estimator = TorqueEstimator(CP) |
|
|
|
|
|
|
|
|
|
lag_estimator = LagEstimator(CP, DT_MDL, MIN_HIST_LEN_SEC, MAX_HIST_LEN_SEC, MAX_LAG_HIST_LEN_SEC, MOVING_CORR_WINDOW, OVERLAP_FACTOR) |
|
|
|
|
lag_estimator = LagEstimator(CP, DT_MDL, MIN_HIST_LEN_SEC, MAX_LAG_HIST_LEN_SEC, MOVING_CORR_WINDOW, OVERLAP_FACTOR) |
|
|
|
|
|
|
|
|
|
while True: |
|
|
|
|
sm.update() |
|
|
|
|