mirror of
https://github.com/infiniteCable2/openpilot.git
synced 2026-08-22 00:43:44 +08:00
Shane Smiskol
f5d1c4f371
* check newline * nothing catches this old-commit-hash: 97da129e11e58b07d5d136093827cd1c62311fea
13 lines
366 B
Python
13 lines
366 B
Python
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
|