def GEN_RLS_id(id_method, y, u, na, nb, nc, nd, nf, theta, max_iterations): ylength = y.size # input/output number m = 1; p = 1 # max number of non predictable data nbth = nb + theta val = max(na, nbth, nc, nd, nf) # whole data N = ylength # Total Order: both LTI and time varying part nt = na + nb + nc + nd + nf + 1 nh = max([na,nc]) ## Iterative Identification Algorithm ## Parameters Initialization # Confidence Parameter Beta = 1e4 # Covariance matrix of parameter teta p_t = Beta*np.eye(nt-1,nt-1) P_t = np.zeros((nt-1,nt-1,N)) for i in range(N): P_t[:,:,i] = p_t # Gain K_t = np.zeros((nt-1,N)) # First estimatate teta = np.zeros((nt-1,N)) eta = np.zeros(N) # Forgetting factors L_t = 1 l_t = L_t*np.ones(N) # Yp = np.zeros(N) E = np.zeros(N) fi = np.zeros((1,nt-1,N)) ## Propagation for k in range(N): if k > val: ## Step 1: Regressor vector vecY = y[k-na:k][::-1] # Y vector vecYp = Yp[k-nf:k][::-1] # Yp vector # vecU = u[k-nb-theta:k-theta][::-1] # U vector # #vecE = E[k-nh:k][::-1] # E vector vecE = E[k-nc:k][::-1] # choose input-output model if id_method == 'ARMAX': fi[:,:,k] = np.hstack((-vecY, vecU, vecE)) elif id_method == 'ARX': fi[:,:,k] = np.hstack((-vecY, vecU)) elif id_method == 'OE': fi[:,:,k] = np.hstack((-vecYp, vecU)) elif id_method == 'FIR': fi[:,:,k] = np.hstack((vecU)) phi = fi[:,:,k].T ## Step 2: Gain Update # Gain of parameter teta K_t[:,k:k+1] = np.dot(np.dot(P_t[:,:,k-1],phi),np.linalg.inv(l_t[k-1] + np.dot(np.dot(phi.T,P_t[:,:,k-1]),phi))) ## Step 3: Parameter Update teta[:,k] = teta[:,k-1] + np.dot(K_t[:,k:k+1],(y[k] - np.dot(phi.T,teta[:,k-1]))) ## Step 4: A posteriori prediction-error Yp[k] = np.dot(phi.T,teta[:,k]) + eta[k] E[k] = y[k] - Yp[k] ## Step 5. Parameter estimate covariance update P_t[:,:,k] = (1/l_t[k-1])*(np.dot(np.eye(nt-1) - np.dot(K_t[:,k:k+1],phi.T),P_t[:,:,k-1])) ## Step 6: Forgetting factor update l_t[k] = 1.0 # Error Norm Vn = old_div((np.linalg.norm(y - Yp, 2) ** 2), (2 * (N-val))) # Model Output y_id = Yp # Parameters THETA = teta[:,-1] # building TF coefficient vectors valH = max(nc, na + nd) valG = max(nb + theta, na + nf) # G # numG (B) if id_method == 'ARMA': NUM = 1.0 else: NUM = np.zeros(valG) ng = nf if id_method == 'OE' else na NUM[theta:nb + theta] = THETA[ng:nb+ng] # denG (A*F) A = cnt.tf(np.hstack((1, np.zeros((na)))), np.hstack((1, THETA[:na])),1) if id_method == 'OE': F = cnt.tf(np.hstack((1, np.zeros((nf)))), np.hstack((1, THETA[:nf])),1) else: F = cnt.tf(np.hstack((1, np.zeros((nf)))), np.hstack((1, THETA[na+nb+nc+nd:na+nb+nc+nd+nf])),1) _, deng = cnt.tfdata(A*F) denG = np.array(deng[0]) DEN = np.zeros(valG + 1) DEN[0:na+nf+1] = denG # H # numH (C) if id_method == 'OE': NUMH = 1 else: NUMH = np.zeros(valH + 1) NUMH[0] = 1. NUMH[1:nc + 1] = THETA[na+nb:na+nb+nc] # denH (A*D) D = cnt.tf(np.hstack((1, np.zeros((nd)))), np.hstack((1, THETA[na+nb+nc:na+nb+nc+nd])),1) _, denh = cnt.tfdata(A*D) denH = np.array(denh[0]) DENH = np.zeros(valH + 1) DENH[0:na+nd+1] = denH return NUM, DEN, NUMH, DENH, Vn, y_id
def do_sys_id(self): num_poles = 2 num_zeros = 1 if not self._use_subspace: method = 'ARMAX' #sys_id = system_identification(self._y.T, self._u[0], method, IC='BIC', na_ord=[0, 5], nb_ord=[1, 5], nc_ord=[0, 5], delays=[0, 5], ARMAX_max_iterations=300, tsample=self._Ts, centering='MeanVal') sys_id = system_identification(self._y.T, self._u[0], method, ARMAX_orders=[num_poles, 1, 1, 0], ARMAX_max_iterations=300, tsample=self._Ts, centering='MeanVal') print(sys_id.G) if self._verbose: print(sys_id.G) print("System poles of discrete G: ", cnt.pole(sys_id.G)) # Convert to continuous tf G = harold.Transfer(sys_id.NUMERATOR, sys_id.DENOMINATOR, dt=self._Ts) G_cont = harold.undiscretize(G, method='zoh') self._sys_tf = G_cont self._A, self._B, self._C, self._D = harold.transfer_to_state( G_cont, output='matrices') if self._verbose: print("Continuous tf:", G_cont) # Convert to state space, because ARMAX gives transfer function ss_roll = cnt.tf2ss(sys_id.G) A = np.asarray(ss_roll.A) B = np.asarray(ss_roll.B) C = np.asarray(ss_roll.C) D = np.asarray(ss_roll.D) if self._verbose: print(ss_roll) # simulate identified system using input from data xid, yid = fsetSIM.SS_lsim_process_form(A, B, C, D, self._u) y_error = self._y - yid self._fitness = 1 - (y_error.var() / self._y.var())**2 if self._verbose: print("Fittness %", self._fitness * 100) if self._plot: plt.figure(1) plt.plot(self._t[0], self._y[0]) plt.plot(self._t[0], yid[0]) plt.xlabel("Time") plt.title("Time response Y(t)=U*G(t)") plt.legend([ self._y_name, self._y_name + '_identified: ' + '{:.3f} fitness'.format(self._fitness) ]) plt.grid() plt.show() else: sys_id = system_identification(self._y, self._u, self._subspace_method, SS_fixed_order=num_poles, SS_p=self._subspace_p, SS_f=50, tsample=self._Ts, SS_A_stability=True, centering='MeanVal') #sys_id = system_identification(self._y, self._u, self._subspace_method, SS_orders=[1,10], SS_p=self._subspace_p, SS_f=50, tsample=self._Ts, SS_A_stability=True, centering='MeanVal') if self._verbose: print("x0", sys_id.x0) print("A", sys_id.A) print("B", sys_id.B) print("C", sys_id.C) print("D", sys_id.D) A = sys_id.A B = sys_id.B C = sys_id.C D = sys_id.D # Get discrete transfer function from state space sys_tf = cnt.ss2tf(A, B, C, D) if self._verbose: print("TF ***in z domain***", sys_tf) # Get numerator and denominator (num, den) = cnt.tfdata(sys_tf) # Convert to continuous tf G = harold.Transfer(num, den, dt=self._Ts) if self._verbose: print(G) G_cont = harold.undiscretize(G, method='zoh') self._sys_tf = G_cont self._A, self._B, self._C, self._D = harold.transfer_to_state( G_cont, output='matrices') if self._verbose: print("Continuous tf:", G_cont) # get zeros tmp_tf = cnt.ss2tf(self._A, self._B, self._C, self._D) self._zeros = cnt.zero(tmp_tf) # simulate identified system using discrete system xid, yid = fsetSIM.SS_lsim_process_form(A, B, C, D, self._u, sys_id.x0) y_error = self._y - yid self._fitness = 1 - (y_error.var() / self._y.var())**2 if self._verbose: print("Fittness %", self._fitness * 100) if self._plot: plt.figure(1) plt.plot(self._t[0], self._y[0]) plt.plot(self._t[0], yid[0]) plt.xlabel("Time") plt.title("Time response Y(t)=U*G(t)") plt.legend([ self._y_name, self._y_name + '_identified: ' + '{:.3f} fitness'.format(self._fitness) ]) plt.grid() plt.show()
def testSISOtfdata(self, siso): """Call tfdata()""" tfdata_1 = tfdata(siso.tf2) tfdata_2 = tfdata(siso.tf2) for i in range(len(tfdata_1)): np.testing.assert_array_almost_equal(tfdata_1[i], tfdata_2[i])
# 制御モデル import control from control.matlab import tf, tfdata P = tf([0, 1], [1, 2, 3]) print(P) [[numP]], [[denP]] = tfdata(P) print(numP, denP)
def GEN_id(id_method, y, u, na, nb, nc, nd, nf, theta, max_iterations, st_m, st_c): ylength = y.size # max predictable order val = max(nb + theta, na, nc, nd, nf) # input/output number m = 1 p = 1 # number of optimization variables n_coeff = na + nb + nc + nd + nf # Calling the optimization problem (solver, w_lb, w_ub, g_lb, g_ub) = opt_id(m, p, na, np.array([nb]), nc, nd, nf, n_coeff, np.array([theta]), val, np.atleast_2d(u), y, id_method, max_iterations, st_m, st_c) # Set first-guess solution w_0 = np.zeros((1, n_coeff)) w_y = np.zeros((1, ylength)) w_0 = np.hstack([w_0, w_y]) if id_method == 'BJ' or id_method == 'GEN' or id_method == 'ARARX' or id_method == 'ARARMAX': w_0 = np.hstack([w_0, w_y, w_y]) # Call the NLP solver sol = solver(lbx=w_lb, ubx=w_ub, x0=w_0, lbg=g_lb, ubg=g_ub) # model output: info from the solver f_opt = sol["f"] # objective function x_opt = sol["x"] # optimization variables = model coefficients iterations = solver.stats()['iter_count'] # iteration number y_id = x_opt[-ylength:].full()[:, 0] # model output THETA = np.array(x_opt[:n_coeff])[:, 0] # estimated error norm Vn = old_div((np.linalg.norm((y_id - y), 2)**2), (2 * ylength)) # building TF coefficient vectors valH = max(nc, na + nd) valG = max(nb + theta, na + nf) # G # numG (B) if id_method == 'ARMA': NUM = 1.0 else: NUM = np.zeros(valG) NUM[theta:nb + theta] = THETA[na:nb + na] # denG (A*F) A = cnt.tf(np.hstack((1, np.zeros((na)))), np.hstack((1, THETA[:na])), 1) F = cnt.tf(np.hstack((1, np.zeros((nf)))), np.hstack((1, THETA[na + nb + nc + nd:na + nb + nc + nd + nf])), 1) _, deng = cnt.tfdata(A * F) denG = np.array(deng[0]) DEN = np.zeros(valG + 1) DEN[0:na + nf + 1] = denG # H # numH (C) if id_method == 'OE': NUMH = 1 else: NUMH = np.zeros(valH + 1) NUMH[0] = 1. NUMH[1:nc + 1] = THETA[na + nb:na + nb + nc] # denH (A*D) D = cnt.tf(np.hstack((1, np.zeros((nd)))), np.hstack((1, THETA[na + nb + nc:na + nb + nc + nd])), 1) _, denh = cnt.tfdata(A * D) denH = np.array(denh[0]) DENH = np.zeros(valH + 1) DENH[0:na + nd + 1] = denH return NUM, DEN, NUMH, DENH, Vn, y_id
def GEN_MISO_id(id_method, y, u, na, nb, nc, nd, nf, theta, max_iterations, st_m, st_c): #nb = np.array(nb) #theta = np.array(theta) u = 1. * np.atleast_2d(u) ylength = y.size ystd, y = rescale(y) [udim, ulength] = u.shape eps = np.zeros(y.size) Reached_max = False # checking dimension if nb.size != udim: sys.exit( "Error! nb must be a matrix, whose dimensions must be equal to yxu" ) # return np.array([[1.]]),np.array([[0.]]),np.array([[0.]]),np.inf,Reached_max elif theta.size != udim: sys.exit("Error! theta matrix must have yxu dimensions") # return np.array([[1.]]),np.array([[0.]]),np.array([[0.]]),np.inf,Reached_max else: nbth = nb + theta Ustd = np.zeros(udim) for j in range(udim): Ustd[j], u[j] = rescale(u[j]) # max predictable dimension val = max(na, np.max(nbth), nc, nd, nf) # input/output number m = udim p = 1 # number of optimization variables n_coeff = na + np.sum(nb[:]) + nc + nd + nf # Build the optimization problem (solver, w_lb, w_ub, g_lb, g_ub) = opt_id(m, p, na, nb, nc, nd, nf, n_coeff, theta, val, np.atleast_2d(u), y, id_method, max_iterations, st_m, st_c) # Set first-guess solution w_0 = np.zeros((1, n_coeff)) w_0 = np.hstack([w_0, np.atleast_2d(y)]) if id_method == 'BJ' or id_method == 'GEN' or id_method == 'ARARX' or id_method == 'ARARMAX': w_0 = np.hstack([w_0, np.atleast_2d(y), np.atleast_2d(y)]) # Call the NLP solver sol = solver(lbx=w_lb, ubx=w_ub, x0=w_0, lbg=g_lb, ubg=g_ub) # model output: info from the solver f_opt = sol["f"] # objective function x_opt = sol["x"] # optimization variables = model coefficients iterations = solver.stats()['iter_count'] # iteration number y_id0 = x_opt[-ylength:].full()[:, 0] # model output THETA = np.array(x_opt[:n_coeff])[:, 0] # Check iteration numbers if iterations >= max_iterations: print("Warning! Reached maximum iterations") Reached_max = True # estimated error norm Vn = old_div((np.linalg.norm((y_id0 - y), 2)**2), (2 * ylength)) # rescaling Yid y_id = y_id0 * ystd # building TF coefficient vectors valH = max(nc, na + nd) valG = max(np.max(nbth), na + nf) Nb = np.sum(nb[:]) # H = (C/(A*D)) if id_method == 'OE': NUMH = np.ones((1, 1)) else: NUMH = np.zeros((1, valH + 1)) NUMH[0, 0] = 1. NUMH[0, 1:nc + 1] = THETA[na + Nb:na + Nb + nc] # # DENH = np.zeros((1, val + 1)) # DENH[0, 0] = 1. # DENH[0, 1:nd + 1] = THETA[Nb+na+nc:Nb+na+nc+nd] A = cnt.tf(np.hstack((1, np.zeros((na)))), np.hstack((1, THETA[:na])), 1) D = cnt.tf(np.hstack((1, np.zeros((nd)))), np.hstack((1, THETA[na + Nb + nc:na + Nb + nc + nd])), 1) _, denh = cnt.tfdata(A * D) denH = np.array(denh[0]) DENH = np.zeros((1, valH + 1)) DENH[0, 0:na + nd + 1] = denH # G = (B/(A*F)) F = cnt.tf( np.hstack((1, np.zeros((nf)))), np.hstack((1, THETA[na + Nb + nc + nd:na + Nb + nc + nd + nf])), 1) _, deng = cnt.tfdata(A * F) denG = np.array(deng[0]) DEN = np.zeros((udim, valG + 1)) #DEN = np.zeros((udim, den.shape[1] + 1)) #DEN = np.zeros((udim, den.shape[1])) #DEN[:, 0] = np.ones(udim) if id_method == 'ARMA': NUM = np.ones((udim, 1)) else: NUM = np.zeros((udim, valG)) # for k in range(udim): if id_method != 'ARMA': THETA[na + np.sum(nb[0:k]):na + np.sum(nb[0:k + 1] )] = THETA[na + np.sum(nb[0:k]):na + np.sum(nb[0:k + 1])] * ystd / Ustd[k] NUM[k, theta[k]:theta[k] + nb[k]] = THETA[na + np.sum(nb[0:k]):na + np.sum(nb[0:k + 1])] #DEN[k, 1:den.shape[1] + 1] = den #DEN[k,:] = den DEN[k, 0:na + nf + 1] = denG # check_stH = True if any(np.roots(DENH)>=1.0) else False # check_stG = True if any(np.roots(DEN)>=1.0) else False # if check_stH or check_stG: # IDsys_unst = # if st_c is True: # if solver.stats()['return_status'] != 'Maximum_Iterations_Exceeded': return DEN, NUM, NUMH, DENH, Vn, y_id, Reached_max
def GEN_RLS_MISO_id(id_method, y, u, na, nb, nc, nd, nf, theta, max_iterations): nb = np.array(nb) theta = np.array(theta) u = 1. * np.atleast_2d(u) ylength = y.size ystd, y = rescale(y) [udim, ulength] = u.shape eps = np.zeros(y.size) Reached_max = False # checking dimension if nb.size != udim: sys.exit( "Error! nb must be a matrix, whose dimensions must be equal to yxu" ) # return np.array([[1.]]),np.array([[0.]]),np.array([[0.]]),np.inf,Reached_max elif theta.size != udim: sys.exit("Error! theta matrix must have yxu dimensions") # return np.array([[1.]]),np.array([[0.]]),np.array([[0.]]),np.inf,Reached_max else: nbth = nb + theta Ustd = np.zeros(udim) for j in range(udim): Ustd[j], u[j] = rescale(u[j]) # max number of non predictable data val = max(na, np.max(nbth), nc, nd, nf) # whole data N = ylength # Total Order: both LTI and time varying part nt = na + np.sum(nb[:]) + nc + nd + nf + 1 nh = max([na, nc, nf]) ## Iterative Identification Algorithm ## Parameters Initialization # Confidence Parameter Beta = 1e4 # Covariance matrix of parameter teta p_t = Beta * np.eye(nt - 1, nt - 1) P_t = np.zeros((nt - 1, nt - 1, N)) for i in range(N): P_t[:, :, i] = p_t # Gain K_t = np.zeros((nt - 1, N)) # First estimatate teta = np.zeros((nt - 1, N)) #eta = np.zeros(N) # Forgetting factors L_t = 1 l_t = L_t * np.ones(N) # Yp = y.copy() E = np.zeros(N) fi = np.zeros((1, nt - 1, N)) ## Propagation for k in range(N): if k > val: ## Step 1: Regressor vector vecY = y[k - na:k][::-1] # Y vector vecYp = Yp[k - nf:k][::-1] # Yp vector # vecU = [] for nb_i in range(udim): # U vector vecu = u[nb_i, :][k - nb[nb_i] - theta[nb_i]:k - theta[nb_i]][::-1] vecU = np.hstack((vecU, vecu)) # #vecE = E[k-nh:k][::-1] # E vector vecE = E[k - nc:k][::-1] # choose input-output model if id_method == 'ARMAX': fi[:, :, k] = np.hstack((-vecY, vecU, vecE)) elif id_method == 'ARX': fi[:, :, k] = np.hstack((-vecY, vecU)) elif id_method == 'OE': fi[:, :, k] = np.hstack((-vecYp, vecU)) elif id_method == 'FIR': fi[:, :, k] = np.hstack((vecU)) phi = fi[:, :, k].T ## Step 2: Gain Update # Gain of parameter teta K_t[:, k:k + 1] = np.dot( np.dot(P_t[:, :, k - 1], phi), np.linalg.inv(l_t[k - 1] + np.dot(np.dot(phi.T, P_t[:, :, k - 1]), phi))) ## Step 3: Parameter Update teta[:, k] = teta[:, k - 1] + np.dot( K_t[:, k:k + 1], (y[k] - np.dot(phi.T, teta[:, k - 1]))) ## Step 4: A posteriori prediction-error Yp[k] = np.dot(phi.T, teta[:, k]) #+ eta[k] E[k] = y[k] - Yp[k] ## Step 5. Parameter estimate covariance update P_t[:, :, k] = (1 / l_t[k - 1]) * (np.dot( np.eye(nt - 1) - np.dot(K_t[:, k:k + 1], phi.T), P_t[:, :, k - 1])) ## Step 6: Forgetting factor update l_t[k] = 1.0 # Error Norm Vn = old_div((np.linalg.norm(y - Yp, 2)**2), (2 * (N - val))) # Model Output y_id = Yp * ystd # Parameters THETA = teta[:, -1] #if iterations >= max_iterations: # print("Warning! Reached maximum iterations") # Reached_max = True # building TF coefficient vectors valH = max(nc, na + nd) valG = max(np.max(nbth), na + nf) Nb = np.sum(nb[:]) # H = (C/(A*D)) if id_method == 'OE': NUMH = np.ones((1, 1)) else: NUMH = np.zeros((1, valH + 1)) NUMH[0, 0] = 1. NUMH[0, 1:nc + 1] = THETA[na + Nb:na + Nb + nc] # # DENH = np.zeros((1, val + 1)) # DENH[0, 0] = 1. # DENH[0, 1:nd + 1] = THETA[Nb+na+nc:Nb+na+nc+nd] A = cnt.tf(np.hstack((1, np.zeros((na)))), np.hstack((1, THETA[:na])), 1) D = cnt.tf(np.hstack((1, np.zeros((nd)))), np.hstack((1, THETA[na + Nb + nc:na + Nb + nc + nd])), 1) _, denh = cnt.tfdata(A * D) denH = np.array(denh[0]) DENH = np.zeros((1, valH + 1)) DENH[0, 0:na + nd + 1] = denH # G = (B/(A*F)) if id_method == 'OE': F = cnt.tf(np.hstack((1, np.zeros((nf)))), np.hstack((1, THETA[:nf])), 1) else: F = cnt.tf( np.hstack((1, np.zeros((nf)))), np.hstack( (1, THETA[na + Nb + nc + nd:na + Nb + nc + nd + nf])), 1) _, deng = cnt.tfdata(A * F) denG = np.array(deng[0]) DEN = np.zeros((udim, valG + 1)) #DEN = np.zeros((udim, den.shape[1] + 1)) #DEN = np.zeros((udim, den.shape[1])) #DEN[:, 0] = np.ones(udim) if id_method == 'ARMA': NUM = np.ones((udim, 1)) else: NUM = np.zeros((udim, valG)) # ng = nf if id_method == 'OE' else na for k in range(udim): if id_method != 'ARMA': THETA[ng + np.sum(nb[0:k]):ng + np.sum(nb[0:k + 1] )] = THETA[ng + np.sum(nb[0:k]):ng + np.sum(nb[0:k + 1])] * ystd / Ustd[k] NUM[k, theta[k]:theta[k] + nb[k]] = THETA[ng + np.sum(nb[0:k]):ng + np.sum(nb[0:k + 1])] #DEN[k, 1:den.shape[1] + 1] = den #DEN[k,:] = den DEN[k, 0:na + nf + 1] = denG return DEN, NUM, NUMH, DENH, Vn, y_id, Reached_max
MixerGain = 8 lpfoutput = (lpfoutputI + 1j * lpfoutputQ) # there are L Nyquist periods in one period of p(t) (for Tropp's analysis # Tx=Tp or L=N). L = N # 1-st order RC low-pass filter # En este fragmento de codigo se hallo la respuesta al impulso del filtro RC # Implementado analogamente. tau = 1 / (2 * pi * fc) B = 1 A = [tau, 1] lpf = tf(B, A) lpf_d = c2d(lpf, 1 / (float(W)), 'tustin') [[Bd]], [[Ad]] = tfdata(lpf_d) original = x ### Simulando la funcion impz de matlab. x = zeros(25) x[0] = 1 h = lfilter(Bd, Ad, x) # Finds closest time value to perform sampling i_tstart = argmin(abs(tspice - Td)) lpfoutput = lpfoutput[i_tstart:-1] tspice = tspice[i_tstart:-1] lpfoutput = lpfoutput - mean(lpfoutput) lpfoutput = lpfoutput / MixerGain