Cython KF1D to Python (#30773)

* Cython KF1D to Python

* cleanup

* set x

* less nesting

* fix release

* Revert "fix release"

This reverts commit 97e5d0f.

common/SConscript

@@ -30,11 +30,10 @@ if GetOption('extras'):
 params_python = envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [_common, 'zmq', 'json11'])
 
 SConscript([
-  'kalman/SConscript',
-  'transformations/SConscript'  
+  'transformations/SConscript',
 ])
 
-Import('simple_kalman_python', 'transformations_python')
-common_python = [params_python, simple_kalman_python, transformations_python]
+Import('transformations_python')
+common_python = [params_python, transformations_python]
 
 Export('common_python')

common/simple_kalman.py

@@ -0,0 +1,54 @@
+import numpy as np
+
+
+def get_kalman_gain(dt, A, C, Q, R, iterations=100):
+  P = np.zeros_like(Q)
+  for _ in range(iterations):
+    P = A.dot(P).dot(A.T) + dt * Q
+    S = C.dot(P).dot(C.T) + R
+    K = P.dot(C.T).dot(np.linalg.inv(S))
+    P = (np.eye(len(P)) - K.dot(C)).dot(P)
+  return K
+
+
+class KF1D:
+  # this EKF assumes constant covariance matrix, so calculations are much simpler
+  # the Kalman gain also needs to be precomputed using the control module
+
+  def __init__(self, x0, A, C, K):
+    self.x0_0 = x0[0][0]
+    self.x1_0 = x0[1][0]
+    self.A0_0 = A[0][0]
+    self.A0_1 = A[0][1]
+    self.A1_0 = A[1][0]
+    self.A1_1 = A[1][1]
+    self.C0_0 = C[0]
+    self.C0_1 = C[1]
+    self.K0_0 = K[0][0]
+    self.K1_0 = K[1][0]
+
+    self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0
+    self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1
+    self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0
+    self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1
+
+    # K matrix needs to  be pre-computed as follow:
+    # import control
+    # (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R)
+    # self.K = np.transpose(K)
+
+  def update(self, meas):
+    #self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas)
+    x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas
+    x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas
+    self.x0_0 = x0_0
+    self.x1_0 = x1_0
+    return [self.x0_0, self.x1_0]
+
+  @property
+  def x(self):
+    return [[self.x0_0], [self.x1_0]]
+
+  def set_x(self, x):
+    self.x0_0 = x[0][0]
+    self.x1_0 = x[1][0]

common/tests/test_simple_kalman.py

@@ -0,0 +1,35 @@
+import unittest
+
+from openpilot.common.simple_kalman import KF1D
+
+
+class TestSimpleKalman(unittest.TestCase):
+  def setUp(self):
+    dt = 0.01
+    x0_0 = 0.0
+    x1_0 = 0.0
+    A0_0 = 1.0
+    A0_1 = dt
+    A1_0 = 0.0
+    A1_1 = 1.0
+    C0_0 = 1.0
+    C0_1 = 0.0
+    K0_0 = 0.12287673
+    K1_0 = 0.29666309
+
+    self.kf = KF1D(x0=[[x0_0], [x1_0]],
+                   A=[[A0_0, A0_1], [A1_0, A1_1]],
+                   C=[C0_0, C0_1],
+                   K=[[K0_0], [K1_0]])
+
+  def test_getter_setter(self):
+    self.kf.set_x([[1.0], [1.0]])
+    self.assertEqual(self.kf.x, [[1.0], [1.0]])
+
+  def update_returns_state(self):
+    x = self.kf.update(100)
+    self.assertEqual(x, self.kf.x)
+
+
+if __name__ == "__main__":
+  unittest.main()

release/files_common

@@ -24,9 +24,6 @@ common/__init__.py
 common/*.py
 common/*.pyx
 
-common/kalman/.gitignore
-common/kalman/*
-
 common/transformations/__init__.py
 common/transformations/camera.py
 common/transformations/model.py

selfdrive/car/interfaces.py

@@ -8,7 +8,7 @@
 from cereal import car
 from openpilot.common.basedir import BASEDIR
 from openpilot.common.conversions import Conversions as CV
-from openpilot.common.kalman.simple_kalman import KF1D, get_kalman_gain
+from openpilot.common.simple_kalman import KF1D, get_kalman_gain
 from openpilot.common.numpy_fast import clip
 from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG
@@ -346,7 +346,7 @@ def __init__(self, CP):
 
   def update_speed_kf(self, v_ego_raw):
     if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0:  # Prevent large accelerations when car starts at non zero speed
-      self.v_ego_kf.x = [[v_ego_raw], [0.0]]
+      self.v_ego_kf.set_x([[v_ego_raw], [0.0]])
 
     v_ego_x = self.v_ego_kf.update(v_ego_raw)
     return float(v_ego_x[0]), float(v_ego_x[1])

selfdrive/controls/radard.py

@@ -11,7 +11,7 @@
 from openpilot.common.realtime import Ratekeeper, Priority, config_realtime_process
 from openpilot.common.swaglog import cloudlog
 
-from openpilot.common.kalman.simple_kalman import KF1D
+from openpilot.common.simple_kalman import KF1D
 
 
 # Default lead acceleration decay set to 50% at 1s