B_matrix

Phase 2/filter_py_lib_test/kf_test.py

@@ -159,10 +159,16 @@ def gaussian_plot(self):
 
 print(position_data[3])
 print(velocity_data[3])
+print(acceleration_data[3])
 
 X = np.array([[position_data[3][0]],[position_data[3][1]],
               [velocity_data[3][0]],[velocity_data[3][1]]])
-print(X.size)
+u = np.array([[acceleration_data[3][0]],
+              [acceleration_data[3][1]]])
+# print(X)
+# print(X.shape)
+# print(u)
+# print(u.shape)
 
 ## Using FilterPy library
 
@@ -208,7 +214,7 @@ def gaussian_plot(self):
 
 ## Step 1 - Predicted State ##
 class Prediction(object):
-    def __init__(self, X_previous, P_previous, dt=2., u=0, w=0, Q=0) -> None:
+    def __init__(self, X_previous, P_previous, u, dt=2., w=0, Q=0) -> None:
         self.X_previous = X_previous
         self.u = u
         self.w = w
@@ -234,8 +240,8 @@ def A_matrix(self):
         return self.A
 
     def B_matrix(self):
-        u_shape = self.u.shape
-
+        u_shape = self.u.shape # 2,1
+        
         if u_shape[0] == 1: 
             self.B = np.array([[0.5*self.dt**2],
                                [self.dt**2]])
@@ -248,13 +254,15 @@ def B_matrix(self):
         return self.B
 
     def X_predicted(self):
-        pass
+        return (self.A_matrix()@self.X_previous) + (self.B_matrix()@self.u) #+ self.w 
 
     def P_predicted(self):
         pass
 
-predict = Prediction(X,0)
-print(predict.A_matrix())
+predict = Prediction(X,0,u) 
+print(predict.X_predicted())
+# print(predict.u)
+print(predict.B_matrix())
 
 ## Step 2 - Measurement from sensor ##