mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-05 13:32:05 +08:00
Revert "Online lateral lag learning" (#34974)
Revert "Online lateral lag learning (#34531)"
This reverts commit 1034dbd37c.
This commit is contained in:
Kacper Rączy
@@ -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);
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -71,7 +71,6 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"LastPowerDropDetected", CLEAR_ON_MANAGER_START},
|
||||
{"LastUpdateException", CLEAR_ON_MANAGER_START},
|
||||
{"LastUpdateTime", PERSISTENT},
|
||||
{"LiveLag", PERSISTENT},
|
||||
{"LiveParameters", PERSISTENT},
|
||||
{"LiveTorqueParameters", PERSISTENT | DONT_LOG},
|
||||
{"LocationFilterInitialState", PERSISTENT},
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
+201
-52
@@ -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)
|
||||
msg = learner.get_msg(sm.all_checks(), debug=DEBUG)
|
||||
|
||||
# 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)
|
||||
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 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)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
@@ -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"),
|
||||
|
||||
@@ -1 +1 @@
|
||||
0ef214e7f4f06d6d591a2257d254f3c00db6a0e9
|
||||
887623a18d82088dc5ed9ecdced55eb0d3f718b1
|
||||
Reference in New Issue
Block a user