From 55e180ba4654b4f40db25259b8a76e38a0f49cc7 Mon Sep 17 00:00:00 2001 From: Andreas Tsouchlos Date: Mon, 25 Apr 2022 10:59:52 +0200 Subject: [PATCH] Fixed 2 possible bugs in Kalman Filter implementation --- filter/kalman_filter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/filter/kalman_filter.py b/filter/kalman_filter.py index af40a99..249d677 100644 --- a/filter/kalman_filter.py +++ b/filter/kalman_filter.py @@ -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)