Merge pull request #66 from Gavin-Furtado/Experimenting

Kalman gain matrix & Process covariance matrix

Phase 2/filter_py_lib_test/high_level.py

@@ -19,6 +19,7 @@
 '''
 Things to go
 1. verify the Process covarinace matrix, the last two rows do not seem to change
+    I checked this on matrix calculator the numbers are right
 2. build kalman filter class
 3. Measurement matrix Y
 '''
@@ -91,13 +92,13 @@ def main():
 
     ## Initalisation ##
     covar = kal.kalman_initial(position,velocity, acceleration)
-    P_initial = covar.P_initial(2,1,5,3)
+    P_initial = covar.P_initial(2,1,8,7)
 
     # P_predict = kal.Prediction(None,None,None)
     # print(P_predict.P_predicted(P_initial))
 
     P_prev = P_initial
-
+    
     ## Kalman Filter Loop ##
     for idx, (pos,vel,acc) in enumerate(zip(position,velocity,acceleration)):    
         state_matrix = np.array([[pos[0]],
@@ -116,13 +117,19 @@ def main():
         predict_state = predict.X_predicted()
         predict_P = predict.P_predicted()
 
+        # Kalman Gain
+        R = kal.R_matrix(4,5,2,9)
+        K = kal.KalmanGain(predict_P,R)
+
+        print(state_matrix)
         '''
-        Now you need kalman gain
+        Y matrix is just the same as state matrix without Z
         '''
 
         # Updation
         P_prev = predict_P
-        print(predict_P, P_prev)
+        # print(predict_P, P_prev)
+        # print(predict_P)
         '''
         The last two rows are not changing
         '''

Phase 2/filter_py_lib_test/kf_computation.py

@@ -146,7 +146,7 @@ def P_predicted(self):
         A.P_prev.A^T + Q
         '''
         prev = self.P_previous
-        P = A_matrix(prev)@prev@A_matrix(prev).transpose() #+Q
+        P = A_matrix(prev)@prev@(A_matrix(prev).transpose()) #+Q
 
         ## Simplyfing the calcuations
         P[0][1] = 0 
@@ -169,10 +169,61 @@ def P_predicted(self):
 
 
 ## Step 3 - Kalman Gain ##
-class KalmanGain(object):
-    def __init__(self)-> None:
-        pass
+def H_matrix():
+    '''
+    Transformation matrix for easy of multiplication.
+    In this case it is 4x4 identity matrix
+    '''
+    return np.array([[1,0,0,0],
+                      [0,1,0,0],
+                      [0,0,1,0],
+                      [0,0,0,1],])
+
+def R_matrix(x_pos_obs_err,y_pos_obs_err,x_vel_obs_err,y_vel_obs_err):
+    '''
+    Observation error matrix
+
+    Attributes
+    ----------
+    x_pos : x-position observation error
+    y_pos : y-position observation error
+    x_vel : x-velocity observation error
+    y_vel : y-velocity observation error
+
+    Matrix
+    ------
+    np.array([[x-position-observation-error**2,0,0,0],
+              [0,y-position-observation-error**2,0,0],
+              [0,0,x-velocity-observation-error**2,0],
+              [0,0,0,y-velocity-observation-error**2]])
+    '''
+    R = np.array([[x_pos_obs_err**2,0,0,0],
+                    [0,y_pos_obs_err**2,0,0],
+                    [0,0,x_vel_obs_err**2,0],
+                    [0,0,0,y_vel_obs_err**2]])
+    return R
+
+
+def KalmanGain(P, R):
+    '''
+    K = P_predict . H^T / (H.P_predict.H^T  + R)
+    '''
+    numerator = P@H_matrix().transpose()
+    denominator = H_matrix()@P@H_matrix().transpose() + R
+    k = numerator @ np.linalg.inv(denominator)
+    return k
 
+## Step 4 - Measurement input
+def measurement_input():
+    '''
+    Y = C.X + Z
+    '''
+    C = np.array([[1,0,0,0],
+                  [0,1,0,0],
+                  [0,0,1,0],
+                  [0,0,0,1],])
+
+    return 
 
 ## Step 4 - Update measurement & Kalman Gain ##
 class updation(object):