autotune: Change id interface to work with SIPPY

SIPPY support will be added in the near future

Author: bresch

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

@@ -452,29 +452,20 @@ 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()
 
@@ -500,7 +491,7 @@ def plotStateVector(self, a_coeffs, b_coeffs):
     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)