diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index 1efd955837..b440cc9bb0 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/selfdrive/locationd/models/car_kf.py @@ -4,7 +4,6 @@ import sys from typing import Any, Dict import numpy as np -import sympy as sp from selfdrive.locationd.models.constants import ObservationKind from selfdrive.swaglog import cloudlog @@ -12,6 +11,7 @@ from selfdrive.swaglog import cloudlog from rednose.helpers.kalmanfilter import KalmanFilter if __name__ == '__main__': # Generating sympy + import sympy as sp from rednose.helpers.ekf_sym import gen_code else: from rednose.helpers.ekf_sym_pyx import EKF_sym # pylint: disable=no-name-in-module, import-error @@ -75,12 +75,12 @@ class CarKalman(KalmanFilter): } 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'), + 'mass', + 'rotational_inertia', + 'center_to_front', + 'center_to_rear', + 'stiffness_front', + 'stiffness_rear', ] @staticmethod @@ -89,7 +89,8 @@ class CarKalman(KalmanFilter): name = CarKalman.name # globals - m, j, aF, aR, cF_orig, cR_orig = CarKalman.global_vars + global_vars = [sp.Symbol(name) for name in CarKalman.global_vars] + m, j, aF, aR, cF_orig, cR_orig = global_vars # make functions and jacobians with sympy # state variables @@ -143,7 +144,7 @@ class CarKalman(KalmanFilter): [sp.Matrix([x]), ObservationKind.STIFFNESS, None], ] - gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=CarKalman.global_vars) + gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars) def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0): # pylint: disable=super-init-not-called dim_state = self.initial_x.shape[0] @@ -154,8 +155,7 @@ class CarKalman(KalmanFilter): x_init[States.ANGLE_OFFSET] = angle_offset # init filter - global_var_names = [x.name for x in self.global_vars] # pylint: disable=no-member - self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, dim_state, dim_state_err, global_vars=global_var_names, logger=cloudlog) + self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog) if __name__ == "__main__":