From 99beba66e5bcc3e5110701f1369fe07178fb8e5d Mon Sep 17 00:00:00 2001 From: Andreas Tsouchlos Date: Fri, 8 Apr 2022 03:18:57 +0200 Subject: [PATCH] Replaced dot with np.dot --- kalman_filter/__main__.py | 4 ++-- kalman_filter/kalman_filter.py | 19 +++++++++---------- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/kalman_filter/__main__.py b/kalman_filter/__main__.py index 5aea9e9..6f540f3 100644 --- a/kalman_filter/__main__.py +++ b/kalman_filter/__main__.py @@ -56,11 +56,11 @@ def main(): for i in range(1, n_iterations): - measurement = dot(F,x_actual[i-1]) + np.random.normal([0,0], std_dist) + measurement = np.dot(F,x_actual[i-1]) + np.random.normal([0,0], std_dist) f.step(measurement, R, 0) - x_actual[i] = dot(F, x_actual[i-1]) + x_actual[i] = np.dot(F, x_actual[i-1]) x_kalman[i] = f.x x_measurement[i] = measurement diff --git a/kalman_filter/kalman_filter.py b/kalman_filter/kalman_filter.py index f36085e..2ceb5de 100644 --- a/kalman_filter/kalman_filter.py +++ b/kalman_filter/kalman_filter.py @@ -1,5 +1,4 @@ import numpy as np -from numpy import dot # @@ -43,28 +42,28 @@ class KalmanFilter: def update(self, z, R): # Calculate Kalman Gain - temp = dot(self.H, dot(self.P,self.H)) + R + temp = np.dot(self.H, np.dot(self.P,self.H)) + R inv = inv_gen(temp) - K = dot(self.P, dot(np.transpose(self.H), inv)) + K = np.dot(self.P, np.dot(np.transpose(self.H), inv)) # Update state - innovation = z - dot(self.H, self.x) - self.x = self.x + dot(K, innovation) + innovation = z - np.dot(self.H, self.x) + self.x = self.x + np.dot(K, innovation) # Update uncertainty - KH = dot(K, self.H) + KH = np.dot(K, self.H) K_complementary = identity_gen(KH) - KH - self.P = dot(K_complementary, dot(self.P, np.transpose(K_complementary))) \ - + dot(K, dot(R, np.transpose(K))) + self.P = np.dot(K_complementary, np.dot(self.P, np.transpose(K_complementary))) \ + + np.dot(K, np.dot(R, np.transpose(K))) def predict(self, u): - self.x = dot(self.F, self.x) + dot(self.G,u) - self.P = dot(self.F, dot(self.P, inv_gen(self.F))) + self.Q + 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 def step(self, z, R, u): self.predict(u)