Kalman filter to identify vehicle parameters (#1123)

* full vehicle model simulator * Add vehicle model * Model compiles * Close enough * Simulation works * Add fast angle offset * Tune fast angle offset learner * Create live service for paramsd * Better clamping * Fix rotation matrix * Cleanup before merge * move debug script to debug/internal * revert plannerd change * switch vehicle model to corolla * fix valid flag * Bigger stiffness range * Lower process noise on steer ratio * Tuning * Decimation * No maha tests
5 years ago · c9ecab2139
parent 63b52b60cb
commit c9ecab2139
7 changed files with 495 additions and 24 deletions
--- a/release/files_common
+++ b/release/files_common
@ -244,6 +244,7 @@ selfdrive/locationd/ublox_msg.h
 selfdrive/locationd/test/*.py

 selfdrive/locationd/locationd.py
+selfdrive/locationd/paramsd.py
 selfdrive/locationd/kalman/.gitignore
 selfdrive/locationd/kalman/__init__.py
 selfdrive/locationd/kalman/README.md
@ -251,6 +252,7 @@ selfdrive/locationd/kalman/SConscript
 selfdrive/locationd/kalman/templates/*
 selfdrive/locationd/kalman/helpers/*
 selfdrive/locationd/kalman/models/live_kf.py
+selfdrive/locationd/kalman/models/car_kf.py

 selfdrive/locationd/calibrationd.py
 selfdrive/locationd/calibration_helpers.py
--- a/selfdrive/debug/internal/test_paramsd.py
+++ b/selfdrive/debug/internal/test_paramsd.py
@ -0,0 +1,129 @@
+#!/usr/bin/env python3
+import numpy as np
+import math
+from tqdm import tqdm
+
+import seaborn as sns
+import matplotlib.pyplot as plt
+
+
+from selfdrive.car.honda.interface import CarInterface
+from selfdrive.car.honda.values import CAR
+from selfdrive.controls.lib.vehicle_model import VehicleModel, create_dyn_state_matrices
+from selfdrive.locationd.kalman.models.car_kf import CarKalman, ObservationKind, States
+
+T_SIM = 5 * 60  # s
+DT = 0.01
+
+
+CP = CarInterface.get_params(CAR.CIVIC)
+VM = VehicleModel(CP)
+
+x, y = 0, 0  # m, m
+psi = math.radians(0)  # rad
+
+# The state is x = [v, r]^T
+# with v lateral speed [m/s], and r rotational speed [rad/s]
+state = np.array([[0.0], [0.0]])
+
+
+ts = np.arange(0, T_SIM, DT)
+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, [float(state[1])])
+  kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, [[u, float(state[0])]])
+  kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, [sa])
+  kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, [0])
+  kf.predict(t)
+
+  speed_ys.append(float(state[0]))
+  yaw_rates.append(float(state[1]))
+  kf_states.append(kf.x.copy())
+  kf_ps.append(kf.P.copy())
+
+  xs.append(x)
+  ys.append(y)
+  psis.append(psi)
+
+
+xs = np.asarray(xs)
+ys = np.asarray(ys)
+psis = np.asarray(psis)
+speed_ys = np.asarray(speed_ys)
+kf_states = np.asarray(kf_states)
+kf_ps = np.asarray(kf_ps)
+
+
+palette = sns.color_palette()
+
+def plot_with_bands(ts, state, label, ax, idx=1, converter=None):
+  mean = kf_states[:, state].flatten()
+  stds = np.sqrt(kf_ps[:, state, state].flatten())
+
+  if converter is not None:
+    mean = converter(mean)
+    stds = converter(stds)
+
+  sns.lineplot(ts, mean, label=label, ax=ax)
+  ax.fill_between(ts, mean - stds, mean + stds, alpha=.2, color=palette[idx])
+
+
+print(kf.x)
+
+sns.set_context("paper")
+f, axes = plt.subplots(6, 1)
+
+sns.lineplot(ts, np.degrees(steering_angles), label='Steering Angle [deg]', ax=axes[0])
+plot_with_bands(ts, States.STEER_ANGLE, 'Steering Angle kf [deg]', axes[0], converter=np.degrees)
+
+sns.lineplot(ts, np.degrees(yaw_rates), label='Yaw Rate [deg]', ax=axes[1])
+plot_with_bands(ts, States.YAW_RATE, 'Yaw Rate kf [deg]', axes[1], converter=np.degrees)
+
+sns.lineplot(ts, np.ones_like(ts) * VM.sR, label='Steer ratio [-]', ax=axes[2])
+plot_with_bands(ts, States.STEER_RATIO, 'Steer ratio kf [-]', axes[2])
+axes[2].set_ylim([10, 20])
+
+
+sns.lineplot(ts, np.ones_like(ts), label='Tire stiffness[-]', ax=axes[3])
+plot_with_bands(ts, States.STIFFNESS, 'Tire stiffness kf [-]', axes[3])
+axes[3].set_ylim([0.8, 1.2])
+
+
+sns.lineplot(ts, np.degrees(angle_offsets), label='Angle offset [deg]', ax=axes[4])
+plot_with_bands(ts, States.ANGLE_OFFSET, 'Angle offset kf deg', axes[4], converter=np.degrees)
+plot_with_bands(ts, States.ANGLE_OFFSET_FAST, 'Fast Angle offset kf deg', axes[4], converter=np.degrees, idx=2)
+
+axes[4].set_ylim([-2, 2])
+
+sns.lineplot(ts, speeds, ax=axes[5])
+
+plt.show()
--- a/selfdrive/locationd/kalman/SConscript
+++ b/selfdrive/locationd/kalman/SConscript
@ -8,6 +8,7 @@ to_build = {
    'pos_computer_4': 'helpers/lst_sq_computer.py',
    'pos_computer_5': 'helpers/lst_sq_computer.py',
    'feature_handler_5': 'helpers/feature_handler.py',
+    'car': 'models/car_kf.py',
    'gnss': 'models/gnss_kf.py',
    'loc_4': 'models/loc_kf.py',
    'live': 'models/live_kf.py',
--- a/selfdrive/locationd/kalman/helpers/init.py
+++ b/selfdrive/locationd/kalman/helpers/init.py
@ -56,7 +56,15 @@ class ObservationKind():
  PSEUDORANGE = 22
  PSEUDORANGE_RATE = 23

-  names = ['Unknown',
+  CAL_DEVICE_FRAME_XY_SPEED = 24  # (x, y) [m/s]
+  CAL_DEVICE_FRAME_YAW_RATE = 25  # [rad/s]
+  STEER_ANGLE = 26  # [rad]
+  ANGLE_OFFSET_FAST = 27  # [rad]
+  STIFFNESS = 28  # [-]
+  STEER_RATIO = 29  # [-]
+
+  names = [
+    'Unknown',
    'No observation',
    'GPS NED',
    'Odometric speed',
@ -77,7 +85,15 @@ class ObservationKind():
    'Lane ecef point',
    'imu frame eulers',
    'GLONASS pseudorange',
-           'GLONASS pseudorange rate']
+    'GLONASS pseudorange rate',
+
+    'Calibrated Device Frame x,y speed',
+    'Calibrated Device Frame yaw rate',
+    'Steer Angle',
+    'Fast Angle Offset',
+    'Stiffness',
+    'Steer Ratio',
+  ]

  @classmethod
  def to_string(cls, kind):
--- a/selfdrive/locationd/kalman/helpers/ekf_sym.py
+++ b/selfdrive/locationd/kalman/helpers/ekf_sym.py
@ -74,8 +74,13 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No

  # linearize with jacobians
  F_sym = f_err_sym.jacobian(x_err_sym)
+
+  if eskf_params:
    for sym in x_err_sym:
      F_sym = F_sym.subs(sym, 0)
+
+  assert dt_sym in F_sym.free_symbols
+
  for i in range(len(obs_eqs)):
    obs_eqs[i].append(obs_eqs[i][0].jacobian(x_sym))
    if msckf and obs_eqs[i][1] in feature_track_kinds:
@ -336,6 +341,17 @@ class EKF_sym():
    self.rewind_states = self.rewind_states[-REWIND_TO_KEEP:]
    self.rewind_obscache = self.rewind_obscache[-REWIND_TO_KEEP:]

+  def predict(self, t):
+    # initialize time
+    if self.filter_time is None:
+      self.filter_time = t
+
+    # predict
+    dt = t - self.filter_time
+    assert dt >= 0
+    self.x, self.P = self._predict(self.x, self.P, dt)
+    self.filter_time = t
+
  def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False):
    # TODO handle rewinding at this level"

--- a/selfdrive/locationd/kalman/models/car_kf.py
+++ b/selfdrive/locationd/kalman/models/car_kf.py
@ -0,0 +1,198 @@
+#!/usr/bin/env python3
+
+import math
+import numpy as np
+import sympy as sp
+
+from selfdrive.locationd.kalman.helpers import ObservationKind
+from selfdrive.locationd.kalman.helpers.ekf_sym import EKF_sym, gen_code
+
+i = 0
+
+
+def _slice(n):
+  global i
+  s = slice(i, i + n)
+  i += n
+
+  return s
+
+
+class States():
+  # Vehicle model params
+  STIFFNESS = _slice(1)  # [-]
+  STEER_RATIO = _slice(1)  # [-]
+  ANGLE_OFFSET = _slice(1)  # [rad]
+  ANGLE_OFFSET_FAST = _slice(1)  # [rad]
+
+  VELOCITY = _slice(2)  # (x, y) [m/s]
+  YAW_RATE = _slice(1)  # [rad/s]
+  STEER_ANGLE = _slice(1)  # [rad]
+
+
+class CarKalman():
+  name = 'car'
+
+  x_initial = np.array([
+    1.0,
+    15.0,
+    0.0,
+    0.0,
+
+    10.0, 0.0,
+    0.0,
+    0.0,
+  ])
+
+  # state covariance
+  P_initial = np.diag([
+    .1**2,
+    .1**2,
+    math.radians(0.1)**2,
+    math.radians(0.1)**2,
+
+    10**2, 10**2,
+    1**2,
+    1**2,
+  ])
+
+  # process noise
+  Q = np.diag([
+    (.05/10)**2,
+    .0001**2,
+    math.radians(0.01)**2,
+    math.radians(0.2)**2,
+
+    .1**2, .1**2,
+    math.radians(0.1)**2,
+    math.radians(0.1)**2,
+  ])
+
+  obs_noise = {
+    ObservationKind.CAL_DEVICE_FRAME_XY_SPEED: np.diag([0.1**2, 0.1**2]),
+    ObservationKind.CAL_DEVICE_FRAME_YAW_RATE: np.atleast_2d(math.radians(0.1)**2),
+    ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.1)**2),
+    ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(5.0)**2),
+    ObservationKind.STEER_RATIO: np.atleast_2d(50.0**2),
+    ObservationKind.STIFFNESS: np.atleast_2d(50.0**2),
+  }
+
+  maha_test_kinds = []  # [ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, ObservationKind.CAL_DEVICE_FRAME_XY_SPEED]
+
+  @staticmethod
+  def generate_code():
+    dim_state = CarKalman.x_initial.shape[0]
+    name = CarKalman.name
+    maha_test_kinds = CarKalman.maha_test_kinds
+
+    # make functions and jacobians with sympy
+    # state variables
+    state_sym = sp.MatrixSymbol('state', dim_state, 1)
+    state = sp.Matrix(state_sym)
+
+    # Vehicle model constants
+    # TODO: Read from car params at runtime
+    from selfdrive.controls.lib.vehicle_model import VehicleModel
+    from selfdrive.car.toyota.interface import CarInterface
+    from selfdrive.car.toyota.values import CAR
+
+    CP = CarInterface.get_params(CAR.COROLLA_TSS2)
+    VM = VehicleModel(CP)
+
+    m = VM.m
+    j = VM.j
+    aF = VM.aF
+    aR = VM.aR
+
+    x = state[States.STIFFNESS, :][0, 0]
+
+    cF, cR = x * VM.cF, x * VM.cR
+    angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
+    angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
+    sa = state[States.STEER_ANGLE, :][0, 0]
+
+    sR = state[States.STEER_RATIO, :][0, 0]
+    u, v = state[States.VELOCITY, :]
+    r = state[States.YAW_RATE, :][0, 0]
+
+    A = sp.Matrix(np.zeros((2, 2)))
+    A[0, 0] = -(cF + cR) / (m * u)
+    A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
+    A[1, 0] = -(cF * aF - cR * aR) / (j * u)
+    A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
+
+    B = sp.Matrix(np.zeros((2, 1)))
+    B[0, 0] = cF / m / sR
+    B[1, 0] = (cF * aF) / j / sR
+
+    x = sp.Matrix([v, r])  # lateral velocity, yaw rate
+    x_dot = A * x + B * (sa - angle_offset - angle_offset_fast)
+
+    dt = sp.Symbol('dt')
+    state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+    state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
+    state_dot[States.YAW_RATE.start, 0] = x_dot[1]
+
+    # Basic descretization, 1st order integrator
+    # Can be pretty bad if dt is big
+    f_sym = state + dt * state_dot
+
+    #
+    # Observation functions
+    #
+    obs_eqs = [
+      [sp.Matrix([r]), ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, None],
+      [sp.Matrix([u, v]), ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, None],
+      [sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
+      [sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
+      [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
+      [sp.Matrix([x]), ObservationKind.STIFFNESS, None],
+    ]
+
+    gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds)
+
+  def __init__(self):
+    self.dim_state = self.x_initial.shape[0]
+
+    # init filter
+    self.filter = EKF_sym(self.name, self.Q, self.x_initial, self.P_initial, self.dim_state, self.dim_state, maha_test_kinds=self.maha_test_kinds)
+
+  @property
+  def x(self):
+    return self.filter.state()
+
+  @property
+  def P(self):
+    return self.filter.covs()
+
+  def predict(self, t):
+    return self.filter.predict(t)
+
+  def rts_smooth(self, estimates):
+    return self.filter.rts_smooth(estimates, norm_quats=False)
+
+  def get_R(self, kind, n):
+    obs_noise = self.obs_noise[kind]
+    dim = obs_noise.shape[0]
+    R = np.zeros((n, dim, dim))
+    for i in range(n):
+      R[i, :, :] = obs_noise
+    return R
+
+  def init_state(self, state, covs_diag=None, covs=None, filter_time=None):
+    if covs_diag is not None:
+      P = np.diag(covs_diag)
+    elif covs is not None:
+      P = covs
+    else:
+      P = self.filter.covs()
+    self.filter.init_state(state, P, filter_time)
+
+  def predict_and_observe(self, t, kind, data):
+    if len(data) > 0:
+      data = np.atleast_2d(data)
+    self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data)))
+
+
+if __name__ == "__main__":
+  CarKalman.generate_code()
--- a/selfdrive/locationd/paramsd.py
+++ b/selfdrive/locationd/paramsd.py
@ -0,0 +1,109 @@
+#!/usr/bin/env python3
+import math
+
+import cereal.messaging as messaging
+import common.transformations.orientation as orient
+from selfdrive.locationd.kalman.models.car_kf import CarKalman, ObservationKind, States
+
+CARSTATE_DECIMATION = 5
+
+
+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
+
+
+def main(sm=None, pm=None):
+  if sm is None:
+    sm = messaging.SubMaster(['liveLocation', 'carState'])
+  if pm is None:
+    pm = messaging.PubMaster(['liveParameters'])
+
+  learner = ParamsLearner()
+
+  while True:
+    sm.update()
+
+    for which, updated in sm.updated.items():
+      if not updated:
+        continue
+      t = sm.logMonoTime[which] * 1e-9
+      learner.handle_log(t, which, sm[which])
+
+    # TODO: set valid to false when locationd stops sending
+    # TODO: make sure controlsd knows when there is no gyro
+    # TODO: move posenetValid somewhere else to show the model uncertainty alert
+    # TODO: Save and resume values from param
+    # TODO: Change KF to allow mass, etc to be inputs in predict step
+
+    if sm.updated['carState']:
+      msg = messaging.new_message()
+      msg.logMonoTime = sm.logMonoTime['carState']
+
+      msg.init('liveParameters')
+      msg.liveParameters.valid = True  # TODO: Check if learned values are sane
+      msg.liveParameters.posenetValid = True
+      msg.liveParameters.sensorValid = True
+
+      x = learner.kf.x
+      msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
+      msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
+      msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET])
+      msg.liveParameters.angleOffset = math.degrees(x[States.ANGLE_OFFSET_FAST])
+
+      # P = learner.kf.P
+      # print()
+      # print("sR", float(x[States.STEER_RATIO]), float(P[States.STEER_RATIO, States.STEER_RATIO])**0.5)
+      # print("x ", float(x[States.STIFFNESS]), float(P[States.STIFFNESS, States.STIFFNESS])**0.5)
+      # print("ao avg ", math.degrees(x[States.ANGLE_OFFSET]), math.degrees(P[States.ANGLE_OFFSET, States.ANGLE_OFFSET])**0.5)
+      # print("ao ", math.degrees(x[States.ANGLE_OFFSET_FAST]), math.degrees(P[States.ANGLE_OFFSET_FAST, States.ANGLE_OFFSET_FAST])**0.5)
+
+      pm.send('liveParameters', msg)
+
+
+if __name__ == "__main__":
+  main()