Online lateral lag learning (#34531)
* Add struct and services * Proof of concept * Use vego * Process replay support * Fix issues * Max not min * Remove print * Make lag positive * Points and updates * Sliding cross corr * Stuff * Remove redundant param * Move to lagd * Update alert * Something that works * Report isEstimated * Corr field * Use skimage masked corr * Fixes * Move to lat_accel_loc * Use the delay it in the model * Mark as executable * Add 0.2 to initial * add install line * Back to 5m window * Move the import up * corr window 120 * Show is estimated * Sort messages * New impl * Fix * Params * Set initial_lag elsewhere * Add param * Rename Param * Fix type isues * More blocks * Masked fft NCC implementation * Remove package installation * Use 20hz data and interp the correlation peak * Move ncc code to separate function * steering_saturated support * Constants * Add recovery buffer * Fix static * min_valid_block_count nad liveDelay * lateralDelayEstimate * Fix comm issues. Do estimations at 4hz * Fix * Display the estimate * Increase the block size * Cache every minute * DEBUG flag * Add progress to the alert * Fix crash * points only in debug * Fix BlockAverage restoration * Comment * Move estimators into estimator directory * Remove lagd * Feed only relevant messages * Rewrite as a set literal * np.diag with P_initial * Fix static * Struct renames * Update ref commit * Make it nicer * Some renamespull/34970/head
Kacper Rączy
4 weeks ago
committed by
GitHub
parent
39d4148c70
commit
1034dbd37c
9 changed files with 545 additions and 206 deletions
@ -0,0 +1,273 @@ |
||||
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 |
||||
@ -0,0 +1,197 @@ |
||||
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 |
||||
@ -1 +1 @@ |
||||
887623a18d82088dc5ed9ecdced55eb0d3f718b1 |
||||
0ef214e7f4f06d6d591a2257d254f3c00db6a0e9 |
||||
Loading…
Reference in new issue