add filter combo

peeper/analysis/cli.py

@@ -67,24 +67,16 @@ def analyze_motors(folder):
         os.path.join(folder, "AMK1_{}.csv".format(motor))
         for motor in MOTOR_LABELS
     ]
-    ti_file = os.path.join(folder, "TI.csv")
 
     # 2 x 2 plots
-
-    ti_plotter = Plotter(ti_file)
-    ti_plots = []  # ["throttle (%)", "brake (%)"]
-
     for i, file in enumerate(files):
         plt.subplot(2, 2, i + 1)  # select subplot
 
         driver = Plotter(file)
-        driver.plot_filter("actual velocity (x100 rpm)", kalman)
 
         combo_inputs = ["actual velocity (x100 rpm)", "calc torque (Nm)"]
-        driver.plot_combo(combo_inputs, current_combo, "current (A)")
-
-        for k in ti_plots:
-            ti_plotter.plot(k)
+        current_values = driver._get_combo(combo_inputs, current_combo)
+        driver.plot_values(current_values, "current (A)", with_filter=kalman)
 
     folder_name = get_folder_name(folder)
     title = '{}'.format(folder_name)
@@ -95,7 +87,6 @@ def analyze_motors(folder):
 def main():
     input_path = parse_args(create_args())
     analyze_motors(input_path)
-    # analyze_test(file)
 
 
 if __name__ == '__main__':

peeper/analysis/models.py

@@ -5,22 +5,9 @@
 import matplotlib.pyplot as plt
 import numpy as np
 import pandas as pd
-from scipy.optimize import curve_fit
 
-
-def interpol_function(x, a, b, c):
-    # return a * (1 - np.power(x, b)) + c
-    return a * np.log(np.multiply(x, b)) + c
-    # return a - np.divide(b, np.multiply(x, c))
-
-
-def log_interpol_events(x, y):
-    coeffs, covariance = curve_fit(interpol_function, x, y)
-
-    def interpol(x_data):
-        return interpol_function(x_data, *coeffs)
-
-    return interpol, coeffs
+from analysis.tools.combos import get_combo
+from analysis.tools.interpol import log_interpol_events
 
 
 class Plotter:
@@ -66,6 +53,10 @@ def _pretty_number(x):
 
         return x
 
+    @staticmethod
+    def _plot_data(x, y, label):
+        plt.plot(x, y, label=label)
+
     @staticmethod
     def _plot_trend(x, y, label):
         interpol, coeffs = log_interpol_events(x, y)
@@ -87,6 +78,16 @@ def _finalize():
         plt.xlabel('Time (s)')
         plt.legend()
 
+    def _get_time_index(self):
+        return self.data.index.tolist()
+
+    def _get_combo(self, labels, f):
+        values = [
+            self.plots[label].values.tolist()
+            for label in labels
+        ]
+        return get_combo(values, f)
+
     def plot(self, column_name, with_trend=False):
         df = self.plots[column_name]
         x, y = df.index.tolist(), df.values.tolist()
@@ -97,12 +98,19 @@ def plot(self, column_name, with_trend=False):
 
         self._finalize()
 
+    def plot_values(self, values, label, with_filter=None):
+        x = self._get_time_index()
+        plt.plot(x, values, label=label)
+
+        if with_filter:
+            y_filtered = with_filter(values)
+            label = '{} (filtered)'.format(label)
+            plt.plot(x, y_filtered, label=label)
+
+        self._finalize()
+
     def plot_combo(self, labels, f, label, with_trend=False):
-        values = [
-            self.plots[label].values.tolist()
-            for label in labels
-        ]
-        y = f(*values)
+        y = self._get_combo(labels, f)
         x = self.plots[labels[0]].index.tolist()  # the same for all
         plt.plot(x, y, label=label)
 

peeper/analysis/tools/combos.py

@@ -3,3 +3,7 @@ def current_combo(rpms, torques):
         rpms[j] * 100 / 60 * 2 * 3.14159265354 * torques[j] / 350
         for j in range(len(rpms))
     ]
+
+
+def get_combo(values, f):
+    return f(*values)

peeper/analysis/tools/filters.py

@@ -1,2 +1,47 @@
-def kalman(values):
-    return values  # todo implement
+import numpy as np
+
+
+class KalmanFilter:
+    def __init__(self, F=None, B=None, H=None, Q=None, R=None, P=None, x0=None):
+        self.n = F.shape[1]
+        self.m = H.shape[1]
+
+        self.F = F
+        self.H = H
+        self.B = 0 if B is None else B
+        self.Q = np.eye(self.n) if Q is None else Q
+        self.R = np.eye(self.n) if R is None else R
+        self.P = np.eye(self.n) if P is None else P
+        self.x = np.zeros((self.n, 1)) if x0 is None else x0
+
+    def predict(self, u=0):
+        self.x = np.dot(self.F, self.x) + np.dot(self.B, u)
+        self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q
+        return self.x
+
+    def update(self, z):
+        y = z - np.dot(self.H, self.x)
+        S = self.R + np.dot(self.H, np.dot(self.P, self.H.T))
+        K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
+        self.x = self.x + np.dot(K, y)
+        I = np.eye(self.n)
+        self.P = np.dot(np.dot(I - np.dot(K, self.H), self.P),
+                        (I - np.dot(K, self.H)).T) + np.dot(np.dot(K, self.R), K.T)
+
+
+def kalman(measurements):
+    dt = 0.001
+    F = np.array([[1, dt, 0], [0, 1, dt], [0, 0, 1]])
+    H = np.array([1, 0, 0]).reshape(1, 3)
+    Q = np.array([[0.0001, 0.0001, 0.0], [0.0001, 0.0001, 0.0], [0.0001, 0.0001, 0.0]])
+    R = np.array([1]).reshape(1, 1)
+
+    kf = KalmanFilter(F=F, H=H, Q=Q, R=R)
+    predictions = []
+
+    for z in measurements:
+        prediction = np.dot(H, kf.predict())[0]
+        predictions.append(prediction)
+        kf.update(z)
+
+    return predictions

peeper/analysis/tools/interpol.py

@@ -0,0 +1,18 @@
+import numpy as np
+
+from scipy.optimize import curve_fit
+
+
+def interpol_function(x, a, b, c):
+    # return a * (1 - np.power(x, b)) + c
+    return a * np.log(np.multiply(x, b)) + c
+    # return a - np.divide(b, np.multiply(x, c))
+
+
+def log_interpol_events(x, y):
+    coeffs, covariance = curve_fit(interpol_function, x, y)
+
+    def interpol(x_data):
+        return interpol_function(x_data, *coeffs)
+
+    return interpol, coeffs