def __init__(self,
CP,
steer_ratio,
stiffness_factor,
angle_offset,
P_initial=None):
self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor,
angle_offset, P_initial)
self.kf.filter.set_global("mass", CP.mass)
self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia)
self.kf.filter.set_global("center_to_front", CP.centerToFront)
self.kf.filter.set_global("center_to_rear",
CP.wheelbase - CP.centerToFront)
self.kf.filter.set_global("stiffness_front", CP.tireStiffnessFront)
self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear)
self.active = False
self.speed = 0.0
self.roll = 0.0
self.steering_pressed = False
self.steering_angle = 0.0
self.valid = True
class ParamsLearner:
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor,
angle_offset)
self.kf.filter.set_mass(CP.mass) # pylint: disable=no-member
self.kf.filter.set_rotational_inertia(CP.rotationalInertia) # pylint: disable=no-member
self.kf.filter.set_center_to_front(CP.centerToFront) # pylint: disable=no-member
self.kf.filter.set_center_to_rear(CP.wheelbase - CP.centerToFront) # pylint: disable=no-member
self.kf.filter.set_stiffness_front(CP.tireStiffnessFront) # pylint: disable=no-member
self.kf.filter.set_stiffness_rear(CP.tireStiffnessRear) # pylint: disable=no-member
self.active = False
self.speed = 0
self.steering_pressed = False
self.steering_angle = 0
self.carstate_counter = 0
self.valid = True
def handle_log(self, t, which, msg):
if which == 'liveLocationKalman':
yaw_rate = msg.angularVelocityCalibrated.value[2]
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
if self.active:
if msg.inputsOK and msg.posenetOK and msg.status == KalmanStatus.valid:
self.kf.predict_and_observe(
t, ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[[-yaw_rate]]]),
np.array([np.atleast_2d(yaw_rate_std**2)]))
self.kf.predict_and_observe(t,
ObservationKind.ANGLE_OFFSET_FAST,
np.array([[[0]]]))
elif which == 'carState':
self.carstate_counter += 1
if self.carstate_counter % CARSTATE_DECIMATION == 0:
self.steering_angle = msg.steeringAngle
self.steering_pressed = msg.steeringPressed
self.speed = msg.vEgo
in_linear_region = abs(
self.steering_angle) < 45 or not self.steering_pressed
self.active = self.speed > 5 and in_linear_region
if self.active:
self.kf.predict_and_observe(
t, ObservationKind.STEER_ANGLE,
np.array([[[math.radians(msg.steeringAngle)]]]))
self.kf.predict_and_observe(
t, ObservationKind.ROAD_FRAME_X_SPEED,
np.array([[[self.speed]]]))
if not self.active:
# Reset time when stopped so uncertainty doesn't grow
self.kf.filter.filter_time = t
self.kf.filter.reset_rewind()
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor,
angle_offset)
self.kf.filter.set_mass(CP.mass) # pylint: disable=no-member
self.kf.filter.set_rotational_inertia(CP.rotationalInertia) # pylint: disable=no-member
self.kf.filter.set_center_to_front(CP.centerToFront) # pylint: disable=no-member
self.kf.filter.set_center_to_rear(CP.wheelbase - CP.centerToFront) # pylint: disable=no-member
self.kf.filter.set_stiffness_front(CP.tireStiffnessFront) # pylint: disable=no-member
self.kf.filter.set_stiffness_rear(CP.tireStiffnessRear) # pylint: disable=no-member
self.active = False
self.speed = 0
self.steering_pressed = False
self.steering_angle = 0
self.carstate_counter = 0
class ParamsLearner:
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor,
angle_offset)
self.kf.filter.set_global("mass", CP.mass)
self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia)
self.kf.filter.set_global("center_to_front", CP.centerToFront)
self.kf.filter.set_global("center_to_rear",
CP.wheelbase - CP.centerToFront)
self.kf.filter.set_global("stiffness_front", CP.tireStiffnessFront)
self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear)
self.active = False
self.speed = 0
self.steering_pressed = False
self.steering_angle = 0
self.valid = True
def handle_log(self, t, which, msg):
if which == 'liveLocationKalman':
yaw_rate = msg.angularVelocityCalibrated.value[2]
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
yaw_rate_valid = msg.angularVelocityCalibrated.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 self.active:
if msg.inputsOK and msg.posenetOK and yaw_rate_valid:
self.kf.predict_and_observe(
t, ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[-yaw_rate]]),
np.array([np.atleast_2d(yaw_rate_std**2)]))
self.kf.predict_and_observe(t,
ObservationKind.ANGLE_OFFSET_FAST,
np.array([[0]]))
elif which == 'carState':
self.steering_angle = msg.steeringAngleDeg
self.steering_pressed = msg.steeringPressed
self.speed = msg.vEgo
in_linear_region = abs(
self.steering_angle) < 45 or not self.steering_pressed
self.active = self.speed > 5 and in_linear_region
if self.active:
self.kf.predict_and_observe(
t, ObservationKind.STEER_ANGLE,
np.array([[math.radians(msg.steeringAngleDeg)]]))
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_X_SPEED,
np.array([[self.speed]]))
if not self.active:
# Reset time when stopped so uncertainty doesn't grow
self.kf.filter.set_filter_time(t)
self.kf.filter.reset_rewind()
class ParamsLearner:
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None):
self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset, P_initial)
self.kf.filter.set_global("mass", CP.mass)
self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia)
self.kf.filter.set_global("center_to_front", CP.centerToFront)
self.kf.filter.set_global("center_to_rear", CP.wheelbase - CP.centerToFront)
self.kf.filter.set_global("stiffness_front", CP.tireStiffnessFront)
self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear)
self.active = False
self.speed = 0.0
self.roll = 0.0
self.steering_pressed = False
self.steering_angle = 0.0
self.valid = True
def handle_log(self, t, which, msg):
if which == 'liveLocationKalman':
yaw_rate = msg.angularVelocityCalibrated.value[2]
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
localizer_roll = msg.orientationNED.value[0]
localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0]
roll_valid = msg.orientationNED.valid and ROLL_MIN < localizer_roll < ROLL_MAX
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.roll = clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
yaw_rate_valid = msg.angularVelocityCalibrated.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 self.active:
if msg.posenetOK:
if yaw_rate_valid:
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[-yaw_rate]]),
np.array([np.atleast_2d(yaw_rate_std**2)]))
self.kf.predict_and_observe(t,
ObservationKind.ROAD_ROLL,
np.array([[self.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])
steer_ratio = float(self.kf.x[States.STEER_RATIO])
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 == 'carState':
self.steering_angle = msg.steeringAngleDeg
self.steering_pressed = msg.steeringPressed
self.speed = msg.vEgo
in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed
self.active = self.speed > 5 and in_linear_region
if self.active:
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]]))
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.speed]]))
if not self.active:
# Reset time when stopped so uncertainty doesn't grow
self.kf.filter.set_filter_time(t)
self.kf.filter.reset_rewind()
