Skip to content

Instantly share code, notes, and snippets.

@geohot
Last active March 9, 2021 07:36
Show Gist options
  • Save geohot/7c387246949e343c1ce21315de844fd2 to your computer and use it in GitHub Desktop.
Save geohot/7c387246949e343c1ce21315de844fd2 to your computer and use it in GitHub Desktop.
Prius Steering Angle Kalman Filter
%pylab inline
%load_ext autoreload
%autoreload 2
from tools.lib.route import Route
from tools.lib.logreader import LogReader
r,num = Route("ce2fbd370f78ef21|2020-11-27--16-27-28"),10
#r,num = Route("f66032c2b5aa18ac|2020-12-04--09-33-54"),30
alr = []
for n in range(num-1, num+5):
print("loading %d" % n)
lr = LogReader(r.log_paths()[n])
alr.extend([x for x in lr])
llr = sorted(alr, key=lambda x: x.logMonoTime)
cs = [x for x in llr if x.which() == "carState"]
ccs = [x for x in llr if x.which() == "controlsState"]
can = [x for x in llr if x.which() == "can"]
sc = [x for x in llr if x.which() == "sendcan"]
se = [x for x in llr if x.which() == "sensorEvents"]
ld = [x for x in llr if x.which() == "liveLocationKalman"]
from opendbc.can.parser import CANParser
signals = [
("ZORRO_STEER", "SECONDARY_STEER_ANGLE", 0),
("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0),
("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0),
("STEER_RATE", "STEER_ANGLE_SENSOR", 0),
("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0),
("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0),
("STEER_REQUEST", "STEERING_LKA", 0),
("STEER_TORQUE_CMD", "STEERING_LKA", 0),
("YAW_RATE", "KINEMATICS", 0)
]
cp = CANParser("toyota_prius_2017_pt_generated", signals)
#cp = CANParser("toyota_nodsu_pt_generated", signals)
from collections import defaultdict
msgs = defaultdict(list)
msgs_ts = defaultdict(list)
for c in can+sc:
updated = cp.update_string(c.as_builder().to_bytes())
for u in updated:
msgs[u].append({x:y for x,y in cp.vl[u].items()})
msgs_ts[u].append(c.logMonoTime)
from selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
from selfdrive.locationd.models.constants import GENERATED_DIR
from selfdrive.car import scale_tire_stiffness, STD_CARGO_KG, scale_rot_inertia
from selfdrive.config import Conversions as CV
steer_ratio = 15.74
wheelbase = 2.70
centerToFront = wheelbase * 0.44
mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
tire_stiffness_factor = 0.6371
tireStiffnessFront, tireStiffnessRear = scale_tire_stiffness(mass, wheelbase, centerToFront, tire_stiffness_factor=tire_stiffness_factor)
rotationalInertia = scale_rot_inertia(mass, wheelbase)
stiffness_factor = 1.0
angle_offset = 0.02
kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset)
kf.filter.set_mass(mass) # pylint: disable=no-member
kf.filter.set_rotational_inertia(rotationalInertia) # pylint: disable=no-member
kf.filter.set_center_to_front(centerToFront) # pylint: disable=no-member
kf.filter.set_center_to_rear(wheelbase - centerToFront) # pylint: disable=no-member
kf.filter.set_stiffness_front(tireStiffnessFront) # pylint: disable=no-member
kf.filter.set_stiffness_rear(tireStiffnessRear) # pylint: disable=no-member
ssa = []
for x in llr:
t = x.logMonoTime * 1e-9
if x.which() == 'sendcan' or x.which() == 'can':
updated = cp.update_string(x.as_builder().to_bytes())
for u in updated:
if u == 37:
steeringAngle = cp.vl[u]['STEER_ANGLE']+cp.vl[u]['STEER_FRACTION']
#print(sa)
kf.predict_and_observe(t, ObservationKind.STEER_ANGLE,
np.array([[[math.radians(steeringAngle)]]]),
np.array([np.atleast_2d(math.radians(1)**2)]))
if u == 608:
# needs fixed offset
steeringAngle = cp.vl[u]['STEER_ANGLE']
#kf.predict_and_observe(t, ObservationKind.STEER_ANGLE,
# np.array([[[math.radians(steeringAngle)]]]))
steeringTorque = cp.vl[u]['STEER_TORQUE_EPS']
kf.predict_and_observe(t, ObservationKind.STEER_TORQUE, np.array([[[steeringTorque]]]),
np.array([[[10]]]))
if x.which() == 'liveLocationKalman':
msg = x.liveLocationKalman
yaw_rate = msg.angularVelocityCalibrated.value[2]
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
#print('liveLocationKalman', yaw_rate, yaw_rate_std)
kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[[-yaw_rate]]]), np.array([np.atleast_2d(yaw_rate_std**2)]))
kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]]))
elif x.which() == 'carState':
msg = x.carState
#print('carState', msg.steeringAngle, msg.vEgo)
#kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]]),
# np.array([np.atleast_2d(math.radians(0.01)**2)]))
kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[msg.vEgo]]]))
ssa.append(np.array(kf.x))
assert not np.any(np.isnan(kf.x))
ssa = np.array(ssa)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment