Prius Steering Angle Kalman Filter

prius_kf.py

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