def test_dcgain(self): """Test function dcgain with different systems""" if slycot_check(): #Test MIMO systems A, B, C, D = self.make_MIMO_mats() gain1 = dcgain(ss(A, B, C, D)) gain2 = dcgain(A, B, C, D) sys_tf = ss2tf(A, B, C, D) gain3 = dcgain(sys_tf) gain4 = dcgain(sys_tf.num, sys_tf.den) #print("gain1:", gain1) assert_array_almost_equal(gain1, array([[0.0269, 0.], [0., 0.0269]]), decimal=4) assert_array_almost_equal(gain1, gain2) assert_array_almost_equal(gain3, gain4) assert_array_almost_equal(gain1, gain4) #Test SISO systems A, B, C, D = self.make_SISO_mats() gain1 = dcgain(ss(A, B, C, D)) assert_array_almost_equal(gain1, array([[0.0269]]), decimal=4)
def c2d(sys, Ts, method='ZOH'): """ @summary: From continuous time convert discrete time @param sys: an instance of the LTI class or a tuple describing the system. The following gives the number of elements in the tuple and the interpretation: 2: (num, den) 4: (A, B, C, D) @param Ts: Sampling time @param method: The name of the discretization method @return: The discrete system """ if (len(sys) == 2): (A, B, C, D) = tf2ss(sys) else: (A, B, C, D) = sys n = A.shape[0] nb = B.shape[1] if (method == 'ZOH'): ztmp = zeros((nb, n + nb)) tmp = hstack((A, B)) tmp = vstack((tmp, ztmp)) tmp = expm(tmp * Ts) A = tmp[0:n, 0:n] B = tmp[0:n, n:n + nb] sysd = (A, B, C, D) if (len(sys) == 2): return ss2tf(sysd) return sysd
def test_dcgain(self): """Test function dcgain with different systems""" #Test MIMO systems A, B, C, D = self.make_MIMO_mats() gain1 = dcgain(ss(A, B, C, D)) gain2 = dcgain(A, B, C, D) sys_tf = ss2tf(A, B, C, D) gain3 = dcgain(sys_tf) gain4 = dcgain(sys_tf.num, sys_tf.den) #print("gain1:", gain1) assert_array_almost_equal(gain1, array([[0.0269, 0. ], [0. , 0.0269]]), decimal=4) assert_array_almost_equal(gain1, gain2) assert_array_almost_equal(gain3, gain4) assert_array_almost_equal(gain1, gain4) #Test SISO systems A, B, C, D = self.make_SISO_mats() gain1 = dcgain(ss(A, B, C, D)) assert_array_almost_equal(gain1, array([[0.0269]]), decimal=4)
def get_tf(self, desired_windspeed, desired_azimuth, desired_input, desired_output): windspeed_ii = np.where(self.windspeeds == desired_windspeed)[0] desiredinput_ii = self.inputnames.index(desired_input) desiredoutput_ii = self.outputnames.index(desired_output) SS = self.SS[windspeed_ii[0]] desired_wtg = ctrl.ss2tf(SS.A, SS.B[:, desiredinput_ii], SS.C[desiredoutput_ii, :], SS.D[desiredoutput_ii, desiredinput_ii]) return desired_wtg
def get_transfer_functions(statespace_model): transfer_functions = ss2tf(statespace_model) # Loop through the coefficients and set ones close to zero equal to zero. # This will modify the actual transfer_functions arrays all_coeffs = [ transfer_functions.num[0][0], transfer_functions.den[0][0], transfer_functions.num[1][0], transfer_functions.den[1][0] ] for i in range(len(all_coeffs)): for j in range(len(all_coeffs[i])): if abs(all_coeffs[i][j]) < (10**(-10)): all_coeffs[i][j] = 0 theta_tf = tf(all_coeffs[0], all_coeffs[1]) cart_tf = tf(all_coeffs[2], all_coeffs[3]) return theta_tf, cart_tf
def test_dcgain_mimo(self, MIMO_mats): """Test function dcgain with MIMO systems""" #Test MIMO systems A, B, C, D = MIMO_mats gain1 = dcgain(ss(A, B, C, D)) gain2 = dcgain(A, B, C, D) sys_tf = ss2tf(A, B, C, D) gain3 = dcgain(sys_tf) gain4 = dcgain(sys_tf.num, sys_tf.den) #print("gain1:", gain1) assert_array_almost_equal(gain1, array([[0.0269, 0.], [0., 0.0269]]), decimal=4) assert_array_almost_equal(gain1, gain2) assert_array_almost_equal(gain3, gain4) assert_array_almost_equal(gain1, gain4)
def siso(self): """Set up some systems for testing out MATLAB functions""" s = tsystems() A = np.array([[1., -2.], [3., -4.]]) B = np.array([[5.], [7.]]) C = np.array([[6., 8.]]) D = np.array([[9.]]) s.ss1 = ss(A, B, C, D) # Create some transfer functions s.tf1 = tf([1], [1, 2, 1]) s.tf2 = tf([1, 1], [1, 2, 3, 1]) # Conversions s.tf3 = tf(s.ss1) s.ss2 = ss(s.tf2) s.ss3 = tf2ss(s.tf3) s.tf4 = ss2tf(s.ss2) return s
def c2d(sys, Ts, method="ZOH"): """ @summary: From continuous time convert discrete time @param sys: an instance of the LTI class or a tuple describing the system. The following gives the number of elements in the tuple and the interpretation: 2: (num, den) 4: (A, B, C, D) @param Ts: Sampling time @param method: The name of the discretization method @return: The discrete system """ if method == "ZOH": if len(sys) == 2: (A, B, C, D) = tf2ss(sys) else: (A, B, C, D) = sys n = A.shape[0] nb = B.shape[1] if method == "ZOH": ztmp = zeros((nb, n + nb)) tmp = hstack((A, B)) tmp = vstack((tmp, ztmp)) tmp = expm(tmp * Ts) A = tmp[0:n, 0:n] B = tmp[0:n, n : n + nb] sysd = (A, B, C, D) if len(sys) == 2: return ss2tf(sysd) return sysd return None
def pid_design(self): n_x = self._A.shape[0] # number of sates n_u = self._B.shape[1] # number of inputs n_y = self._C.shape[0] # number of outputs # Augmented state system (for LQR) A_lqr = np.block([[self._A, np.zeros((n_x, n_y))], [self._C, np.zeros((n_y, n_y))]]) B_lqr = np.block([[self._B], [np.zeros((n_y, 1))]]) # Define Q,R Q = np.diag(self._q) R = np.array([self._r]) # Solve for P in continuous-time algebraic Riccati equation (CARE) #print("A_lqr shape", A_lqr.shape) #print("Q shape",Q.shape) (P, L, F) = cnt.care(A_lqr, B_lqr, Q, R) if self._verbose: print("P matrix", P) print("Feedback gain", F) # Calculate Ki_bar, Kp_bar Kp_bar = np.array([F[0][0:n_x]]) Ki_bar = np.array([F[0][n_x:]]) if self._verbose: print("Kp_bar", Kp_bar) print("Ki_bar", Ki_bar) # Calculate the PID kp kd gains C_bar = np.block( [[self._C], [self._C.dot(self._A) - (self._C.dot(self._B)).dot(Kp_bar)]]) if self._verbose: print("C_bar", C_bar) print("C_bar shape", C_bar.shape) # Calculate PID kp ki gains kpd = Kp_bar.dot(np.linalg.inv(C_bar)) if self._verbose: print("kpd: ", kpd, "with shape: ", kpd.shape) kp = kpd[0][0] kd = kpd[0][1] # ki gain ki = (1. + kd * self._C.dot(self._B)).dot(Ki_bar) self._K = [kp, ki[0][0], kd] G_plant = cnt.ss2tf(self._A, self._B, self._C, self._D) # create PID transfer function d_tc = 1. / 125. # Derivative time constant at nyquist freq p_tf = self._K[0] i_tf = self._K[1] * cnt.tf([1], [1, 0]) d_tf = self._K[2] * cnt.tf([1, 0], [d_tc, 1]) pid_tf = cnt.parallel(p_tf, i_tf, d_tf) open_loop_tf = cnt.series(pid_tf, G_plant) closedLoop_tf = cnt.feedback(open_loop_tf, 1) if self._verbose: print(" *********** PID gains ***********") print("kp: ", self._K[0]) print("ki: ", self._K[1]) print("kd: ", self._K[2]) print(" *********************************") return pid_tf, closedLoop_tf
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()
######################################################## Input (Espaço de estados) ######################################################## ''' A = np.array([[0, 1, 0], [0, 0, 1], [-30, -31, -10]]) B = np.array([[0], [0], [1]]) C = np.array([0, 0, 1]) D = np.array([[0]]) # Espaço de estados -> Funcao de transferencia S = ctl.ss(A, B, C, D) G = ctl.ss2tf(S) # Polos a partir do espaço de estados print('Polos = ', ctl.pole(S)) ''' ######################################################## Input (Funcao de transferencia) ######################################################## ''' # num = [0, 0, 1, 0] # den = [1, 6, 5, 1] # G = ctl.tf(num, den) # # Funcao de transferencia -> Espaço de estados
''' # System matrices as 2D arrays : A = np.array([[0, 0, 1, 0], [0, 0, 0, 1], [-490, -137, -12, -2], [-170, -383, -2.3, -9]]) B = np.array([[0], [0], [1000 / 310], [-693 / 250]]) C = np.array([[1, 0, 0, 0]]) # C = np.array([[0, 1, 0, 0]]) D = np.array([[0]]) Sp = ctl.ss(A, B, C, D) print('S =', Sp) ################################################################################ # Funcao de transferencia ################################################################################ G = ctl.ss2tf(Sp) print('G =', G) ''' ################################################################################ Input (Funcao de transferencia Gpid) ################################################################################ ''' num = np.array([1, 15000.1, 1500]) den = np.array([0, 1, 0]) Gpid = ctl.tf(num, den) print('Gpid:', Gpid) ################################################################################ # Em serie (G*Gpid) ################################################################################ S = ctl.series(G, Gpid)
import numpy as np end = 8 s = cm.tf([1, 0], [1]) A = np.array([[0, 1, 0, 0], [-1, -1, 1, 0], [0, 0, 0, 1], [1, 0, -1, 1]]) b = np.array([[0], [0], [0], [1]]) c = np.array([1, 0, 0, 0]) d = [0] SS = cm.feedback(cm.ss(A, b, c, d), 1) TF_conv = cm.ss2tf(SS) TF = cm.feedback((1) / (s**4 + s**2)) out_SS, t = cm.step(1 * SS, np.linspace(0, end, 200)) out_TF, t = cm.step(1 * TF, np.linspace(0, end, 200)) out_TF_conv, t = cm.step(1 * TF_conv, np.linspace(0, end, 200)) plt.plot(t, out_SS, lw=2, label="ss") plt.plot(t, out_TF, lw=1, label="tf") plt.plot(t, out_TF_conv, lw=0.5, label="tf_conv") plt.legend() plt.show() # https://www.wolframalpha.com/input/?i=(%7B%7Bs,+0,+0,+0%7D,+%7B0,+s,+0,+0%7D,+%7B0,+0,+s,+0%7D,+%7B0,+0,+0,+s%7D%7D+-+%7B%7B0,+1,+0,+0%7D,+%7B-1,+-1,+1,+0%7D,+%7B0,+0,+0,+1%7D,+%7B1,+0,+-1,+1%7D%7D)
from control import matlab import numpy as np import matplotlib.pyplot as plt A = np.array([[0, 1], [-1, -1.8]]) b = np.array([[0], [1]]) c = np.array([1, 0]) d = 0 T = 0.3 S = matlab.ss(A, b, c, d) #print(S) Gs = matlab.ss2tf(S) #print(Gs) P = matlab.c2d(S, T, method='zoh') #print(P) Gz = matlab.ss2tf(P) #PはSを離散時間系に変換したやつ #print(Gz) #p.13の問題6.の答え #matlab.bode(S ,matlab.logspace(-2, 2)) #ボード線図を描こう! yt, tt = matlab.step(S, np.arange(0, 10, 0.01)) #まずは連続時間系 #plt.plot(tt, yt) #連続時間系での出力をグラフにする yk, tk = matlab.step(P, np.arange(0, 10, T)) #次は離散時間系 plt.plot(tk, yk, marker='o') #離散時間系のプロット、プロット点も描画する
def convert_transfer_function_model(ss_model): tf_model = ss2tf(ss_model) return tf_model