From 8960f7659784c3295fba32adc6b096064deb5462 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= <harald.the.engineer@gmail.com>
Date: Fri, 25 Aug 2023 13:44:03 -0700
Subject: [PATCH] K1FD: generate kalman gain at init (#29610)

* K1FD: generate kalman gain at init

* Update interfaces.py

* Update interfaces.py

* Update interfaces.py

* Update ref_commit
old-commit-hash: f1b8a8646438008b35c0445812b818745ad01fc2
---
 selfdrive/car/interfaces.py              | 16 +++++++++-------
 selfdrive/test/process_replay/ref_commit |  2 +-
 2 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index 53b3e381f..1b68b1dbc 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -1,13 +1,14 @@
 import yaml
 import os
 import time
+import numpy as np
 from abc import abstractmethod, ABC
 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
+from openpilot.common.kalman.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
@@ -335,12 +336,13 @@ class CarStateBase(ABC):
     self.cluster_speed_hyst_gap = 0.0
     self.cluster_min_speed = 0.0  # min speed before dropping to 0
 
-    # Q = np.matrix([[0.0, 0.0], [0.0, 100.0]])
-    # R = 0.3
-    self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
-                         A=[[1.0, DT_CTRL], [0.0, 1.0]],
-                         C=[1.0, 0.0],
-                         K=[[0.17406039], [1.65925647]])
+    Q = [[0.0, 0.0], [0.0, 100.0]]
+    R = 0.3
+    A = [[1.0, DT_CTRL], [0.0, 1.0]]
+    C = [[1.0, 0.0]]
+    x0=[[0.0], [0.0]]
+    K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R)
+    self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K)
 
   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
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index d5ff4b606..955c5c7e9 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-9d6369c5aa88c37499c330a06aa7a81a5f4f3283
\ No newline at end of file
+3fd85c41e61044a86bf364b9a7a0470e5758f231