EKF global variable support (#1230)

* Add global variables to kalman filter code

* fix linter

selfdrive/locationd/kalman/helpers/ekf_sym.py

@@ -27,7 +27,7 @@ def null(H, eps=1e-12):
   return np.transpose(null_space)
 
 
-def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[]):
+def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[], global_vars=None):
   # optional state transition matrix, H modifier
   # and err_function if an error-state kalman filter (ESKF)
   # is desired. Best described in "Quaternion kinematics
@@ -110,7 +110,7 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No
       sympy_functions.append(('He_%d' % kind, He_sym, [x_sym, ea_sym]))
 
   # Generate and wrap all th c code
-  header, code = sympy_into_c(sympy_functions)
+  header, code = sympy_into_c(sympy_functions, global_vars)
   extra_header = "#define DIM %d\n" % dim_x
   extra_header += "#define EDIM %d\n" % dim_err
   extra_header += "#define MEDIM %d\n" % dim_main_err
@@ -140,6 +140,17 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No
   code += '\nextern "C"{\n' + extra_header + "\n}\n"
   code += "\n" + open(os.path.join(TEMPLATE_DIR, "ekf_c.c")).read()
   code += '\nextern "C"{\n' + extra_post + "\n}\n"
+
+  if global_vars is not None:
+    global_code = '\nextern "C"{\n'
+    for var in global_vars:
+      global_code += f"\ndouble {var.name};\n"
+      global_code += f"\nvoid set_{var.name}(double x){{ {var.name} = x;}}\n"
+      extra_header += f"\nvoid set_{var.name}(double x);\n"
+
+    global_code += '\n}\n'
+    code = global_code + code
+
   header += "\n" + extra_header
 
   write_code(name, code, header)
@@ -147,7 +158,7 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No
 
 class EKF_sym():
   def __init__(self, name, Q, x_initial, P_initial, dim_main, dim_main_err,
-               N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[]):
+               N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[], global_vars=None):
     """Generates process function and all observation functions for the kalman filter."""
     self.msckf = N > 0
     self.N = N
@@ -168,6 +179,8 @@ class EKF_sym():
     # tested for outlier rejection
     self.maha_test_kinds = maha_test_kinds
 
+    self.global_vars = global_vars
+
     # process noise
     self.Q = Q
 
@@ -226,6 +239,11 @@ class EKF_sym():
       if self.msckf and kind in self.feature_track_kinds:
         self.Hes[kind] = wrap_2lists("He_%d" % kind)
 
+    if self.global_vars is not None:
+      for var in self.global_vars:
+        fun_name = f"set_{var.name}"
+        setattr(self, fun_name, getattr(lib, fun_name))
+
     # wrap the C++ predict function
     def _predict_blas(x, P, dt):
       lib.predict(ffi.cast("double *", x.ctypes.data),

selfdrive/locationd/kalman/helpers/sympy_helpers.py

@@ -46,11 +46,11 @@ def quat_matrix_r(p):
                     [p[3],  p[2], -p[1],  p[0]]])
 
 
-def sympy_into_c(sympy_functions):
+def sympy_into_c(sympy_functions, global_vars=None):
   from sympy.utilities import codegen
   routines = []
   for name, expr, args in sympy_functions:
-    r = codegen.make_routine(name, expr, language="C99")
+    r = codegen.make_routine(name, expr, language="C99", global_vars=global_vars)
 
     # argument ordering input to sympy is broken with function with output arguments
     nargs = []

selfdrive/locationd/kalman/models/car_kf.py

@@ -78,6 +78,14 @@ class CarKalman():
   }
 
   maha_test_kinds = []  # [ObservationKind.ROAD_FRAME_YAW_RATE, ObservationKind.ROAD_FRAME_XY_SPEED]
+  global_vars = [
+    sp.Symbol('mass'),
+    sp.Symbol('rotational_inertia'),
+    sp.Symbol('center_to_front'),
+    sp.Symbol('center_to_rear'),
+    sp.Symbol('stiffness_front'),
+    sp.Symbol('stiffness_rear'),
+  ]
 
   @staticmethod
   def generate_code():
@@ -85,28 +93,18 @@ class CarKalman():
     name = CarKalman.name
     maha_test_kinds = CarKalman.maha_test_kinds
 
+    # globals
+    m, j, aF, aR, cF_orig, cR_orig = CarKalman.global_vars
+
     # 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
+    cF, cR = x * cF_orig, x * cR_orig
     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]
@@ -149,13 +147,13 @@ class CarKalman():
       [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)
+    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):
     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)
+    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)
 
   @property
   def x(self):

selfdrive/locationd/paramsd.py

@@ -8,8 +8,16 @@ CARSTATE_DECIMATION = 5
 
 
 class ParamsLearner:
-  def __init__(self):
+  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
@@ -66,7 +74,12 @@ def main(sm=None, pm=None):
   if pm is None:
     pm = messaging.PubMaster(['liveParameters'])
 
-  learner = ParamsLearner()
+  # 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)
 
   while True:
     sm.update()