Copy kalman filter to selfdrive/car (#33307)

move to car/common/ and give proper names
old-commit-hash: bcfb50d98c

.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
 

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')

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]

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

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

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
 

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

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, \

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

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

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
 
 

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

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

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, \

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

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