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)