paramsd improvements from branch

old-commit-hash: a24d08e569

selfdrive/locationd/kalman/helpers/ekf_sym.py

@@ -291,6 +291,11 @@ class EKF_sym():
     self.rewind_t = []
     self.rewind_states = []
 
+  def reset_rewind(self):
+    self.rewind_obscache = []
+    self.rewind_t = []
+    self.rewind_states = []
+
   def augment(self):
     # TODO this is not a generalized way of doing this and implies that the augmented states
     # are simply the first (dim_augment_state) elements of the main state.

selfdrive/locationd/kalman/models/car_kf.py

@@ -69,8 +69,6 @@ class CarKalman():
   ])
 
   obs_noise = {
-    ObservationKind.ROAD_FRAME_XY_SPEED: np.diag([0.1**2, 0.1**2]),
-    ObservationKind.ROAD_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),
@@ -149,8 +147,12 @@ class CarKalman():
 
     gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds, global_vars=CarKalman.global_vars)
 
-  def __init__(self):
+  def __init__(self, steer_ratio=15, stiffness_factor=1, angle_offset=0):
     self.dim_state = self.x_initial.shape[0]
+    x_init = self.x_initial
+    x_init[States.STEER_RATIO] = steer_ratio
+    x_init[States.STIFFNESS] = stiffness_factor
+    x_init[States.ANGLE_OFFSET] = angle_offset
 
     # 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, global_vars=self.global_vars)
@@ -186,10 +188,14 @@ class CarKalman():
       P = self.filter.covs()
     self.filter.init_state(state, P, filter_time)
 
-  def predict_and_observe(self, t, kind, data):
+  def predict_and_observe(self, t, kind, data, R=None):
     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 R is None:
+      R = self.get_R(kind, len(data))
+
+    self.filter.predict_and_update_batch(t, kind, data, R)
 
 
 if __name__ == "__main__":

selfdrive/locationd/paramsd.py

@@ -1,15 +1,22 @@
 #!/usr/bin/env python3
 import math
 
+import json
+import numpy as np
+
 import cereal.messaging as messaging
-from selfdrive.locationd.kalman.models.car_kf import CarKalman, ObservationKind, States
+from cereal import car
+from common.params import Params, put_nonblocking
+from selfdrive.locationd.kalman.models.car_kf import (CarKalman,
+                                                      ObservationKind, States)
+from selfdrive.swaglog import cloudlog
 
 CARSTATE_DECIMATION = 5
 
 
 class ParamsLearner:
-  def __init__(self, CP):
-    self.kf = CarKalman()
+  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
@@ -25,34 +32,37 @@ class ParamsLearner:
     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]
+      v_calibrated_std = msg.velocityCalibrated.std
 
       yaw_rate = msg.angularVelocityCalibrated.value[2]
-      # yaw_rate_std = msg.angularVelocityCalibrated.std[2]
+      yaw_rate_std = msg.angularVelocityCalibrated.std[2]
+
+      self.active = v_calibrated[0] > 5
+      in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed
 
-      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]]])
+      if self.active and in_linear_region:
+        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.ROAD_FRAME_XY_SPEED,
+                                    np.array([[[v_calibrated[0], -v_calibrated[1]]]]),
+                                    np.array([np.diag([v_calibrated_std[0]**2, v_calibrated_std[1]**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, [15.0])
+          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, [1.0])
-
-      else:
-        self.kf.filter.filter_time = t - 0.1
+          self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[[1.0]]]))
 
     elif which == 'carState':
       self.carstate_counter += 1
@@ -60,12 +70,13 @@ class ParamsLearner:
         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
+          self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]]))
+
+    if not self.active:
+      # Reset time when stopped so uncertainty doesn't grow
+      self.kf.filter.filter_time = t
+      self.kf.filter.reset_rewind()
 
 
 def main(sm=None, pm=None):
@@ -74,13 +85,33 @@ def main(sm=None, pm=None):
   if pm is None:
     pm = messaging.PubMaster(['liveParameters'])
 
-  # TODO: Read from car params at runtime
-  from selfdrive.car.toyota.interface import CarInterface
-  from selfdrive.car.toyota.values import CAR
-
-  CP = CarInterface.get_params(CAR.COROLLA_TSS2)
-  learner = ParamsLearner(CP)
-
+  params_reader = Params()
+  # wait for stats about the car to come in from controls
+  cloudlog.info("paramsd is waiting for CarParams")
+  CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
+  cloudlog.info("paramsd got CarParams")
+
+  params = params_reader.get("LiveParameters")
+
+  # Check if car model matches
+  if params is not None:
+    params = json.loads(params)
+    if params.get('carFingerprint', None) != CP.carFingerprint:
+      cloudlog.info("Parameter learner found parameters for wrong car.")
+      params = None
+
+  if params is None:
+    params = {
+      'carFingerprint': CP.carFingerprint,
+      'steerRatio': CP.steerRatio,
+      'stiffnessFactor': 1.0,
+      'angleOffsetAverage': 0.0,
+    }
+    cloudlog.info("Parameter learner resetting to default values")
+
+  learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverage']))
+
+  i = 0
   while True:
     sm.update()
 
@@ -92,9 +123,6 @@ def main(sm=None, pm=None):
 
     # 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('liveParameters')
@@ -110,6 +138,16 @@ def main(sm=None, pm=None):
       msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET])
       msg.liveParameters.angleOffset = math.degrees(x[States.ANGLE_OFFSET_FAST])
 
+      i += 1
+      if i % 6000 == 0:   # once a minute
+        params = {
+          'carFingerprint': CP.carFingerprint,
+          'steerRatio': msg.liveParameters.steerRatio,
+          'stiffnessFactor': msg.liveParameters.stiffnessFactor,
+          'angleOffsetAverage': msg.liveParameters.angleOffsetAverage,
+        }
+        put_nonblocking("LiveParameters", json.dumps(params))
+
       # P = learner.kf.P
       # print()
       # print("sR", float(x[States.STEER_RATIO]), float(P[States.STEER_RATIO, States.STEER_RATIO])**0.5)