diff --git a/common/SConscript b/common/SConscript index 9b81e1dd29..52b3adf1d3 100644 --- a/common/SConscript +++ b/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') diff --git a/common/kalman/.gitignore b/common/kalman/.gitignore deleted file mode 100644 index d86912e7d0..0000000000 --- a/common/kalman/.gitignore +++ /dev/null @@ -1 +0,0 @@ -simple_kalman_impl.c 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/__init__.py b/common/kalman/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/common/kalman/simple_kalman.py b/common/kalman/simple_kalman.py deleted file mode 100644 index cd3b5a1df9..0000000000 --- a/common/kalman/simple_kalman.py +++ /dev/null @@ -1,12 +0,0 @@ -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): - 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 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 d11770faf6..0000000000 --- a/common/kalman/simple_kalman_old.py +++ /dev/null @@ -1,23 +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) - - # 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 diff --git a/common/kalman/tests/__init__.py b/common/kalman/tests/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/common/kalman/tests/test_simple_kalman.py b/common/kalman/tests/test_simple_kalman.py deleted file mode 100644 index 32cc79fc3d..0000000000 --- a/common/kalman/tests/test_simple_kalman.py +++ /dev/null @@ -1,87 +0,0 @@ -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): - 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_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.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) - self.assertTrue(kf_speed < kf_old_speed / 4) - -if __name__ == "__main__": - unittest.main() diff --git a/common/simple_kalman.py b/common/simple_kalman.py new file mode 100644 index 0000000000..194b27204b --- /dev/null +++ b/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] diff --git a/common/tests/test_simple_kalman.py b/common/tests/test_simple_kalman.py new file mode 100644 index 0000000000..f641cd19e6 --- /dev/null +++ b/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() diff --git a/release/files_common b/release/files_common index 3dd116613c..2039347048 100644 --- a/release/files_common +++ b/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 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 94b3bdfc84..3bbf16c789 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -8,7 +8,7 @@ from typing import Any, Dict, Optional, Tuple, List, Callable 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 @@ class CarStateBase(ABC): 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]) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index f75b4b2059..4acadee7a7 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -11,7 +11,7 @@ from openpilot.common.params import Params 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