diff --git a/selfdrive/locationd/kalman/helpers/ekf_sym.py b/selfdrive/locationd/kalman/helpers/ekf_sym.py index 0078f3fcbf..c9eb093a4b 100644 --- a/selfdrive/locationd/kalman/helpers/ekf_sym.py +++ b/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), diff --git a/selfdrive/locationd/kalman/helpers/sympy_helpers.py b/selfdrive/locationd/kalman/helpers/sympy_helpers.py index f8c4f1c5f1..dee9fb3731 100644 --- a/selfdrive/locationd/kalman/helpers/sympy_helpers.py +++ b/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 = [] diff --git a/selfdrive/locationd/kalman/models/car_kf.py b/selfdrive/locationd/kalman/models/car_kf.py index 3ba2b4b844..aa729fc102 100755 --- a/selfdrive/locationd/kalman/models/car_kf.py +++ b/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): diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 417e1069ed..93523502c1 100755 --- a/selfdrive/locationd/paramsd.py +++ b/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()