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
old-commit-hash: e6a8157916e9f8365f3f4ac70e49a552e40a8511

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,13 +6,18 @@ 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
@@ -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
-    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
-
+    self.filter.normalize_state(States.ECEF_ORIENTATION.start, States.ECEF_ORIENTATION.stop)
     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:]