From 915524d408a9e3131b92cb94401ad810d730f540 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 14 Aug 2024 18:10:11 -0700 Subject: [PATCH] Copy kalman filter to selfdrive/car (#33307) move to car/common/ and give proper names old-commit-hash: bcfb50d98cfb554bab352517793987027ad0a586 --- .importlinter | 3 -- selfdrive/car/__init__.py | 2 +- selfdrive/car/common/__init__.py | 0 selfdrive/car/{ => common}/filter_simple.py | 0 .../car/{helpers.py => common/numpy_fast.py} | 0 selfdrive/car/common/simple_kalman.py | 54 +++++++++++++++++++ selfdrive/car/ford/carcontroller.py | 2 +- selfdrive/car/gm/carcontroller.py | 2 +- selfdrive/car/gm/carstate.py | 2 +- selfdrive/car/honda/carcontroller.py | 2 +- selfdrive/car/honda/carstate.py | 2 +- selfdrive/car/honda/interface.py | 2 +- selfdrive/car/hyundai/carcontroller.py | 2 +- selfdrive/car/hyundai/hyundaicanfd.py | 2 +- selfdrive/car/interfaces.py | 4 +- selfdrive/car/subaru/carcontroller.py | 2 +- selfdrive/car/toyota/carcontroller.py | 2 +- selfdrive/car/toyota/carstate.py | 4 +- selfdrive/car/volkswagen/carcontroller.py | 2 +- 19 files changed, 70 insertions(+), 19 deletions(-) create mode 100644 selfdrive/car/common/__init__.py rename selfdrive/car/{ => common}/filter_simple.py (100%) rename selfdrive/car/{helpers.py => common/numpy_fast.py} (100%) create mode 100644 selfdrive/car/common/simple_kalman.py diff --git a/.importlinter b/.importlinter index 46f70f91f6..ebc180a3ad 100644 --- a/.importlinter +++ b/.importlinter @@ -29,9 +29,6 @@ ignore_imports = openpilot.selfdrive.car.tests.test_docs -> openpilot.common.basedir openpilot.selfdrive.car.docs -> openpilot.common.basedir - # car interface will not filter the speed - openpilot.selfdrive.car.interfaces -> openpilot.common.simple_kalman - openpilot.selfdrive.car.gm.interface -> openpilot.common.basedir openpilot.selfdrive.car.interfaces -> openpilot.common.basedir diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 52fbf0512a..567ac7199c 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -11,7 +11,7 @@ from cereal import car from panda.python.uds import SERVICE_TYPE from openpilot.selfdrive.car.can_definitions import CanData from openpilot.selfdrive.car.docs_definitions import CarDocs -from openpilot.selfdrive.car.helpers import clip, interp +from openpilot.selfdrive.car.common.numpy_fast import clip, interp # set up logging carlog = logging.getLogger('carlog') diff --git a/selfdrive/car/common/__init__.py b/selfdrive/car/common/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/filter_simple.py b/selfdrive/car/common/filter_simple.py similarity index 100% rename from selfdrive/car/filter_simple.py rename to selfdrive/car/common/filter_simple.py diff --git a/selfdrive/car/helpers.py b/selfdrive/car/common/numpy_fast.py similarity index 100% rename from selfdrive/car/helpers.py rename to selfdrive/car/common/numpy_fast.py diff --git a/selfdrive/car/common/simple_kalman.py b/selfdrive/car/common/simple_kalman.py new file mode 100644 index 0000000000..194b27204b --- /dev/null +++ b/selfdrive/car/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/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 961d7086c4..1eaccf7a3e 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -3,7 +3,7 @@ from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_std_steer_angle_limits from openpilot.selfdrive.car.ford import fordcan from openpilot.selfdrive.car.ford.values import CarControllerParams, FordFlags -from openpilot.selfdrive.car.helpers import clip +from openpilot.selfdrive.car.common.numpy_fast import clip from openpilot.selfdrive.car.interfaces import CarControllerBase, V_CRUISE_MAX LongCtrlState = car.CarControl.Actuators.LongControlState diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index dff280ed9a..60d472620f 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -4,7 +4,7 @@ from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.gm import gmcan from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons -from openpilot.selfdrive.car.helpers import interp +from openpilot.selfdrive.car.common.numpy_fast import interp from openpilot.selfdrive.car.interfaces import CarControllerBase VisualAlert = car.CarControl.HUDControl.VisualAlert diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 2efe2b41e6..d6e729ef4a 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from openpilot.selfdrive.car import create_button_events from openpilot.selfdrive.car.conversions import Conversions as CV -from openpilot.selfdrive.car.helpers import mean +from openpilot.selfdrive.car.common.numpy_fast import mean from openpilot.selfdrive.car.interfaces import CarStateBase from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, CruiseButtons, STEER_THRESHOLD diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 0f08ee21a8..87b867d2b3 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -3,7 +3,7 @@ from collections import namedtuple from cereal import car from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import DT_CTRL, rate_limit, make_tester_present_msg -from openpilot.selfdrive.car.helpers import clip, interp +from openpilot.selfdrive.car.common.numpy_fast import clip, interp from openpilot.selfdrive.car.honda import hondacan from openpilot.selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams from openpilot.selfdrive.car.interfaces import CarControllerBase diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index d8ecba8303..1f4cec7758 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -5,7 +5,7 @@ from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from openpilot.selfdrive.car import create_button_events from openpilot.selfdrive.car.conversions import Conversions as CV -from openpilot.selfdrive.car.helpers import interp +from openpilot.selfdrive.car.common.numpy_fast import interp from openpilot.selfdrive.car.honda.hondacan import CanBus, get_cruise_speed_conversion from openpilot.selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, \ HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS, \ diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index c6c25ae359..28b6849769 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -2,7 +2,7 @@ from cereal import car from panda import Panda from openpilot.selfdrive.car.conversions import Conversions as CV -from openpilot.selfdrive.car.helpers import interp +from openpilot.selfdrive.car.common.numpy_fast import interp from openpilot.selfdrive.car.honda.hondacan import CanBus from openpilot.selfdrive.car.honda.values import CarControllerParams, HondaFlags, CAR, HONDA_BOSCH, \ HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index e121faa790..cfca7ab1d9 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -2,7 +2,7 @@ from cereal import car from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg from openpilot.selfdrive.car.conversions import Conversions as CV -from openpilot.selfdrive.car.helpers import clip +from openpilot.selfdrive.car.common.numpy_fast import clip from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican from openpilot.selfdrive.car.hyundai.carstate import CarState from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index bd46f18612..10c3095b9d 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -1,5 +1,5 @@ from openpilot.selfdrive.car import CanBusBase -from openpilot.selfdrive.car.helpers import clip +from openpilot.selfdrive.car.common.numpy_fast import clip from openpilot.selfdrive.car.hyundai.values import HyundaiFlags diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index ae0df0123c..556652f88f 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -10,11 +10,11 @@ from functools import cache from cereal import car from openpilot.common.basedir import BASEDIR -from openpilot.common.simple_kalman import KF1D, get_kalman_gain from openpilot.selfdrive.car import DT_CTRL, apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, get_friction, STD_CARGO_KG from openpilot.selfdrive.car.can_definitions import CanData, CanRecvCallable, CanSendCallable from openpilot.selfdrive.car.conversions import Conversions as CV -from openpilot.selfdrive.car.helpers import clip +from openpilot.selfdrive.car.common.simple_kalman import KF1D, get_kalman_gain +from openpilot.selfdrive.car.common.numpy_fast import clip from openpilot.selfdrive.car.values import PLATFORMS GearShifter = car.CarState.GearShifter diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index c8c2e7417b..ba8a87b2d5 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,6 +1,6 @@ from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg -from openpilot.selfdrive.car.helpers import clip, interp +from openpilot.selfdrive.car.common.numpy_fast import clip, interp from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.subaru import subarucan from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 8f7a7ed9dd..427796cae9 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,7 +1,7 @@ from cereal import car from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_tester_present_msg from openpilot.selfdrive.car.can_definitions import CanData -from openpilot.selfdrive.car.helpers import clip +from openpilot.selfdrive.car.common.numpy_fast import clip from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.toyota import toyotacan from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index fd8306ef11..bea4e40896 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -5,8 +5,8 @@ from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from openpilot.selfdrive.car import DT_CTRL, create_button_events from openpilot.selfdrive.car.conversions import Conversions as CV -from openpilot.selfdrive.car.filter_simple import FirstOrderFilter -from openpilot.selfdrive.car.helpers import mean +from openpilot.selfdrive.car.common.filter_simple import FirstOrderFilter +from openpilot.selfdrive.car.common.numpy_fast import mean from openpilot.selfdrive.car.interfaces import CarStateBase from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \ TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 4f8552b16a..cc76aaf493 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -2,7 +2,7 @@ from cereal import car from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits from openpilot.selfdrive.car.conversions import Conversions as CV -from openpilot.selfdrive.car.helpers import clip +from openpilot.selfdrive.car.common.numpy_fast import clip from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.volkswagen import mqbcan, pqcan from openpilot.selfdrive.car.volkswagen.values import CANBUS, CarControllerParams, VolkswagenFlags