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()
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()