From b37c587c23b2f94b6dd138b2789807aea7467257 Mon Sep 17 00:00:00 2001
From: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date: Sat, 16 Dec 2023 22:56:06 -0800
Subject: [PATCH] Cython KF1D to Python

---
 common/kalman/simple_kalman_old.py        | 31 +++++++++++++++++++++--
 common/kalman/tests/test_simple_kalman.py |  2 ++
 2 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/common/kalman/simple_kalman_old.py b/common/kalman/simple_kalman_old.py
index d11770faf6..9afecc05c3 100644
--- a/common/kalman/simple_kalman_old.py
+++ b/common/kalman/simple_kalman_old.py
@@ -13,11 +13,38 @@ class KF1D:
 
     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):
-    self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas)
-    return self.x
+    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]
+
+
diff --git a/common/kalman/tests/test_simple_kalman.py b/common/kalman/tests/test_simple_kalman.py
index 32cc79fc3d..7c4d95c10d 100644
--- a/common/kalman/tests/test_simple_kalman.py
+++ b/common/kalman/tests/test_simple_kalman.py
@@ -81,6 +81,8 @@ kf = KF1D(x0=[[x0_0], [x1_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__":