autotune: Change id interface to work with SIPPY

SIPPY support will be added in the near future

remove state vector plot as not really useful and won't be available
later, when using SIPPY

Author: bresch

autotune/readme.md autotune/README.mdautotune/readme.md renamed to autotune/README.md

@@ -452,55 +452,27 @@ def runIdentification(self):
         n = self.sys_id_n_poles # order of the denominator (a_1,...,a_n)
         m = self.sys_id_n_zeros # order of the numerator (b_0,...,b_m)
         d = self.sys_id_delays # number of delays
-        tau = 60.0 # forgetting period
-        lbda = 1.0 - self.dt/tau
-        self.sysid = SystemIdentification(n, m, d)
-        self.sysid.lbda = lbda
+        id = SystemIdentification(n, m, d, self.dt)
 
-        (theta_hat, a_coeffs, b_coeffs) = self.sysid.run(self.t, self.u, self.y)
+        est = id.fit(self.u.reshape(-1, 1), self.y.reshape(-1, 1))
 
-        self.plotStateVector(a_coeffs, b_coeffs)
+        self.num = est.G_.num_list[0][0]
+        self.den = est.G_.den_list[0][0][0:n+1]
+        self.Gz = ctrl.TransferFunction(self.num, self.den, self.dt)
+
+        num_coeffs = self.num
+        den_coeffs = self.den[1:n+1]
         self.is_system_identified = True
         self.btn_run_sys_id.setEnabled(False)
-        dt = self.dt
-        # num = self.sysid.getNum()
-        # den = self.sysid.getDen()
-        # self.Gz_dot = ctrl.TransferFunction(num, den, dt)
 
-        # Uncomment below to add integrator
-        # self.sysid.addIntegrator()
-        self.num = self.sysid.getNum()
-
-        self.den = self.sysid.getDen()
-
-        self.Gz = ctrl.TransferFunction(self.num, self.den, dt)
-        self.updateTfDisplay(a_coeffs[:, -1], b_coeffs[:, -1])
+        self.updateTfDisplay(den_coeffs, num_coeffs)
         self.plotPolesZeros()
         self.replayInputData()
 
-    def plotStateVector(self, a_coeffs, b_coeffs):
-        t = self.t
-        ax = self.figure.add_subplot(3,3,(4,5))
-        legend = []
-
-        for i in range(len(a_coeffs)):
-            ax.plot(t, a_coeffs[i])
-            legend.append("a{}".format(i+1))
-
-        for i in range(len(b_coeffs)):
-            ax.plot(t, b_coeffs[i])
-            legend.append("b{}".format(i))
-
-        ax.set_title("Parameter identification")
-        ax.set_xlabel("Time (s)")
-        ax.legend(legend, loc='lower left')
-
-        self.canvas.draw()
-
     def replayInputData(self):
         if not self.is_system_identified:
             return
-        d = self.sysid.d
+        d = self.sys_id_delays
         u_detrended = detrend(self.u)
         u_delayed = np.concatenate(([0 for k in range(d)], u_detrended[0:(len(u_detrended)-d)]))
         self.t_est, self.y_est = ctrl.forced_response(self.Gz, T=self.t, U=u_delayed)

autotune/autotune.py

@@ -44,23 +44,30 @@
 from arx_rls import ArxRls
 from scipy.optimize import lsq_linear, minimize
 
+class SysIdResult(object):
+    def __init__(self, num, den, dt):
+        self.G_ = ctrl.TransferFunction(num, den, dt)
+
 class SystemIdentification(object):
-    def __init__(self, n=2, m=2, d=1):
+    def __init__(self, n=2, m=2, d=1, dt = 0.0):
         self.n = n # order of the denominator (a_1,...,a_n)
         self.m = m # order of the numerator (b_0,...,b_m)
         self.d = d # number of delays
         self.forgetting_tc = 60.0 # forgetting factor for weighted RLS in seconds
         self.f_hp = 0.5 # high-pass filter cutoff frequency
         self.f_lp = 30.0 # low-pass filter cutoff frequency
+        self.dt = dt
+
+        tau = 60.0 # forgetting period
+        self.lbda = 1.0 - self.dt/tau
 
-    def run(self, t, u, y):
-        n_steps = len(t)
-        dt = t[1] - t[0]
+    def fit(self, u, y):
+        n_steps = len(u)
 
         # High-pass filter parameters
         if self.f_hp > 0.0:
             tau_hp = 1/(2*np.pi*self.f_hp)
-            alpha_hp = tau_hp/(tau_hp+dt)
+            alpha_hp = tau_hp/(tau_hp+self.dt)
         else:
             alpha_hp = 0.0
 
@@ -71,7 +78,7 @@ def run(self, t, u, y):
 
         # Low-pass filter parameters
         tau_lp = 1/(2*np.pi*self.f_lp)
-        alpha_lp = tau_lp/(tau_lp+dt)
+        alpha_lp = tau_lp/(tau_lp+self.dt)
         u_lp = np.zeros(n_steps)
         y_lp = np.zeros(n_steps)
         u_lp[0] = u[0]
@@ -97,7 +104,7 @@ def run(self, t, u, y):
         use_rls = True
         if use_rls:
             # Identification
-            rls = ArxRls(self.n, self.m, self.d, lbda=(1 - dt / self.forgetting_tc))
+            rls = ArxRls(self.n, self.m, self.d, lbda=(1 - self.dt / self.forgetting_tc))
 
             for k in range(n_steps):
                 # Update model
@@ -126,7 +133,7 @@ def run(self, t, u, y):
             theta_hat = res.x
 
             # Refine model using output-error optimization
-            J = lambda x: np.sum(np.power(abs(np.array(B)-self.simulateModel(x, u_lp, dt)), 2.0)) # cost function
+            J = lambda x: np.sum(np.power(abs(np.array(B)-self.simulateModel(x, u_lp, self.dt)), 2.0)) # cost function
             x0 = np.append(res.x, 0) # initial conditions
             res = minimize(J, x0, method='nelder-mead', options={'disp': True})
             theta_hat = res.x
@@ -138,7 +145,8 @@ def run(self, t, u, y):
 
         self.theta_hat = theta_hat
 
-        return (theta_hat, a_coeffs, b_coeffs)
+        estimate = SysIdResult(self.getNum(), self.getDen(), self.dt)
+        return estimate
 
     def simulateModel(self, x, u, dt):
         a_coeffs = np.ones(self.n+1)