diff --git a/cereal/log.capnp b/cereal/log.capnp index 797c2cd8ac..593399f832 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2278,21 +2278,6 @@ struct LiveTorqueParametersData { useParams @12 :Bool; } -struct LiveDelayData { - lateralDelay @0 :Float32; - validBlocks @1 :Int32; - status @2 :Status; - - lateralDelayEstimate @3 :Float32; - points @4 :List(Float32); - - enum Status { - unestimated @0; - estimated @1; - invalid @2; - } -} - struct LiveMapDataDEPRECATED { speedLimitValid @0 :Bool; speedLimit @1 :Float32; @@ -2523,7 +2508,6 @@ struct Event { gnssMeasurements @91 :GnssMeasurements; liveParameters @61 :LiveParametersData; liveTorqueParameters @94 :LiveTorqueParametersData; - liveDelay @146 : LiveDelayData; cameraOdometry @63 :CameraOdometry; thumbnail @66: Thumbnail; onroadEvents @134: List(OnroadEvent); diff --git a/cereal/services.py b/cereal/services.py index 82fc04bd00..aad83177bb 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -36,7 +36,6 @@ _services: dict[str, tuple] = { "errorLogMessage": (True, 0., 1), "liveCalibration": (True, 4., 4), "liveTorqueParameters": (True, 4., 1), - "liveDelay": (True, 4., 1), "androidLog": (True, 0.), "carState": (True, 100., 10), "carControl": (True, 100., 10), diff --git a/common/params_keys.h b/common/params_keys.h index c7f0578430..2b540b744c 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -71,7 +71,6 @@ inline static std::unordered_map keys = { {"LastPowerDropDetected", CLEAR_ON_MANAGER_START}, {"LastUpdateException", CLEAR_ON_MANAGER_START}, {"LastUpdateTime", PERSISTENT}, - {"LiveLag", PERSISTENT}, {"LiveParameters", PERSISTENT}, {"LiveTorqueParameters", PERSISTENT | DONT_LOG}, {"LocationFilterInitialState", PERSISTENT}, diff --git a/selfdrive/locationd/estimators/__init__.py b/selfdrive/locationd/estimators/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/selfdrive/locationd/estimators/lateral_lag.py b/selfdrive/locationd/estimators/lateral_lag.py deleted file mode 100644 index 87f0b123a7..0000000000 --- a/selfdrive/locationd/estimators/lateral_lag.py +++ /dev/null @@ -1,273 +0,0 @@ -import numpy as np -from collections import deque -from functools import partial - -import cereal.messaging as messaging -from cereal import log -from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose - -BLOCK_SIZE = 100 -BLOCK_NUM = 50 -BLOCK_NUM_NEEDED = 5 -MOVING_WINDOW_SEC = 300.0 -MIN_OKAY_WINDOW_SEC = 30.0 -MIN_RECOVERY_BUFFER_SEC = 2.0 -MIN_VEGO = 15.0 -MIN_ABS_YAW_RATE = np.radians(1.0) -MIN_NCC = 0.95 - - -def parabolic_peak_interp(R, max_index): - if max_index == 0 or max_index == len(R) - 1: - return max_index - - y_m1, y_0, y_p1 = R[max_index - 1], R[max_index], R[max_index + 1] - offset = 0.5 * (y_p1 - y_m1) / (2 * y_0 - y_p1 - y_m1) - - return max_index + offset - - -def masked_normalized_cross_correlation(expected_sig, actual_sig, mask): - """ - References: - D. Padfield. "Masked FFT registration". In Proc. Computer Vision and - Pattern Recognition, pp. 2918-2925 (2010). - :DOI:`10.1109/CVPR.2010.5540032` - """ - - eps = np.finfo(np.float64).eps - expected_sig = np.asarray(expected_sig, dtype=np.float64) - actual_sig = np.asarray(actual_sig, dtype=np.float64) - - expected_sig[~mask] = 0.0 - actual_sig[~mask] = 0.0 - - rotated_expected_sig = expected_sig[::-1] - rotated_mask = mask[::-1] - - n = len(expected_sig) + len(actual_sig) - 1 - fft = partial(np.fft.fft, n=n) - - actual_sig_fft = fft(actual_sig) - rotated_expected_sig_fft = fft(rotated_expected_sig) - actual_mask_fft = fft(mask.astype(np.float64)) - rotated_mask_fft = fft(rotated_mask.astype(np.float64)) - - number_overlap_masked_samples = np.fft.ifft(rotated_mask_fft * actual_mask_fft).real - number_overlap_masked_samples[:] = np.round(number_overlap_masked_samples) - number_overlap_masked_samples[:] = np.fmax(number_overlap_masked_samples, eps) - masked_correlated_actual_fft = np.fft.ifft(rotated_mask_fft * actual_sig_fft).real - masked_correlated_expected_fft = np.fft.ifft(actual_mask_fft * rotated_expected_sig_fft).real - - numerator = np.fft.ifft(rotated_expected_sig_fft * actual_sig_fft).real - numerator -= masked_correlated_actual_fft * masked_correlated_expected_fft / number_overlap_masked_samples - - actual_squared_fft = fft(actual_sig ** 2) - actual_sig_denom = np.fft.ifft(rotated_mask_fft * actual_squared_fft).real - actual_sig_denom -= masked_correlated_actual_fft ** 2 / number_overlap_masked_samples - actual_sig_denom[:] = np.fmax(actual_sig_denom, 0.0) - - rotated_expected_squared_fft = fft(rotated_expected_sig ** 2) - expected_sig_denom = np.fft.ifft(actual_mask_fft * rotated_expected_squared_fft).real - expected_sig_denom -= masked_correlated_expected_fft ** 2 / number_overlap_masked_samples - expected_sig_denom[:] = np.fmax(expected_sig_denom, 0.0) - - denom = np.sqrt(actual_sig_denom * expected_sig_denom) - - # zero-out samples with very small denominators - tol = 1e3 * eps * np.max(np.abs(denom), keepdims=True) - nonzero_indices = denom > tol - - ncc = np.zeros_like(denom, dtype=np.float64) - ncc[nonzero_indices] = numerator[nonzero_indices] / denom[nonzero_indices] - np.clip(ncc, -1, 1, out=ncc) - - return ncc - - -class Points: - def __init__(self, num_points): - self.times = deque(maxlen=num_points) - self.okay = deque(maxlen=num_points) - self.desired = deque(maxlen=num_points) - self.actual = deque(maxlen=num_points) - - @property - def num_points(self): - return len(self.desired) - - @property - def num_okay(self): - return np.count_nonzero(self.okay) - - def update(self, t, desired, actual, okay): - self.times.append(t) - self.okay.append(okay) - self.desired.append(desired) - self.actual.append(actual) - - def get(self): - return np.array(self.times), np.array(self.desired), np.array(self.actual), np.array(self.okay) - - -class BlockAverage: - def __init__(self, num_blocks, block_size, valid_blocks, initial_value): - self.num_blocks = num_blocks - self.block_size = block_size - self.block_idx = valid_blocks % block_size - self.idx = 0 - - self.values = np.tile(initial_value, (num_blocks, 1)) - self.valid_blocks = valid_blocks - - def update(self, value): - self.values[self.block_idx] = (self.idx * self.values[self.block_idx] + (self.block_size - self.idx) * value) / self.block_size - self.idx = (self.idx + 1) % self.block_size - if self.idx == 0: - self.block_idx = (self.block_idx + 1) % self.num_blocks - self.valid_blocks = min(self.valid_blocks + 1, self.num_blocks) - - def get(self): - valid_block_idx = [i for i in range(self.valid_blocks) if i != self.block_idx] - if not valid_block_idx: - return None - return float(np.mean(self.values[valid_block_idx], axis=0).item()) - - -class LateralLagEstimator: - inputs = {"carControl", "carState", "controlsState", "liveCalibration", "livePose"} - - def __init__(self, CP, dt, - block_count=BLOCK_NUM, min_valid_block_count=BLOCK_NUM_NEEDED, block_size=BLOCK_SIZE, - window_sec=MOVING_WINDOW_SEC, okay_window_sec=MIN_OKAY_WINDOW_SEC, min_recovery_buffer_sec=MIN_RECOVERY_BUFFER_SEC, - min_vego=MIN_VEGO, min_yr=MIN_ABS_YAW_RATE, min_ncc=MIN_NCC): - self.dt = dt - self.window_sec = window_sec - self.okay_window_sec = okay_window_sec - self.min_recovery_buffer_sec = min_recovery_buffer_sec - self.initial_lag = CP.steerActuatorDelay + 0.2 - self.block_size = block_size - self.block_count = block_count - self.min_valid_block_count = min_valid_block_count - self.min_vego = min_vego - self.min_yr = min_yr - self.min_ncc = min_ncc - - self.t = 0 - self.lat_active = False - self.steering_pressed = False - self.steering_saturated = False - self.desired_curvature = 0 - self.v_ego = 0 - self.yaw_rate = 0 - - self.last_lat_inactive_t = 0 - self.last_steering_pressed_t = 0 - self.last_steering_saturated_t = 0 - self.last_estimate_t = 0 - - self.calibrator = PoseCalibrator() - - self.reset(self.initial_lag, 0) - - def reset(self, initial_lag, valid_blocks): - window_len = int(self.window_sec / self.dt) - self.points = Points(window_len) - self.block_avg = BlockAverage(self.block_count, self.block_size, valid_blocks, initial_lag) - - def get_msg(self, valid, debug=False): - msg = messaging.new_message('liveDelay') - - msg.valid = valid - - liveDelay = msg.liveDelay - - estimated_lag = self.block_avg.get() - liveDelay.lateralDelayEstimate = estimated_lag or self.initial_lag - if self.block_avg.valid_blocks >= self.min_valid_block_count and estimated_lag is not None: - liveDelay.status = log.LiveDelayData.Status.estimated - liveDelay.lateralDelay = estimated_lag - else: - liveDelay.status = log.LiveDelayData.Status.unestimated - liveDelay.lateralDelay = self.initial_lag - liveDelay.validBlocks = self.block_avg.valid_blocks - if debug: - liveDelay.points = self.block_avg.values.flatten().tolist() - - return msg - - def handle_log(self, t, which, msg): - if which == "carControl": - self.lat_active = msg.latActive - elif which == "carState": - self.steering_pressed = msg.steeringPressed - self.v_ego = msg.vEgo - elif which == "controlsState": - self.steering_saturated = getattr(msg.lateralControlState, msg.lateralControlState.which()).saturated - self.desired_curvature = msg.desiredCurvature - elif which == "liveCalibration": - self.calibrator.feed_live_calib(msg) - elif which == "livePose": - device_pose = Pose.from_live_pose(msg) - calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) - self.yaw_rate = calibrated_pose.angular_velocity.z - self.t = t - - def points_valid(self): - return self.points.num_okay >= int(self.okay_window_sec / self.dt) - - def update_points(self): - if not self.lat_active: - self.last_lat_inactive_t = self.t - if self.steering_pressed: - self.last_steering_pressed_t = self.t - if self.steering_saturated: - self.last_steering_saturated_t = self.t - - la_desired = self.desired_curvature * self.v_ego * self.v_ego - la_actual_pose = self.yaw_rate * self.v_ego - - fast = self.v_ego > self.min_vego - turning = np.abs(self.yaw_rate) >= self.min_yr - has_recovered = all( # wait for recovery after !lat_active, steering_pressed, steering_saturated - self.t - last_t >= self.min_recovery_buffer_sec - for last_t in [self.last_lat_inactive_t, self.last_steering_pressed_t, self.last_steering_saturated_t] - ) - okay = self.lat_active and not self.steering_pressed and not self.steering_saturated and fast and turning and has_recovered - - self.points.update(self.t, la_desired, la_actual_pose, okay) - - def update_estimate(self): - # check if the points are valid overall - if not self.points_valid(): - return - - times, desired, actual, okay = self.points.get() - # check if there are any new valid data points since the last update - if self.last_estimate_t != 0 and times[0] <= self.last_estimate_t: - new_values_start_idx = next(-i for i, t in enumerate(reversed(times)) if t <= self.last_estimate_t) - if (new_values_start_idx == 0 or not np.any(okay[new_values_start_idx:])): - return - - delay, corr = self.actuator_delay(desired, actual, okay, self.dt) - if corr < self.min_ncc: - return - - self.block_avg.update(delay) - self.last_estimate_t = self.t - - def correlation_lags(self, sig_len, dt): - return np.arange(0, sig_len) * dt - - def actuator_delay(self, expected_sig, actual_sig, mask, dt, max_lag=1.): - ncc = masked_normalized_cross_correlation(expected_sig, actual_sig, mask) - - # only consider lags from 0 to max_lag - max_lag_samples = int(max_lag / dt) - roi_ncc = ncc[len(expected_sig) - 1: len(expected_sig) - 1 + max_lag_samples] - - max_corr_index = np.argmax(roi_ncc) - corr = roi_ncc[max_corr_index] - lag = parabolic_peak_interp(roi_ncc, max_corr_index) * dt - - return lag, corr diff --git a/selfdrive/locationd/estimators/vehicle_params.py b/selfdrive/locationd/estimators/vehicle_params.py deleted file mode 100644 index 025d28dee0..0000000000 --- a/selfdrive/locationd/estimators/vehicle_params.py +++ /dev/null @@ -1,197 +0,0 @@ -import numpy as np -import capnp - -import cereal.messaging as messaging -from cereal import car, log -from openpilot.common.realtime import DT_MDL -from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States -from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR -from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose - -MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s -ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits -ROLL_MIN, ROLL_MAX = np.radians(-10), np.radians(10) -ROLL_LOWERED_MAX = np.radians(8) -ROLL_STD_MAX = np.radians(1.5) -LATERAL_ACC_SENSOR_THRESHOLD = 4.0 -OFFSET_MAX = 10.0 -OFFSET_LOWERED_MAX = 8.0 -MIN_ACTIVE_SPEED = 1.0 -LOW_ACTIVE_SPEED = 10.0 - - -def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: float, lowered_threshold: float): - if current_valid: - current_valid = abs(val) < threshold - else: - current_valid = abs(val) < lowered_threshold - return current_valid - - -class VehicleParamsLearner: - inputs = {'carState', 'liveCalibration', 'livePose'} - - def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None): - self.kf = CarKalman(GENERATED_DIR) - - self.x_initial = CarKalman.initial_x.copy() - self.x_initial[States.STEER_RATIO] = steer_ratio - self.x_initial[States.STIFFNESS] = stiffness_factor - self.x_initial[States.ANGLE_OFFSET] = angle_offset - self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial - - self.kf.set_globals( - mass=CP.mass, - rotational_inertia=CP.rotationalInertia, - center_to_front=CP.centerToFront, - center_to_rear=CP.wheelbase - CP.centerToFront, - stiffness_front=CP.tireStiffnessFront, - stiffness_rear=CP.tireStiffnessRear - ) - - self.min_sr, self.max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio - - self.calibrator = PoseCalibrator() - - self.observed_speed = 0.0 - self.observed_yaw_rate = 0.0 - self.observed_roll = 0.0 - - self.avg_offset_valid = True - self.total_offset_valid = True - self.roll_valid = True - - self.reset(None) - - def reset(self, t: float | None): - self.kf.init_state(self.x_initial, covs=self.P_initial, filter_time=t) - - self.angle_offset, self.roll, self.active = np.degrees(self.x_initial[States.ANGLE_OFFSET].item()), 0.0, False - self.avg_angle_offset = self.angle_offset - - def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader): - if which == 'livePose': - device_pose = Pose.from_live_pose(msg) - calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) - - yaw_rate, yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std - yaw_rate_valid = msg.angularVelocityDevice.valid - yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10 # rad/s - yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1 # rad/s - if not yaw_rate_valid: - # This is done to bound the yaw rate estimate when localizer values are invalid or calibrating - yaw_rate, yaw_rate_std = 0.0, np.radians(10.0) - self.observed_yaw_rate = yaw_rate - - localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std - localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std - roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK - if roll_valid: - roll = localizer_roll - # Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar? - roll_std = 2 * localizer_roll_std - else: - # This is done to bound the road roll estimate when localizer values are invalid - roll = 0.0 - roll_std = np.radians(10.0) - self.observed_roll = np.clip(roll, self.observed_roll - ROLL_MAX_DELTA, self.observed_roll + ROLL_MAX_DELTA) - - if self.active: - if msg.posenetOK: - self.kf.predict_and_observe(t, - ObservationKind.ROAD_FRAME_YAW_RATE, - np.array([[-self.observed_yaw_rate]]), - np.array([np.atleast_2d(yaw_rate_std**2)])) - - self.kf.predict_and_observe(t, - ObservationKind.ROAD_ROLL, - np.array([[self.observed_roll]]), - np.array([np.atleast_2d(roll_std**2)])) - self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]])) - - # We observe the current stiffness and steer ratio (with a high observation noise) to bound - # the respective estimate STD. Otherwise the STDs keep increasing, causing rapid changes in the - # states in longer routes (especially straight stretches). - stiffness = float(self.kf.x[States.STIFFNESS].item()) - steer_ratio = float(self.kf.x[States.STEER_RATIO].item()) - self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]])) - self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]])) - - elif which == 'liveCalibration': - self.calibrator.feed_live_calib(msg) - - elif which == 'carState': - steering_angle = msg.steeringAngleDeg - - in_linear_region = abs(steering_angle) < 45 - self.observed_speed = msg.vEgo - self.active = self.observed_speed > MIN_ACTIVE_SPEED and in_linear_region - - if self.active: - self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[np.radians(steering_angle)]])) - self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.observed_speed]])) - - if not self.active: - # Reset time when stopped so uncertainty doesn't grow - self.kf.filter.set_filter_time(t) # type: ignore - self.kf.filter.reset_rewind() # type: ignore - - def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder: - x = self.kf.x - P = np.sqrt(self.kf.P.diagonal()) - if not np.all(np.isfinite(x)): - cloudlog.error("NaN in liveParameters estimate. Resetting to default values") - self.reset(self.kf.t) - x = self.kf.x - - self.avg_angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item()), - self.avg_angle_offset - MAX_ANGLE_OFFSET_DELTA, self.avg_angle_offset + MAX_ANGLE_OFFSET_DELTA) - self.angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()), - self.angle_offset - MAX_ANGLE_OFFSET_DELTA, self.angle_offset + MAX_ANGLE_OFFSET_DELTA) - self.roll = np.clip(float(x[States.ROAD_ROLL].item()), self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA) - roll_std = float(P[States.ROAD_ROLL].item()) - if self.active and self.observed_speed > LOW_ACTIVE_SPEED: - # Account for the opposite signs of the yaw rates - # At low speeds, bumping into a curb can cause the yaw rate to be very high - sensors_valid = bool(abs(self.observed_speed * (x[States.YAW_RATE].item() + self.observed_yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD) - else: - sensors_valid = True - self.avg_offset_valid = check_valid_with_hysteresis(self.avg_offset_valid, self.avg_angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX) - self.total_offset_valid = check_valid_with_hysteresis(self.total_offset_valid, self.angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX) - self.roll_valid = check_valid_with_hysteresis(self.roll_valid, self.roll, ROLL_MAX, ROLL_LOWERED_MAX) - - msg = messaging.new_message('liveParameters') - - msg.valid = valid - - liveParameters = msg.liveParameters - liveParameters.posenetValid = True - liveParameters.sensorValid = sensors_valid - liveParameters.steerRatio = float(x[States.STEER_RATIO].item()) - liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item()) - liveParameters.roll = float(self.roll) - liveParameters.angleOffsetAverageDeg = float(self.avg_angle_offset) - liveParameters.angleOffsetDeg = float(self.angle_offset) - liveParameters.steerRatioValid = self.min_sr <= liveParameters.steerRatio <= self.max_sr - liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0 - liveParameters.angleOffsetAverageValid = bool(self.avg_offset_valid) - liveParameters.angleOffsetValid = bool(self.total_offset_valid) - liveParameters.valid = all(( - liveParameters.angleOffsetAverageValid, - liveParameters.angleOffsetValid , - self.roll_valid, - roll_std < ROLL_STD_MAX, - liveParameters.stiffnessFactorValid, - liveParameters.steerRatioValid, - )) - liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item()) - liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item()) - liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item()) - liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item()) - if debug: - liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message() - liveParameters.debugFilterState.value = x.tolist() - liveParameters.debugFilterState.std = P.tolist() - - return msg diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 524319fdee..243abc5c08 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -2,15 +2,201 @@ import os import json import numpy as np +import capnp import cereal.messaging as messaging from cereal import car, log -from cereal.services import SERVICE_LIST from openpilot.common.params import Params -from openpilot.common.realtime import config_realtime_process +from openpilot.common.realtime import config_realtime_process, DT_MDL +from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States +from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR +from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.locationd.estimators.vehicle_params import VehicleParamsLearner -from openpilot.selfdrive.locationd.estimators.lateral_lag import LateralLagEstimator + +MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s +ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits +ROLL_MIN, ROLL_MAX = np.radians(-10), np.radians(10) +ROLL_LOWERED_MAX = np.radians(8) +ROLL_STD_MAX = np.radians(1.5) +LATERAL_ACC_SENSOR_THRESHOLD = 4.0 +OFFSET_MAX = 10.0 +OFFSET_LOWERED_MAX = 8.0 +MIN_ACTIVE_SPEED = 1.0 +LOW_ACTIVE_SPEED = 10.0 + + +class VehicleParamsLearner: + def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None): + self.kf = CarKalman(GENERATED_DIR) + + self.x_initial = CarKalman.initial_x.copy() + self.x_initial[States.STEER_RATIO] = steer_ratio + self.x_initial[States.STIFFNESS] = stiffness_factor + self.x_initial[States.ANGLE_OFFSET] = angle_offset + self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial + + self.kf.set_globals( + mass=CP.mass, + rotational_inertia=CP.rotationalInertia, + center_to_front=CP.centerToFront, + center_to_rear=CP.wheelbase - CP.centerToFront, + stiffness_front=CP.tireStiffnessFront, + stiffness_rear=CP.tireStiffnessRear + ) + + self.min_sr, self.max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio + + self.calibrator = PoseCalibrator() + + self.observed_speed = 0.0 + self.observed_yaw_rate = 0.0 + self.observed_roll = 0.0 + + self.avg_offset_valid = True + self.total_offset_valid = True + self.roll_valid = True + + self.reset(None) + + def reset(self, t: float | None): + self.kf.init_state(self.x_initial, covs=self.P_initial, filter_time=t) + + self.angle_offset, self.roll, self.active = np.degrees(self.x_initial[States.ANGLE_OFFSET].item()), 0.0, False + self.avg_angle_offset = self.angle_offset + + def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader): + if which == 'livePose': + device_pose = Pose.from_live_pose(msg) + calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) + + yaw_rate, yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std + yaw_rate_valid = msg.angularVelocityDevice.valid + yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10 # rad/s + yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1 # rad/s + if not yaw_rate_valid: + # This is done to bound the yaw rate estimate when localizer values are invalid or calibrating + yaw_rate, yaw_rate_std = 0.0, np.radians(10.0) + self.observed_yaw_rate = yaw_rate + + localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std + localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std + roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK + if roll_valid: + roll = localizer_roll + # Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar? + roll_std = 2 * localizer_roll_std + else: + # This is done to bound the road roll estimate when localizer values are invalid + roll = 0.0 + roll_std = np.radians(10.0) + self.observed_roll = np.clip(roll, self.observed_roll - ROLL_MAX_DELTA, self.observed_roll + ROLL_MAX_DELTA) + + if self.active: + if msg.posenetOK: + self.kf.predict_and_observe(t, + ObservationKind.ROAD_FRAME_YAW_RATE, + np.array([[-self.observed_yaw_rate]]), + np.array([np.atleast_2d(yaw_rate_std**2)])) + + self.kf.predict_and_observe(t, + ObservationKind.ROAD_ROLL, + np.array([[self.observed_roll]]), + np.array([np.atleast_2d(roll_std**2)])) + self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]])) + + # We observe the current stiffness and steer ratio (with a high observation noise) to bound + # the respective estimate STD. Otherwise the STDs keep increasing, causing rapid changes in the + # states in longer routes (especially straight stretches). + stiffness = float(self.kf.x[States.STIFFNESS].item()) + steer_ratio = float(self.kf.x[States.STEER_RATIO].item()) + self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]])) + self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]])) + + elif which == 'liveCalibration': + self.calibrator.feed_live_calib(msg) + + elif which == 'carState': + steering_angle = msg.steeringAngleDeg + + in_linear_region = abs(steering_angle) < 45 + self.observed_speed = msg.vEgo + self.active = self.observed_speed > MIN_ACTIVE_SPEED and in_linear_region + + if self.active: + self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[np.radians(steering_angle)]])) + self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.observed_speed]])) + + if not self.active: + # Reset time when stopped so uncertainty doesn't grow + self.kf.filter.set_filter_time(t) # type: ignore + self.kf.filter.reset_rewind() # type: ignore + + def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder: + x = self.kf.x + P = np.sqrt(self.kf.P.diagonal()) + if not np.all(np.isfinite(x)): + cloudlog.error("NaN in liveParameters estimate. Resetting to default values") + self.reset(self.kf.t) + x = self.kf.x + + self.avg_angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item()), + self.avg_angle_offset - MAX_ANGLE_OFFSET_DELTA, self.avg_angle_offset + MAX_ANGLE_OFFSET_DELTA) + self.angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()), + self.angle_offset - MAX_ANGLE_OFFSET_DELTA, self.angle_offset + MAX_ANGLE_OFFSET_DELTA) + self.roll = np.clip(float(x[States.ROAD_ROLL].item()), self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA) + roll_std = float(P[States.ROAD_ROLL].item()) + if self.active and self.observed_speed > LOW_ACTIVE_SPEED: + # Account for the opposite signs of the yaw rates + # At low speeds, bumping into a curb can cause the yaw rate to be very high + sensors_valid = bool(abs(self.observed_speed * (x[States.YAW_RATE].item() + self.observed_yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD) + else: + sensors_valid = True + self.avg_offset_valid = check_valid_with_hysteresis(self.avg_offset_valid, self.avg_angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX) + self.total_offset_valid = check_valid_with_hysteresis(self.total_offset_valid, self.angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX) + self.roll_valid = check_valid_with_hysteresis(self.roll_valid, self.roll, ROLL_MAX, ROLL_LOWERED_MAX) + + msg = messaging.new_message('liveParameters') + + msg.valid = valid + + liveParameters = msg.liveParameters + liveParameters.posenetValid = True + liveParameters.sensorValid = sensors_valid + liveParameters.steerRatio = float(x[States.STEER_RATIO].item()) + liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item()) + liveParameters.roll = float(self.roll) + liveParameters.angleOffsetAverageDeg = float(self.avg_angle_offset) + liveParameters.angleOffsetDeg = float(self.angle_offset) + liveParameters.steerRatioValid = self.min_sr <= liveParameters.steerRatio <= self.max_sr + liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0 + liveParameters.angleOffsetAverageValid = bool(self.avg_offset_valid) + liveParameters.angleOffsetValid = bool(self.total_offset_valid) + liveParameters.valid = all(( + liveParameters.angleOffsetAverageValid, + liveParameters.angleOffsetValid , + self.roll_valid, + roll_std < ROLL_STD_MAX, + liveParameters.stiffnessFactorValid, + liveParameters.steerRatioValid, + )) + liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item()) + liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item()) + liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item()) + liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item()) + if debug: + liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message() + liveParameters.debugFilterState.value = x.tolist() + liveParameters.debugFilterState.std = P.tolist() + + return msg + + +def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: float, lowered_threshold: float): + if current_valid: + current_valid = abs(val) < threshold + else: + current_valid = abs(val) < lowered_threshold + return current_valid # TODO: Remove this function after few releases (added in 0.9.9) @@ -72,46 +258,22 @@ def retrieve_initial_vehicle_params(params_reader: Params, CP: car.CarParams, re return steer_ratio, stiffness_factor, angle_offset_deg, p_initial -def retrieve_initial_lag(params_reader: Params, CP: car.CarParams): - last_lag_data = params_reader.get("LiveLag") - last_carparams_data = params_reader.get("CarParamsPrevRoute") - - if last_lag_data is not None: - try: - with log.Event.from_bytes(last_lag_data) as last_lag_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP: - ld = last_lag_msg.liveDelay - if last_CP.carFingerprint != CP.carFingerprint: - raise Exception("Car model mismatch") - - lag, valid_blocks = ld.lateralDelayEstimate, ld.validBlocks - return lag, valid_blocks - except Exception as e: - cloudlog.error(f"Failed to retrieve initial lag: {e}") - - return None - - def main(): config_realtime_process([0, 1, 2, 3], 5) DEBUG = bool(int(os.getenv("DEBUG", "0"))) REPLAY = bool(int(os.getenv("REPLAY", "0"))) - pm = messaging.PubMaster(['liveParameters', 'liveDelay']) - sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', 'controlsState', 'carControl'], poll='livePose') + pm = messaging.PubMaster(['liveParameters']) + sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState'], poll='livePose') params_reader = Params() CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams) migrate_cached_vehicle_params_if_needed(params_reader) - steer_ratio, stiffness_factor, angle_offset_deg, p_initial = retrieve_initial_vehicle_params(params_reader, CP, REPLAY, DEBUG) - params_learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), p_initial) - - lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency) - if (initial_lag_params := retrieve_initial_lag(params_reader, CP)) is not None: - lag, valid_blocks = initial_lag_params - lag_learner.reset(lag, valid_blocks) + steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params_reader, CP, REPLAY, DEBUG) + learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial) while True: sm.update() @@ -119,30 +281,17 @@ def main(): for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]): if sm.updated[which]: t = sm.logMonoTime[which] * 1e-9 - if which in params_learner.inputs: - params_learner.handle_log(t, which, sm[which]) - if which in lag_learner.inputs: - lag_learner.handle_log(t, which, sm[which]) - lag_learner.update_points() + learner.handle_log(t, which, sm[which]) - params_msg_dat, lag_msg_dat = None, None if sm.updated['livePose']: - params_msg = params_learner.get_msg(sm.all_checks(), debug=DEBUG) - params_msg_dat = params_msg.to_bytes() - pm.send('liveParameters', params_msg_dat) - - # 4Hz driven by livePose - if sm.frame % 5 == 0: - lag_learner.update_estimate() - lag_msg = lag_learner.get_msg(sm.all_checks(), DEBUG) - lag_msg_dat = lag_msg.to_bytes() - pm.send('liveDelay', lag_msg_dat) - - if sm.frame % 1200 == 0: # cache every 60 seconds - if params_msg_dat is not None: - params_reader.put_nonblocking("LiveParameters", params_msg_dat) - if lag_msg_dat is not None: - params_reader.put_nonblocking("LiveLag", lag_msg_dat) + msg = learner.get_msg(sm.all_checks(), debug=DEBUG) + + msg_dat = msg.to_bytes() + if sm.frame % 1200 == 0: # once a minute + params_reader.put_nonblocking("LiveParameters", msg_dat) + + pm.send('liveParameters', msg_dat) + if __name__ == "__main__": main() diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 34b807fcad..13de985c68 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -542,8 +542,8 @@ CONFIGS = [ ), ProcessConfig( proc_name="paramsd", - pubs=["livePose", "liveCalibration", "carState", "carControl", "controlsState"], - subs=["liveParameters", "liveDelay"], + pubs=["livePose", "liveCalibration", "carState"], + subs=["liveParameters"], ignore=["logMonoTime"], init_callback=get_car_params_callback, should_recv_callback=FrequencyBasedRcvCallback("livePose"), diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index cd31ea53e8..58ff170e74 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -0ef214e7f4f06d6d591a2257d254f3c00db6a0e9 \ No newline at end of file +887623a18d82088dc5ed9ecdced55eb0d3f718b1 \ No newline at end of file