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