def attitude_sysid(y_acc, u_mix, verbose=False): # type: (np.array, np.array, bool) -> (control.tf, float, float) """ roll/pitch system id assuming delay/gain model :param y_acc: (roll or pitch acceleration) :param u_mix: (roll or pitch acceleration command) :param verbose: show debug output :return: (G_ol, delay, k) G_ol: open loop plant delay: time delay (sec) k: gain """ if verbose: print('solving for plant model ', end='') k, delay = delay_and_gain_sysid(y_acc, u_mix, verbose) if verbose: print('done') # open loop, rate output, mix input plant tf_acc = k * control.tf(*control.pade(delay, 1)) # order 1 approx # can neglect motor, pole too fast to matter compared to delay, not used in sysid either tf_integrator = control.tf((1), (1, 0)) G_ol = tf_acc * tf_integrator return G_ol, delay, k
def test_describing_function_plot(): # Simple linear system with at most 1 intersection H_simple = ct.tf([1], [1, 2, 2, 1]) omega = np.logspace(-1, 2, 100) # Saturation nonlinearity F_saturation = ct.descfcn.saturation_nonlinearity(1) amp = np.linspace(1, 4, 10) # No intersection xsects = ct.describing_function_plot(H_simple, F_saturation, amp, omega) assert xsects == [] # One intersection H_larger = H_simple * 8 xsects = ct.describing_function_plot(H_larger, F_saturation, amp, omega) for a, w in xsects: np.testing.assert_almost_equal(H_larger(1j * w), -1 / ct.describing_function(F_saturation, a), decimal=5) # Multiple intersections H_multiple = H_simple * ct.tf(*ct.pade(5, 4)) * 4 omega = np.logspace(-1, 3, 50) F_backlash = ct.descfcn.friction_backlash_nonlinearity(1) amp = np.linspace(0.6, 5, 50) xsects = ct.describing_function_plot(H_multiple, F_backlash, amp, omega) for a, w in xsects: np.testing.assert_almost_equal(-1 / ct.describing_function(F_backlash, a), H_multiple(1j * w), decimal=5)
def test_nyquist_fbs_examples(): s = ct.tf('s') """Run through various examples from FBS2e to compare plots""" plt.figure() plt.title("Figure 10.4: L(s) = 1.4 e^{-s}/(s+1)^2") sys = ct.tf([1.4], [1, 2, 1]) * ct.tf(*ct.pade(1, 4)) count = ct.nyquist_plot(sys) assert _Z(sys) == count + _P(sys) plt.figure() plt.title("Figure 10.4: L(s) = 1/(s + a)^2 with a = 0.6") sys = 1 / (s + 0.6)**3 count = ct.nyquist_plot(sys) assert _Z(sys) == count + _P(sys) plt.figure() plt.title("Figure 10.6: L(s) = 1/(s (s+1)^2) - pole at the origin") sys = 1 / (s * (s + 1)**2) count = ct.nyquist_plot(sys) assert _Z(sys) == count + _P(sys) plt.figure() plt.title("Figure 10.10: L(s) = 3 (s+6)^2 / (s (s+1)^2)") sys = 3 * (s + 6)**2 / (s * (s + 1)**2) count = ct.nyquist_plot(sys) assert _Z(sys) == count + _P(sys) plt.figure() plt.title("Figure 10.10: L(s) = 3 (s+6)^2 / (s (s+1)^2) [zoom]") count = ct.nyquist_plot(sys, omega_limits=[1.5, 1e3]) # Frequency limits for zoom give incorrect encirclement count # assert _Z(sys) == count + _P(sys) assert count == -1
def attitude_sysid(y_acc, u_mix, verbose=False): """ roll/pitch system id assuming delay/gain model :param y_acc: (roll or pitch acceleration) :param u_mix: (roll or pitch acceleration command) :return: (G_ol, delay, k) G_ol: open loop plant delay: time delay (sec) k: gain """ if verbose: print('solving for plant model ', end='') k, delay = delay_and_gain_sysid(y_acc, u_mix, verbose) if verbose: print('done') # open loop, rate output, mix input plant tf_acc = k*control.tf(*control.pade(delay, 1)) # order 1 approx # can neglect motor, pole too fast to matter compared to delay, not used in sysid either tf_integrator = control.tf((1), (1, 0)) G_ol = tf_acc * tf_integrator return G_ol, delay, k
def pt1(smooth, time): smoothed_df = pd.concat([pd.DataFrame(smooth, columns = ['smoothed']), \ pd.DataFrame(time, columns = ['time'])], axis = 1) steady_state = smooth[-1] #last element of the smoothed data is passed as steady state value standard_output = steady_state * (1 - np.exp(-1)) #case when t = time_constant in the eqn. ############################################################################# # standard_output = steady_state * (1 - e ^ (−t / time_constant)) # ############################################################################# delay = smoothed_df.time[smoothed_df.smoothed < 0.02].values[-1] #returns the time at which the step change occurs i.e a transition from 0 to some value. #Values lesser than 0.02 were treatred as zero. time_constant = smoothed_df.time[smoothed_df.index == abs(smoothed_df.smoothed - standard_output)\ .sort_values().index[0]].values[0] #derived from the equation of standard_output tf_pt1 = con.matlab.tf(steady_state, [time_constant, 1]) numerator, denominator = con.pade(delay, 1) delay_tf_pt1 = con.matlab.tf(numerator, denominator) yout_pt1, t_pt1 = con.matlab.step(tf_pt1 * delay_tf_pt1) #first order transfer function is given by ############################################################################### # steady_state * (e ^ - (delay * s)) # # transfer_function(s) = ------------------------------------ # # (time_constant * s + 1 ) # ############################################################################### return tf_pt1 * delay_tf_pt1, yout_pt1, t_pt1, delay, time_constant, steady_state
def pt2(smooth, time): def fourpoint(z): f1_zeta = 0.451465 + (0.066696 * z) + (0.013639 * z**2) f3_zeta = 0.800879 + (0.194550 * z) + (0.101784 * z**2) f6_zeta = 1.202664 + (0.288331 * z) + (0.530572 * z**2) f9_zeta = 1.941112 - (1.237235 * z) + (3.182373 * z**2) return f1_zeta, f3_zeta, f6_zeta, f9_zeta def method(): #the second method from the article is adopted, as the response/data handled strictly follows this method. zeta = np.sqrt( (np.log(overshoot)**2) / ((np.pi**2) + (np.log(overshoot)**2))) f1_zeta, f3_zeta, f6_zeta, f9_zeta = fourpoint(zeta) time_constant = (t9 - t1) / (f9_zeta - f1_zeta) #delay = t1 - time_constant*f1_zeta #based on article. delay = smoothed_df.time[smoothed_df.smoothed < 0.02].values[-1] return zeta, time_constant, delay smoothed_df = pd.concat([pd.DataFrame(smooth, columns = ['smoothed']), \ pd.DataFrame(time, columns = ['time'])], axis = 1) steady_state = smooth[-1] #ssn = steady state at n/10th instant of time ss1 = steady_state * 0.1 ss3 = steady_state * 0.3 ss6 = steady_state * 0.6 ss9 = steady_state * 0.9 #tn = time at n/10th instant t1 = smoothed_df.time[smoothed_df.index == abs( smoothed_df.smoothed - ss1).sort_values().index[0]].values[0] t3 = smoothed_df.time[smoothed_df.index == abs( smoothed_df.smoothed - ss3).sort_values().index[0]].values[0] t6 = smoothed_df.time[smoothed_df.index == abs( smoothed_df.smoothed - ss6).sort_values().index[0]].values[0] t9 = smoothed_df.time[smoothed_df.index == abs( smoothed_df.smoothed - ss9).sort_values().index[0]].values[0] peak = smoothed_df.smoothed.max() #returns the highest output in the response overshoot = (peak - steady_state) / steady_state #represented as Mp in article zeta, time_constant, delay = method() tf_pt2 = con.matlab.tf(steady_state, [time_constant**2, 2 * zeta * time_constant, 1]) n_2, d_2 = con.pade(delay, 1) delay_tf_pt2 = con.matlab.tf(n_2, d_2) yout_pt2, t_pt2 = con.matlab.step(tf_pt2 * delay_tf_pt2) #second order transfer function is given by ######################################################################################################## # steady_state * (e ^ - (delay * s)) # # transfer_function(s) = ---------------------------------------------------------------------- # # ((time_constant ^ 2) * (s ^ 2)) + (2 * zeta * time_constant * s) + 1 ) # ######################################################################################################## return tf_pt2 * delay_tf_pt2, yout_pt2, t_pt2, delay, time_constant, steady_state, zeta
def ideal_pt1(ss_array, tc_array, d_array): ideal_ss = np.average(ss_array) ideal_tc = np.average(tc_array) ideal_d = np.average(d_array) ideal_tf_pt1 = con.matlab.tf(ideal_ss, [ideal_tc, 1]) numerator, denominator = con.pade(ideal_d, 1) ideal_d_tf_pt1 = con.matlab.tf(numerator,denominator) ideal_yout_pt1, ideal_t_pt1 = con.matlab.step(ideal_tf_pt1 * ideal_d_tf_pt1) return ideal_tf_pt1 * ideal_d_tf_pt1, ideal_yout_pt1, ideal_t_pt1
def ideal_pt2(ss_array, tc_array, d_array, z_array): ideal_ss = np.average(ss_array) ideal_tc = np.average(tc_array) ideal_d = np.average(d_array) ideal_z = np.average(z_array) ideal_tf_pt2 = con.matlab.tf(ideal_ss, [ideal_tc ** 2, 2 * ideal_z * ideal_tc, 1]) numerator, denominator = con.pade(ideal_d, 1) ideal_d_tf_pt2 = con.matlab.tf(numerator,denominator) ideal_yout_pt2, ideal_t_pt2 = con.matlab.step(ideal_tf_pt2 * ideal_d_tf_pt2) return ideal_tf_pt2 * ideal_d_tf_pt2, ideal_yout_pt2, ideal_t_pt2
def ActuatorModel(bw, delay=(0, 1)): # Inputs: ['cmd'] # Outputs: ['pos'] sysNom = control.tf2ss(control.tf(1, [1 / bw, 1])) delayPade = control.pade(delay[0], n=delay[1]) sysDelay = control.tf2ss(control.tf(delayPade[0], delayPade[1])) sys = sysDelay * sysNom return sys
def attitude_sysid(y_acc, u_mix): print('solving for plant model ', end='') k, delay = delay_and_gain_sysid(y_acc, u_mix) print('done') # open loop, rate output, mix input plant tf_acc = k * control.tf(*control.pade(delay, 1)) # order 1 approx # can neglect motor, pole too fast to matter compared to delay, not used in sysid either tf_integrator = control.tf((1), (1, 0)) G_ol = tf_acc * tf_integrator return G_ol, delay, k
def WoodBerry(model_id): g11 = control.tf([12.8], [16.7, 1]) * control.tf(*control.pade(1, 1)) g12 = control.tf([-18.9], [21.0, 1]) * control.tf(*control.pade(3, 1)) g21 = control.tf([6.6], [10.9, 1]) * control.tf(*control.pade(7, 1)) g22 = control.tf([-19.4], [14.4, 1]) * control.tf(*control.pade(3, 1)) g1f = control.tf([3.8], [14.9, 1]) * control.tf(*control.pade(8, 1)) g2f = control.tf([4.9], [13.2, 1]) * control.tf(*control.pade(3, 1)) row_1_num = [x[0][0] for x in (g11.num, g12.num, g1f.num)] row_2_num = [x[0][0] for x in (g21.num, g22.num, g2f.num)] row_1_den = [x[0][0] for x in (g11.den, g12.den, g1f.den)] row_2_den = [x[0][0] for x in (g21.den, g22.den, g2f.den)] sys = control.tf([row_1_num, row_2_num], [row_1_den, row_2_den]) R = Tag(0, 'Reflux', 'Reflux flow rate', TagType.INPUT) S = Tag(1, 'Steam', 'Steam flow rate', TagType.INPUT) F = Tag(2, 'Feed', 'Feed flow rate', TagType.INPUT) x_D = Tag(3, 'x_D', 'Distillate purity', TagType.OUTPUT) x_B = Tag(4, 'x_B', 'Bottoms purity', TagType.OUTPUT) return Model('Wood-Berry Distillation Model', sys, OrderedDict([('R', R), ('S', S), ('F', F)]), OrderedDict([('x_D', x_D), ('x_B', x_B)]), model_id)
def SensorModel(bw, delay=(0, 1)): # Inputs: ['meas', 'dist'] # Outputs: ['sens'] sysNom = control.tf2ss(control.tf(1, [1 / bw, 1])) delayPade = control.pade(delay[0], n=delay[1]) sysDelay = control.tf2ss(control.tf(delayPade[0], delayPade[1])) sysDist = control.tf2ss(control.tf(1, 1)) sys = control.append(sysDelay * sysNom, sysDist) sys.C = sys.C[0, :] + sys.C[1, :] sys.D = sys.D[0, :] + sys.D[1, :] sys.outputs = 1 return sys
def attitude_loop_design(m, name, dcut_hz, fbcut_hz, est_delay): tf_est = control.tf(*control.pade(est_delay, 1)) G_rate_ol = model_to_acc_tf(m) K_rate, G_rate_ol, G_rate_comp_cl = ut.lqrofb.pid_design( G=G_rate_ol, K_guess=[0.2, 0.2, 0], dcut_hz=dcut_hz, fbcut_hz=fbcut_hz) tf_integrator = control.tf([1], [1, 0]) G_ol = G_rate_comp_cl * tf_integrator * tf_est K, G_ol, G_comp_cl = ut.lqrofb.pid_design(G=G_ol, K_guess=[1], dcut_hz=dcut_hz, fbcut_hz=None, use_I=False, use_D=False) return { 'MC_{:s}RATE_P'.format(name): K_rate[0, 0], 'MC_{:s}RATE_I'.format(name): K_rate[1, 0], 'MC_{:s}RATE_D'.format(name): K_rate[2, 0], 'MC_{:s}_P'.format(name): K[0, 0], }
def test_fixed_view(self): # respect xlim, ylim set by user g = (tf([1], [1 / 1, 2 * 0.01 / 1, 1]) * tf([1], [1 / 100**2, 2 * 0.001 / 100, 1]) * tf(*pade(0.01, 5))) # normally a broad axis nichols(g) assert (plt.xlim()[0] == -1440) assert (plt.ylim()[0] <= -240) nichols(g, grid=False) # zoom in plt.axis([-360, 0, -40, 50]) # nichols_grid doesn't expand limits nichols_grid() assert (plt.xlim()[0] == -360) assert (plt.ylim()[1] >= -40)
def ideal_pt1(ss_array, tc_array, d_array): ideal_ss = np.average(ss_array) ideal_tc = np.average(tc_array) ideal_d = np.average(d_array) ideal_tf_pt1 = con.matlab.tf(ideal_ss, [ideal_tc, 1]) numerator, denominator = con.pade(ideal_d, 1) ideal_d_tf_pt1 = con.matlab.tf(numerator, denominator) transfer_function = ideal_tf_pt1 * ideal_d_tf_pt1 ############################################################################################################################# # manual fitting of ideal pt1 model # Based on the assumption that the plot of denominator of the transfer function has only one intercept in x-axis # We have ax ^ 2 + bx + c = 0 # Therefore we change b such that b ^ 2 - 4 * a * c = 0 # For more fitting, we round down the result transfer_function.den[0][0][1] = math.floor( math.sqrt(4 * transfer_function.den[0][0][0] * transfer_function.den[0][0][2])) # To obtain the normal result, comment the above line ############################################################################################################################# ideal_yout_pt1, ideal_t_pt1 = con.matlab.step(transfer_function) return transfer_function, ideal_yout_pt1, ideal_t_pt1
#Code by P.Aashrith #April 28, 2020 #Released under GNU GPL import control import numpy as np import matplotlib.pyplot as plt #if using termux import subprocess import shlex #end if num = 4 den = [1,3,2,0] [n1,d1]=control.pade(0.1744,10) #Creating delay of 0.1744 [n2,d2]=control.pade(0.5,10) #Creating delay of 0.5 [n3,d3]=control.pade(0.0005,10) #Creating delay of 0.0005 #Creating a transfer function G = num/den G = control.tf(num,den) G1=control.tf(n1,d1) G2=control.tf(n2,d2) G3=control.tf(n3,d3) #assigning delay to system G G4=G*G1 G5=G*G2 G6=G*G3 #plotting nyquist plot with variable tau
sns.despine() plt.savefig('ctl_id_2z.pdf') H_ring.plot_step(figsize=(4, 3)) plt.ylim(ymax=1.1) sns.despine() plt.grid('on') plt.savefig('ctl_id_2z_step.pdf') pid = ms.PID(0.6, 1.0 * fs, 0.3 / fs) pid = ms.PID(0, .8 * fs, 0. / fs) delay_calc = 2e-3 # 2e-3 delay_adc = 0.5e-3 delay_dac = 0.5e-3 delay = delay_calc + delay_adc + delay_dac H_delay = ms.TF(*control.pade(delay)) pid = ms.PID(0.9, 0.5 * fs, 0.15 / fs) # H_delay = ms.TF([1],[1]) # H = pid*H_delay*H_lp Hpid = pid * H_delay * H_lp Gpid = H_ring / (1 + Hpid * H_ring) Gpid.plot_hw(w=np.linspace(0.0001, 75) * 2 * np.pi, bode=False, xscale='linear', yscale='db', figsize=(4, 3)) sns.despine() plt.savefig('ctl_freqresp_pid.pdf')
# https://etda.libraries.psu.edu/catalog/13071yxl5197 Alat = np.array([[-0.1180, 32.033, 0, 0], [0, -6.411, 1, 0], [0, -20.639, 0, 1], [0, -1.235, 0, 0]]) Blat = np.array([[0], [0.0039974], [0.016275], [0.0040072]]) Clat = np.array([0.3048, 0, 0, 0]) Dlat = 0 sys_lat = control.ss(Alat, Blat, Clat, Dlat) print(sys_lat) # Test if system is controllable ContMatrix = control.ctrb(Alat, Blat) print("Rank of Controllability Matrix: %s" % np.linalg.matrix_rank(ContMatrix)) # Test if system is observable ObsMatrix = control.obsv(Alat, Clat) print("Rank of Observability Matrix: %s" % np.linalg.matrix_rank(ObsMatrix)) sys_tf_lat = control.ss2tf(sys_lat) lat_delay = 0.156 delay_NUM, delay_DEN = control.pade(lat_delay, 4) sys_tf_lat_delay = control.tf(delay_NUM, delay_DEN) sys_tf_lat = sys_tf_lat * sys_tf_lat_delay T, yout = control.step_response(sys_tf_lat * 17.3333) plt.plot(T, yout) plt.show() # Fully controllable, fully observable. So we can employ state feedback # = control.acker(Alat, Blat, [-10]) #print(K)
import numpy as np import control from matplotlib import pyplot as plt # definir las fdt sysgp = control.tf(10, [1, 1]) [numd, dend] = control.pade(0.01, 3) sysd = control.tf(numd, dend) sysgpd = sysgp * sysd #sysga=control.tf(1,[1/10,1]) sysga = control.tf(1, [1 / 10, 1]) sysgt = control.tf(1, 1) Kp = 0.1 Ti = 10 #Ki=1/Ti #Ki=0 Td = 0 N = 1000 sysp = control.tf(Kp, 1) sysi = control.tf(1, [Ti, 0]) sysd = control.tf([Td, 0], [1 / N, 1]) syspid = sysp + sysi + sysd #obtener la fdt en lazo cerrado mediante feedback syslc = control.feedback(syspid * sysga * sysgpd, sysgt) #t=np.arange(0,0.1,100) [t, y1] = control.step_response(4 * syslc, 100) # %% plt.figure(1) plt.plot(t, y1) plt.grid() plt.show()
import matplotlib.pyplot as plt import control as ctr import numpy as np import math counter = 0 Kmin = 0.09 K_p = 0.0910 kdrange = np.linspace(1, 5, 100) * Kmin Tcrit = np.zeros(kdrange.size) a, b = ctr.pade(0.4) print(a, b) sys1 = ctr.tf(a, b) for kd in kdrange: s = ctr.tf('s') # G = ctr.tf([1121*math.exp(-s*0.4)*(K_p+s*kd)],[200*s**2*(1+s)]) G = (1121 * (K_p + s * kd)) / (200 * s**2 * (1 + s)) * sys1 gm, pm, Wcg, Wcp = ctr.margin(G) Tcrit[counter] = pm * (2 * math.pi) / (360 * 0.88) + 0.4 counter += 1 fig1 = plt.figure() plt.plot(kdrange, Tcrit) plt.grid() plt.show() # help(ctr.tf)
def pade_model(tau, n=1): approx = pade(tau, n) approx = tf(approx[0], approx[1]) return approx
def WoodBerry(): g_tf = control.tf([12.8], [16.7, 1]) td = 1 g_delay = control.tf(control.pade(td, 1)[0], control.pade(td, 1)[1]) g11 = g_tf * g_delay g_tf = control.tf([-18.9], [21.0, 1]) td = 3 g_delay = control.tf(control.pade(td, 1)[0], control.pade(td, 1)[1]) g12 = g_tf * g_delay g_tf = control.tf([6.6], [10.9, 1]) td = 7 g_delay = control.tf(control.pade(td, 1)[0], control.pade(td, 1)[1]) g21 = g_tf * g_delay g_tf = control.tf([-19.4], [14.4, 1]) td = 3 g_delay = control.tf(control.pade(td, 1)[0], control.pade(td, 1)[1]) g22 = g_tf * g_delay g_tf = control.tf([3.8], [14.9, 1]) td = 8 g_delay = control.tf(control.pade(td, 1)[0], control.pade(td, 1)[1]) g1f = g_tf * g_delay g_tf = control.tf([4.9], [13.2, 1]) td = 3 g_delay = control.tf(control.pade(td, 1)[0], control.pade(td, 1)[1]) g2f = g_tf * g_delay row_1_num = [x[0][0] for x in (g11.num, g12.num, g1f.num)] row_2_num = [x[0][0] for x in (g21.num, g22.num, g2f.num)] row_1_den = [x[0][0] for x in (g11.den, g12.den, g1f.den)] row_2_den = [x[0][0] for x in (g21.den, g22.den, g2f.den)] sys = control.tf([row_1_num, row_2_num], [row_1_den, row_2_den]) R = Tag('Reflux', 'Reflux flow rate', IOtype='INPUT') S = Tag('Steam', 'Steam flow rate', IOtype='INPUT') F = Tag('Feed', 'Feed flow rate', IOtype='INPUT') xD = Tag('x_D', 'Distillate purity', IOtype='OUTPUT') xB = Tag('x_B', 'Bottoms purity', IOtype='OUTPUT') inputs = {'R': R, 'S': S, 'F': F} outputs = {'xD': xD, 'xB': xB} wood_berry = Model(sys, "Wood Berry Distillation Model", inputs, outputs) return wood_berry
def test_nyquist_arrows(arrows): sys = ct.tf([1.4], [1, 2, 1]) * ct.tf(*ct.pade(1, 4)) plt.figure() plt.title("L(s) = 1.4 e^{-s}/(s+1)^2 / arrows = %s" % arrows) count = ct.nyquist_plot(sys, arrows=arrows) assert _Z(sys) == count + _P(sys)
def model_to_acc_tf(m): acc_tf = m['gain'] * control.tf(*control.pade(m['delay'], 1)) # order 1 approx tf_integrator = control.tf((1), (1, 0)) return acc_tf * tf_integrator