added python kalman filter and added timing into Use example along with plotting

Author: AshleySetter

KalmanFilterC/KalmanFilterPurePython.py

@@ -0,0 +1,32 @@
+import numpy as np
+
+# Implements a linear Kalman filter.
+class KalmanFilterLinear:
+    def __init__(self,_A, _B, _H, _x, _P, _Q, _R):
+        self.A = _A                      # State transition matrix.
+        self.B = _B                      # Control-input matrix.
+        self.H = _H                      # Observation model matrix.
+        self.current_state_estimate = _x # Initial state estimate.
+        self.current_prob_estimate = _P  # Initial covariance estimate.
+        self.Q = _Q                      # Estimated error in process.
+        self.R = _R                      # Estimated error in measurements.
+        self.KG = None
+    def GetCurrentState(self):
+        return self.current_state_estimate
+    def Step(self,control_vector,measurement_vector):
+        #---------------------------Prediction step-----------------------------
+        predicted_state_estimate = self.A * self.current_state_estimate + self.B * control_vector
+        
+        predicted_prob_estimate = (self.A * self.current_prob_estimate) * np.transpose(self.A) + self.Q
+        #--------------------------Observation step-----------------------------
+        innovation = measurement_vector - self.H*predicted_state_estimate
+        self.Innovation = innovation[0, 0]
+        innovation_covariance = self.H*predicted_prob_estimate*np.transpose(self.H) + self.R
+        #-----------------------------Update step-------------------------------
+        kalman_gain = predicted_prob_estimate * np.transpose(self.H) * np.linalg.inv(innovation_covariance)
+        self.KG = kalman_gain
+        self.current_state_estimate = predicted_state_estimate + kalman_gain * innovation
+        # We need the size of the matrix so we can make an identity matrix.
+        size = self.current_prob_estimate.shape[0]
+        # eye(n) = nxn identity matrix.
+        self.current_prob_estimate = (np.eye(size)-kalman_gain*self.H)*predicted_prob_estimate

KalmanFilterC/UseKalmanFilterCython.py

@@ -1,9 +1,12 @@
 import numpy as np
 import sympy
 import matplotlib.pyplot as plt
+import timeit
 
 import KalmanFilter
+import KalmanFilterPurePython
 
+# Calculate parameters for Kalman filter
 t = sympy.symbols('t', real = True, positive=True)
 s = sympy.symbols('s', real = True, positive=True)
 w = sympy.symbols('w', real = True, positive=True)
@@ -31,7 +34,8 @@ def TransformSystemsDynamicsMatrix(F):
 class data:
         pass
 
-data.time = np.arange(0, 10*2*np.pi/w, Ts)
+data.time = np.arange(0, 100*2*np.pi/w, Ts) # 100 periods of oscillation
+print("{} Points to be filtered".format(len(data.time)))
 noiseAmp = 5
 data.trueSignal = 300 + 5*np.sin(w*data.time + 0)
 data.signal = data.trueSignal + np.random.normal(0, noiseAmp, len(data.trueSignal))
@@ -59,12 +63,40 @@ class data:
 
 print("Executing Filtering")
 
+start_time = timeit.default_timer()
 ResultData = kf.FilterData(data.signal, len(data.signal))
+elapsed = timeit.default_timer() - start_time
+print("C++ filtering took {}s".format(elapsed))
 
+print("Constructing Python Filter")
+
+KF2 =  KalmanFilterPurePython.KalmanFilterLinear(A, B, H, x_init, P, Q, R)
+
+print("Executing Python Filtering")
+
+start_time = timeit.default_timer()
+KalmanPredictionArray = []
+KalmanErrorArray = []
+KalmanGainArray = []
+for i, x in enumerate(data.signal):
+    X_state = np.matrix('{0} ; {1}'.format(x, 0))
+    KF2.Step(np.matrix([0]), np.matrix([x]))
+    KalmanPredictionArray.append(KF2.current_state_estimate)
+    KalmanErrorArray.append(KF2.current_prob_estimate)
+    KalmanGainArray.append(KF2.KG)
+    
+KalmanPredictionArray = np.array(KalmanPredictionArray)
+KalmanErrorArray = np.array(KalmanErrorArray)
+KalmanGainArray = np.array(KalmanGainArray)
+
+x = KalmanPredictionArray[:, 0] + KalmanPredictionArray[:, 2]
+elapsed = timeit.default_timer() - start_time
+print("C++ filtering took {}s".format(elapsed))
 
 plt.figure(figsize=[10, 10])
 plt.plot(data.time, data.signal, label="noisy sine wave", color='blue', alpha=0.6)
 plt.plot(data.time, data.trueSignal, label="pure source sine wave", lw=3, color='red', alpha=0.8)
+plt.plot(data.time, x, label="Pure Python Kalman Filter Output", lw=3, color='darkblue', alpha=0.9)
 plt.plot(data.time, ResultData, label="C++ Kalman Filter Output", lw=3, color='darkred', alpha=0.9)
 
 plt.legend(loc="best")