an.tsouchlos/kalman_filter
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Fixed 2 possible bugs in Kalman Filter implementation

Browse Source
This commit is contained in:
Andreas Tsouchlos 2022-04-25 10:59:52 +02:00
parent 5f0274fbff
commit 55e180ba46
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
filter/kalman_filter.py
View File

@ -42,7 +42,7 @@ class LinearKalmanFilter:
def update(self, z, R):
# Calculate Kalman Gain
temp = np.dot(self.H, np.dot(self.P,self.H)) + R
temp = np.dot(self.H, np.dot(self.P, np.transpose(self.H))) + R
inv = inv_gen(temp)
K = np.dot(self.P, np.dot(np.transpose(self.H), inv))
@ -63,7 +63,7 @@ class LinearKalmanFilter:
def predict(self, u):
self.x = np.dot(self.F, self.x) + np.dot(self.G,u)
self.P = np.dot(self.F, np.dot(self.P, inv_gen(self.F))) + self.Q
self.P = np.dot(self.F, np.dot(self.P, np.transpose(self.F))) + self.Q
def step(self, z, R, u):
self.predict(u)