Replaced dot with np.dot

This commit is contained in:
Andreas Tsouchlos 2022-04-08 03:18:57 +02:00
parent 5adc39fc1d
commit 99beba66e5
2 changed files with 11 additions and 12 deletions

View File

@ -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

View File

@ -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)