def __init__(self): self.kf = CarKalman() self.active = False self.speed = 0 self.steering_pressed = False self.steering_angle = 0 self.carstate_counter = 0
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset): self.kf = CarKalman(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(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 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: 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]]])) # Clamp values # x = self.kf.x # if not (10 < x[States.STEER_RATIO] < 25): # self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[[15.0]]])) # if not (0.5 < x[States.STIFFNESS] < 3.0): # self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[[1.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()
speeds = 10 * np.sin(2 * np.pi * ts / 200.) + 25 angle_offsets = math.radians(1.0) * np.ones_like(ts) angle_offsets[ts > 60] = 0 steering_angles = np.radians(5 * np.cos(2 * np.pi * ts / 100.)) xs = [] ys = [] psis = [] yaw_rates = [] speed_ys = [] kf_states = [] kf_ps = [] kf = CarKalman() for i, t in tqdm(list(enumerate(ts))): u = speeds[i] sa = steering_angles[i] ao = angle_offsets[i] A, B = create_dyn_state_matrices(u, VM) state += DT * (A.dot(state) + B.dot(sa + ao)) x += u * math.cos(psi) * DT y += (float(state[0]) * math.sin(psi) + u * math.sin(psi)) * DT psi += float(state[1]) * DT kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_YAW_RATE,
class ParamsLearner: def __init__(self, CP): self.kf = CarKalman() 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 def update_active(self): self.active = (abs(self.steering_angle) < 45 or not self.steering_pressed) and self.speed > 5 def handle_log(self, t, which, msg): if which == 'liveLocationKalman': v_calibrated = msg.velocityCalibrated.value # v_calibrated_std = msg.velocityCalibrated.std self.speed = v_calibrated[0] yaw_rate = msg.angularVelocityCalibrated.value[2] # yaw_rate_std = msg.angularVelocityCalibrated.std[2] self.update_active() if self.active: self.kf.predict_and_observe( t, ObservationKind.ROAD_FRAME_YAW_RATE, [-yaw_rate]) self.kf.predict_and_observe( t, ObservationKind.ROAD_FRAME_XY_SPEED, [[v_calibrated[0], -v_calibrated[1]]]) # Clamp values x = self.kf.x if not (10 < x[States.STEER_RATIO] < 25): self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, [15.0]) if not (0.5 < x[States.STIFFNESS] < 3.0): self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, [1.0]) else: self.kf.filter.filter_time = t - 0.1 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.update_active() if self.active: self.kf.predict_and_observe( t, ObservationKind.STEER_ANGLE, [math.radians(msg.steeringAngle)]) self.kf.predict_and_observe( t, ObservationKind.ANGLE_OFFSET_FAST, [0]) else: self.kf.filter.filter_time = t - 0.1
class ParamsLearner: def __init__(self): self.kf = CarKalman() self.active = False self.speed = 0 self.steering_pressed = False self.steering_angle = 0 self.carstate_counter = 0 def update_active(self): self.active = (abs(self.steering_angle) < 45 or not self.steering_pressed) and self.speed > 5 def handle_log(self, t, which, msg): if which == 'liveLocation': roll, pitch, yaw = math.radians(msg.roll), math.radians( msg.pitch), math.radians(msg.heading) v_device = orient.rot_from_euler([roll, pitch, yaw]).T.dot(msg.vNED) self.speed = v_device[0] self.update_active() if self.active: self.kf.predict_and_observe( t, ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, [-msg.gyro[2]]) self.kf.predict_and_observe( t, ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, [[v_device[0], -v_device[1]]]) # Clamp values x = self.kf.x if not (10 < x[States.STEER_RATIO] < 25): self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, [15.0]) if not (0.5 < x[States.STIFFNESS] < 3.0): self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, [1.0]) else: self.kf.filter.filter_time = t - 0.1 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.update_active() if self.active: self.kf.predict_and_observe( t, ObservationKind.STEER_ANGLE, [math.radians(msg.steeringAngle)]) self.kf.predict_and_observe( t, ObservationKind.ANGLE_OFFSET_FAST, [0]) else: self.kf.filter.filter_time = t - 0.1