diff --git a/common/SConscript b/common/SConscript index b8c23b7019..d4dd89cca4 100644 --- a/common/SConscript +++ b/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') diff --git a/common/kalman/SConscript b/common/kalman/SConscript deleted file mode 100644 index 7cba4a9a61..0000000000 --- a/common/kalman/SConscript +++ /dev/null @@ -1,5 +0,0 @@ -Import('envCython') - -simple_kalman_python = envCython.Program('simple_kalman_impl.so', 'simple_kalman_impl.pyx') - -Export('simple_kalman_python') diff --git a/common/kalman/simple_kalman.py b/common/kalman/simple_kalman.py index cd3b5a1df9..194b27204b 100644 --- a/common/kalman/simple_kalman.py +++ b/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] diff --git a/common/kalman/simple_kalman_impl.pxd b/common/kalman/simple_kalman_impl.pxd deleted file mode 100644 index cb39a45bca..0000000000 --- a/common/kalman/simple_kalman_impl.pxd +++ /dev/null @@ -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 diff --git a/common/kalman/simple_kalman_impl.pyx b/common/kalman/simple_kalman_impl.pyx deleted file mode 100644 index 16aefba2e5..0000000000 --- a/common/kalman/simple_kalman_impl.pyx +++ /dev/null @@ -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] diff --git a/common/kalman/simple_kalman_old.py b/common/kalman/simple_kalman_old.py deleted file mode 100644 index 9afecc05c3..0000000000 --- a/common/kalman/simple_kalman_old.py +++ /dev/null @@ -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] - - diff --git a/common/kalman/tests/test_simple_kalman.py b/common/kalman/tests/test_simple_kalman.py index 7c4d95c10d..3afaf09d24 100644 --- a/common/kalman/tests/test_simple_kalman.py +++ b/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()