locationd and paramsd using cython version of ekfsym (#20610)

* Locationd live_kf using c++ kalman filter

* use both cpp and python live_kf to compare

* Locationd using ekfsym cpp

* Paramsd using c++ ekf_sym

* Other building method

* Cleanup

* cleanup

* Single sconscript for rednose and locationd/models

* CI

* CI

* CI fix

* renamed scons config

* Fix lib loading

* bump rednose

* update cpu usage test

SConstruct

@@ -356,6 +356,28 @@ else:
 
 Export('common', 'gpucommon', 'visionipc')
 
+# Build rednose library and ekf models
+
+rednose_config = {
+  'generated_folder': '#selfdrive/locationd/models/generated',
+  'to_build': {
+    'live': ('#selfdrive/locationd/models/live_kf.py', True),
+    'car': ('#selfdrive/locationd/models/car_kf.py', True),
+  },
+}
+
+if arch != "aarch64":
+  rednose_config['to_build'].update({
+    'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True),
+    'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True),
+    'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False),
+    'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False),
+    'feature_handler_5': ('#rednose/helpers/feature_handler.py', False),
+    'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', True),
+  })
+
+Export('rednose_config')
+SConscript(['rednose/SConscript'])
 
 # Build openpilot
 
@@ -384,7 +406,6 @@ SConscript(['selfdrive/clocksd/SConscript'])
 SConscript(['selfdrive/loggerd/SConscript'])
 
 SConscript(['selfdrive/locationd/SConscript'])
-SConscript(['selfdrive/locationd/models/SConscript'])
 SConscript(['selfdrive/sensord/SConscript'])
 SConscript(['selfdrive/ui/SConscript'])
 

rednose_repo

@@ -1 +1 @@
-Subproject commit 946a0345740abe46241b2168262c01d7239d642e
+Subproject commit fbf65ab4aeec9a6891eaf22ddf430e592ab67aa4

release/files_common

@@ -288,7 +288,6 @@ selfdrive/locationd/ublox_msg.h
 selfdrive/locationd/locationd.py
 selfdrive/locationd/paramsd.py
 selfdrive/locationd/models/.gitignore
-selfdrive/locationd/models/SConscript
 selfdrive/locationd/models/live_kf.py
 selfdrive/locationd/models/car_kf.py
 selfdrive/locationd/models/constants.py

selfdrive/locationd/models/SConscript

@@ -1,37 +0,0 @@
-Import('env', 'arch')
-
-templates = Glob('#rednose/templates/*')
-
-sympy_helpers = "#rednose/helpers/sympy_helpers.py"
-ekf_sym = "#rednose/helpers/ekf_sym.py"
-
-to_build = {
-    'live': ('live_kf.py', 'generated'),
-    'car': ('car_kf.py', 'generated'),
-}
-
-if arch != "aarch64":
-    to_build.update({
-        'gnss': ('gnss_kf.py', 'generated'),
-        'loc_4': ('loc_kf.py', 'generated'),
-        'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', 'generated'),
-        'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', 'generated'),
-        'feature_handler_5': ('#rednose/helpers/feature_handler.py', 'generated'),
-        'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', 'generated'),
-    })
-
-found = {}
-
-for target, (command, generated_folder) in to_build.items():
-    if File(command).exists():
-        found[target] = (command, generated_folder)
-
-for target, (command, generated_folder) in found.items():
-    target_files = File([f'{generated_folder}/{target}.cpp', f'{generated_folder}/{target}.h'])
-    command_file = File(command)
-
-    env.Command(target_files,
-                [templates, command_file, sympy_helpers, ekf_sym],
-                command_file.get_abspath() + " " + target + " " + Dir(generated_folder).get_abspath())
-
-    env.SharedLibrary(f'{generated_folder}/' + target, target_files[0])

selfdrive/locationd/models/car_kf.py

@@ -6,14 +6,19 @@ from typing import Any, Dict
 import numpy as np
 import sympy as sp
 
-from rednose import KalmanFilter
-from rednose.helpers.ekf_sym import EKF_sym, gen_code
 from selfdrive.locationd.models.constants import ObservationKind
 from selfdrive.swaglog import cloudlog
 
-i = 0
+from rednose.helpers.kalmanfilter import KalmanFilter
+
+if __name__ == '__main__':  # Generating sympy
+  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
 
 
+i = 0
+
 def _slice(n):
   global i
   s = slice(i, i + n)
@@ -149,7 +154,8 @@ class CarKalman(KalmanFilter):
     x_init[States.ANGLE_OFFSET] = angle_offset
 
     # init filter
-    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)
+    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)
 
 
 if __name__ == "__main__":

selfdrive/locationd/models/live_kf.py

@@ -1,14 +1,17 @@
 #!/usr/bin/env python3
 
 import sys
-
 import numpy as np
-import sympy as sp
 
 from selfdrive.swaglog import cloudlog
 from selfdrive.locationd.models.constants import ObservationKind
-from rednose.helpers.ekf_sym import EKF_sym, gen_code
+
-from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
+if __name__ == '__main__':  # Generating sympy
+  import sympy as sp
+  from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
+  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
 
 EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)
 
@@ -215,7 +218,7 @@ class LiveKalman():
 
   @property
   def t(self):
-    return self.filter.filter_time
+    return self.filter.get_filter_time()
 
   @property
   def P(self):
@@ -249,10 +252,7 @@ class LiveKalman():
         R = R[None]
       r = self.filter.predict_and_update_batch(t, kind, meas, R)
 
-    # Normalize quats
+    self.filter.normalize_state(States.ECEF_ORIENTATION.start, States.ECEF_ORIENTATION.stop)
-    quat_norm = np.linalg.norm(self.filter.x[3:7, 0])
-    self.filter.x[States.ECEF_ORIENTATION, 0] = self.filter.x[States.ECEF_ORIENTATION, 0] / quat_norm
-
     return r
 
   def get_R(self, kind, n):

selfdrive/locationd/models/loc_kf.py

@@ -392,7 +392,7 @@ class LocKalman():
 
   @property
   def t(self):
-    return self.filter.filter_time
+    return self.filter.get_filter_time()
 
   @property
   def P(self):
@@ -449,15 +449,15 @@ class LocKalman():
     else:
       r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data)))
     # Normalize quats
-    quat_norm = np.linalg.norm(self.filter.x[3:7, 0])
+    quat_norm = np.linalg.norm(self.filter.state()[3:7])
     # Should not continue if the quats behave this weirdly
     if not 0.1 < quat_norm < 10:
       raise RuntimeError("Sir! The filter's gone all wobbly!")
-    self.filter.x[3:7, 0] = self.filter.x[3:7, 0] / quat_norm
+    self.filter.normalize_state(3, 7)
     for i in range(self.N):
       d1 = self.dim_main
       d3 = self.dim_augment
-      self.filter.x[d1 + d3 * i + 3:d1 + d3 * i + 7] /= np.linalg.norm(self.filter.x[d1 + i * d3 + 3:d1 + i * d3 + 7, 0])
+      self.filter.normalize_state(d1 + d3 * i + 3, d1 + d3 * i + 7)
     return r
 
   def get_R(self, kind, n):
@@ -528,7 +528,7 @@ class LocKalman():
     poses = self.x[self.dim_main:].reshape((-1, 7))
     times = tracks.reshape((len(tracks), self.N + 1, 4))[:, :, 0]
     good_counter = 0
-    if times.any() and np.allclose(times[0, :-1], self.filter.augment_times, rtol=1e-6):
+    if times.any() and np.allclose(times[0, :-1], self.filter.get_augment_times(), rtol=1e-6):
       for i, track in enumerate(tracks):
         img_positions = track.reshape((self.N + 1, 4))[:, 2:]
 

selfdrive/locationd/paramsd.py

@@ -16,12 +16,12 @@ class ParamsLearner:
   def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
     self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset)
 
-    self.kf.filter.set_mass(CP.mass)  # pylint: disable=no-member
+    self.kf.filter.set_global("mass", CP.mass)
-    self.kf.filter.set_rotational_inertia(CP.rotationalInertia)  # pylint: disable=no-member
+    self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia)
-    self.kf.filter.set_center_to_front(CP.centerToFront)  # pylint: disable=no-member
+    self.kf.filter.set_global("center_to_front", CP.centerToFront)
-    self.kf.filter.set_center_to_rear(CP.wheelbase - CP.centerToFront)  # pylint: disable=no-member
+    self.kf.filter.set_global("center_to_rear", CP.wheelbase - CP.centerToFront)
-    self.kf.filter.set_stiffness_front(CP.tireStiffnessFront)  # pylint: disable=no-member
+    self.kf.filter.set_global("stiffness_front", CP.tireStiffnessFront)
-    self.kf.filter.set_stiffness_rear(CP.tireStiffnessRear)  # pylint: disable=no-member
+    self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear)
 
     self.active = False
 
@@ -42,9 +42,9 @@ class ParamsLearner:
         if msg.inputsOK and msg.posenetOK and yaw_rate_valid:
           self.kf.predict_and_observe(t,
                                       ObservationKind.ROAD_FRAME_YAW_RATE,
-                                      np.array([[[-yaw_rate]]]),
+                                      np.array([[-yaw_rate]]),
                                       np.array([np.atleast_2d(yaw_rate_std**2)]))
-        self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]]))
+        self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))
 
     elif which == 'carState':
       self.steering_angle = msg.steeringAngleDeg
@@ -55,12 +55,12 @@ class ParamsLearner:
       self.active = self.speed > 5 and in_linear_region
 
       if self.active:
-        self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngleDeg)]]]))
+        self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]]))
-        self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]]))
+        self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.speed]]))
 
     if not self.active:
       # Reset time when stopped so uncertainty doesn't grow
-      self.kf.filter.filter_time = t
+      self.kf.filter.set_filter_time(t)
       self.kf.filter.reset_rewind()
 
 

selfdrive/test/test_onroad.py

@@ -16,10 +16,10 @@ from tools.lib.logreader import LogReader
 PROCS = [
   ("selfdrive.controls.controlsd", 47.0),
   ("./loggerd", 45.0),
-  ("selfdrive.locationd.locationd", 35.0),
+  ("selfdrive.locationd.locationd", 32.8),
   ("selfdrive.controls.plannerd", 20.0),
   ("./_ui", 15.0),
-  ("selfdrive.locationd.paramsd", 12.0),
+  ("selfdrive.locationd.paramsd", 9.1),
   ("./camerad", 7.07),
   ("./_sensord", 6.17),
   ("selfdrive.controls.radard", 5.67),