Cython KF1D to Python (commaai#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