Replaced dot with np.dot
This commit is contained in:
parent
5adc39fc1d
commit
99beba66e5
@ -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
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user