cleanup

common/SConscript

@@ -31,11 +31,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/kalman/SConscript

@@ -1,5 +0,0 @@
-Import('envCython')
-
-simple_kalman_python = envCython.Program('simple_kalman_impl.so', 'simple_kalman_impl.pyx')
-
-Export('simple_kalman_python')

common/kalman/simple_kalman.py

@@ -1,7 +1,6 @@
-from openpilot.common.kalman.simple_kalman_impl import KF1D as KF1D
-assert KF1D
 import numpy as np
 
+
 def get_kalman_gain(dt, A, C, Q, R, iterations=100):
   P = np.zeros_like(Q)
   for _ in range(iterations):
@@ -10,3 +9,46 @@ def get_kalman_gain(dt, A, C, Q, R, iterations=100):
     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/kalman/simple_kalman_impl.pxd

@@ -1,18 +0,0 @@
-# cython: language_level = 3
-
-cdef class KF1D:
-  cdef public:
-    double x0_0
-    double x1_0
-    double K0_0
-    double K1_0
-    double A0_0
-    double A0_1
-    double A1_0
-    double A1_1
-    double C0_0
-    double C0_1
-    double A_K_0
-    double A_K_1
-    double A_K_2
-    double A_K_3

common/kalman/simple_kalman_impl.pyx

@@ -1,37 +0,0 @@
-# distutils: language = c++
-# cython: language_level=3
-
-cdef class KF1D:
-  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
-
-  def update(self, meas):
-    cdef double x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas
-    cdef double 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]]
-
-  @x.setter
-  def x(self, x):
-    self.x0_0 = x[0][0]
-    self.x1_0 = x[1][0]

common/kalman/simple_kalman_old.py

@@ -1,50 +0,0 @@
-import numpy as np
-
-
-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.x = x0
-    self.A = A
-    self.C = np.atleast_2d(C)
-    self.K = K
-
-    self.A_K = self.A - np.dot(self.K, self.C)
-
-    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)
-  #  return self.x
-
-  def update(self, 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]
-
-

common/kalman/tests/test_simple_kalman.py

@@ -1,10 +1,6 @@
 import unittest
-import random
-import timeit
-import numpy as np
 
 from openpilot.common.kalman.simple_kalman import KF1D
-from openpilot.common.kalman.simple_kalman_old import KF1D as KF1D_old
 
 
 class TestSimpleKalman(unittest.TestCase):
@@ -21,69 +17,19 @@ class TestSimpleKalman(unittest.TestCase):
     K0_0 = 0.12287673
     K1_0 = 0.29666309
 
-    self.kf_old = KF1D_old(x0=np.array([[x0_0], [x1_0]]),
-                           A=np.array([[A0_0, A0_1], [A1_0, A1_1]]),
-                           C=np.array([C0_0, C0_1]),
-                           K=np.array([[K0_0], [K1_0]]))
-
     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.x = [[1.0], [1.0]]
+    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)
 
-  def test_old_equal_new(self):
-    for _ in range(1000):
-      v_wheel = random.uniform(0, 200)
-
-      x_old = self.kf_old.update(v_wheel)
-      x = self.kf.update(v_wheel)
-
-      # Compare the output x, verify that the error is less than 1e-4
-      np.testing.assert_almost_equal(x_old[0], x[0])
-      np.testing.assert_almost_equal(x_old[1], x[1])
-
-  def test_new_is_faster(self):
-    setup = """
-import numpy as np
-
-from openpilot.common.kalman.simple_kalman import KF1D
-from openpilot.common.kalman.simple_kalman_old import KF1D as KF1D_old
-
-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
-
-kf_old = KF1D_old(x0=np.array([[x0_0], [x1_0]]),
-                  A=np.array([[A0_0, A0_1], [A1_0, A1_1]]),
-                  C=np.array([C0_0, C0_1]),
-                  K=np.array([[K0_0], [K1_0]]))
-
-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]])
-    """
-    kf_speed = timeit.timeit("kf.update(1234)", setup=setup, number=10000)
-    kf_old_speed = timeit.timeit("kf_old.update(1234)", setup=setup, number=10000)
-    print("speed", kf_speed)
-    print("speed old", kf_old_speed)
-    self.assertTrue(kf_speed < kf_old_speed / 4)
 
 if __name__ == "__main__":
   unittest.main()