Kacper Rączy
3 weeks ago
parent
0d2922fd2c
commit
b1e389c780
3 changed files with 268 additions and 251 deletions
@ -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 |
||||
@ -0,0 +1,65 @@ |
||||
#!/usr/bin/env python3 |
||||
import os |
||||
|
||||
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.swaglog import cloudlog |
||||
from openpilot.selfdrive.locationd.estimators.lateral_lag import LateralLagEstimator |
||||
|
||||
|
||||
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"))) |
||||
|
||||
pm = messaging.PubMaster(['liveDelay']) |
||||
sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', 'controlsState', 'carControl'], poll='livePose') |
||||
|
||||
params_reader = Params() |
||||
CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams) |
||||
|
||||
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) |
||||
|
||||
while True: |
||||
sm.update() |
||||
if sm.all_checks(): |
||||
for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]): |
||||
if sm.updated[which]: |
||||
t = sm.logMonoTime[which] * 1e-9 |
||||
lag_learner.handle_log(t, which, sm[which]) |
||||
lag_learner.update_points() |
||||
|
||||
# 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 |
||||
params_reader.put_nonblocking("LiveLag", lag_msg_dat) |
||||
Loading…
Reference in new issue