From d334798141a22ab72b1fe786fdb1f3badf681e50 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Thu, 24 Aug 2023 15:04:47 -0700 Subject: [PATCH] KF1D: add steady state kalman code old-commit-hash: e7418cfa397ec2185e4b12baaa481017b7b23322 --- common/kalman/simple_kalman.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/common/kalman/simple_kalman.py b/common/kalman/simple_kalman.py index 31ad76c5b2..669da3919a 100644 --- a/common/kalman/simple_kalman.py +++ b/common/kalman/simple_kalman.py @@ -1,3 +1,13 @@ # pylint: skip-file 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 \ No newline at end of file