def bode_cor(num1, den1, num2, den2): sys1 = ctl.tf(num1, den1) sys2 = ctl.tf(num2, den2) sys_BO = ctl.series(sys1, sys2) sys = sys_BO.returnScipySignalLti()[0][0] w, mag, phase = sys.bode() return w, mag, phase
def instantiate (self, specs = None): if specs is not None: self.specs = specs; self.Kc = insert (1, self.specs, 'Kc'); self.Ti = insert (np.inf, self.specs, 'Ti'); self.Td = insert (0, self.specs, 'Td'); self.b = insert (1.0, self.specs, 'b'); self.c = insert (1.0, self.specs, 'c'); self.N = insert (9.0, self.specs, 'N'); num_p = self.Kc * self.b; den_p = 1; Cp = ctrl.tf (num_p, den_p); if (self.Ti != np.inf): num_i = self.Kc; den_i = [self.Ti, 0]; Ci = ctrl.tf (num_i, den_i); else: Ci = 0; if (self.Td != 0): num_d = [self.Kc * self.Td * self.c, 0]; den_d = [(self.Td / self.N), 0]; Cd = ctrl.tf (num_d, den_d); else: Cd = 0; self.tf = Cp + Ci + Cd;
def classic(): # PI controller Kp = 1.5 Ki = 30. P = tf([Kp], [1]) I = tf([Ki], [1, 0]) Gc = parallel(P, I) # Power convertor Gpc = synthesize_2pole_filter(50, 0.707) # Plant Gp = tf([50], [1, 0]) # Sensor # если выбросить из петли становится лучше # "remove sensor lag" Gs = synthesize_1pole_filter(20) # Openloop Gol = series(series(Gc, Gpc), Gp) # Closed loop Gcl = feedback(Gol) ts, xs = step_response( Gcl ) grid() plot(ts, xs)
def fitnessFunction(Kp, Ti, Td): G = control.tf([Ti*Td, Ti, 1], [Ti, 0]) F = control.tf(1,[1,6,11,6,0]) sys = control.feedback(control.series(G, F), 1) y = control.step(sys) yout = y[0] t = y[-1] # Integrated Square Error error = 0 for val in y[0]: error += (val - 1)*(val - 1) # Overshoot OS = (yout.max()/yout[-1]-1)*100 Tr = 0 Ts = 0 # Rising Time for i in range(0, len(yout) - 1): if yout[i] > yout[-1]*.90: Tr = t[i] - t[0] # Settling Time for i in range(2, len(yout) - 1): if abs(yout[-i]/yout[-1]) > 1.02: Ts = t[len(yout) - i] - t[0] return 1/(OS + Tr + Ts + error*error*10)*100000
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 testLists(self): """Make sure that lists of various lengths work for operations""" sys1 = ctrl.tf([1, 1], [1, 2]) sys2 = ctrl.tf([1, 3], [1, 4]) sys3 = ctrl.tf([1, 5], [1, 6]) sys4 = ctrl.tf([1, 7], [1, 8]) sys5 = ctrl.tf([1, 9], [1, 0]) # Series sys1_2 = ctrl.series(sys1, sys2) np.testing.assert_array_almost_equal(sort(pole(sys1_2)), [-4., -2.]) np.testing.assert_array_almost_equal(sort(zero(sys1_2)), [-3., -1.]) sys1_3 = ctrl.series(sys1, sys2, sys3); np.testing.assert_array_almost_equal(sort(pole(sys1_3)), [-6., -4., -2.]) np.testing.assert_array_almost_equal(sort(zero(sys1_3)), [-5., -3., -1.]) sys1_4 = ctrl.series(sys1, sys2, sys3, sys4); np.testing.assert_array_almost_equal(sort(pole(sys1_4)), [-8., -6., -4., -2.]) np.testing.assert_array_almost_equal(sort(zero(sys1_4)), [-7., -5., -3., -1.]) sys1_5 = ctrl.series(sys1, sys2, sys3, sys4, sys5); np.testing.assert_array_almost_equal(sort(pole(sys1_5)), [-8., -6., -4., -2., -0.]) np.testing.assert_array_almost_equal(sort(zero(sys1_5)), [-9., -7., -5., -3., -1.]) # Parallel sys1_2 = ctrl.parallel(sys1, sys2) np.testing.assert_array_almost_equal(sort(pole(sys1_2)), [-4., -2.]) np.testing.assert_array_almost_equal(sort(zero(sys1_2)), sort(zero(sys1 + sys2))) sys1_3 = ctrl.parallel(sys1, sys2, sys3); np.testing.assert_array_almost_equal(sort(pole(sys1_3)), [-6., -4., -2.]) np.testing.assert_array_almost_equal(sort(zero(sys1_3)), sort(zero(sys1 + sys2 + sys3))) sys1_4 = ctrl.parallel(sys1, sys2, sys3, sys4); np.testing.assert_array_almost_equal(sort(pole(sys1_4)), [-8., -6., -4., -2.]) np.testing.assert_array_almost_equal(sort(zero(sys1_4)), sort(zero(sys1 + sys2 + sys3 + sys4))) sys1_5 = ctrl.parallel(sys1, sys2, sys3, sys4, sys5); np.testing.assert_array_almost_equal(sort(pole(sys1_5)), [-8., -6., -4., -2., -0.]) np.testing.assert_array_almost_equal(sort(zero(sys1_5)), sort(zero(sys1 + sys2 + sys3 + sys4 + sys5)))
def step_response_BF_cor(num1, den1, num2, den2, feedback=1, t=None): if t == None: t = np.linspace(0, 10, 101) sys1 = ctl.tf(num1, den1) sys2 = ctl.tf(num2, den2) sys_BO = ctl.series(sys1, sys2) sys_BF = g = ctl.feedback(sys_BO, feedback) lti_BF = sys_BF.returnScipySignalLti()[0][0] t, s = signal.step2(lti_BF, None, t) return t, s
def test_bode_margin(self): num = [1000] den = [1, 25, 100, 0] sys = ctrl.tf(num, den) ctrl.bode_plot(sys, margins=True,dB=False,deg = True) fig = plt.gcf() allaxes = fig.get_axes() mag_to_infinity = (np.array([6.07828691, 6.07828691]), np.array([1.00000000e+00, 1.00000000e-08])) assert_array_almost_equal(mag_to_infinity, allaxes[0].lines[2].get_data()) gm_to_infinty = (np.array([10., 10.]), np.array([4.00000000e-01, 1.00000000e-08])) assert_array_almost_equal(gm_to_infinty, allaxes[0].lines[3].get_data()) one_to_gm = (np.array([10., 10.]), np.array([1., 0.4])) assert_array_almost_equal(one_to_gm, allaxes[0].lines[4].get_data()) pm_to_infinity = (np.array([6.07828691, 6.07828691]), np.array([100000., -157.46405841])) assert_array_almost_equal(pm_to_infinity, allaxes[1].lines[2].get_data()) pm_to_phase = (np.array([6.07828691, 6.07828691]), np.array([-157.46405841, -180.])) assert_array_almost_equal(pm_to_phase, allaxes[1].lines[3].get_data()) phase_to_infinity = (np.array([10., 10.]), np.array([1.00000000e-08, -1.80000000e+02])) assert_array_almost_equal(phase_to_infinity, allaxes[1].lines[4].get_data())
def test_arguments(self): # Additional unit tests added on 25 May 2019 to increase coverage # Unknown canonical forms should generate exception sys = tf([1], [1, 2, 1]) np.testing.assert_raises( ControlNotImplemented, canonical_form, sys, 'unknown')
def test_observable_form(self): """Test the observable canonical form""" # Create a system in the observable canonical form coeffs = [1.0, 2.0, 3.0, 4.0, 1.0] A_true = np.polynomial.polynomial.polycompanion(coeffs) A_true = np.fliplr(np.flipud(A_true)) B_true = np.matrix("1.0 1.0 1.0 1.0").T C_true = np.matrix("1.0 0.0 0.0 0.0") D_true = 42.0 # Perform a coordinate transform with a random invertible matrix T_true = np.matrix([[-0.27144004, -0.39933167, 0.75634684, 0.44135471], [-0.74855725, -0.39136285, -0.18142339, -0.50356997], [-0.40688007, 0.81416369, 0.38002113, -0.16483334], [-0.44769516, 0.15654653, -0.50060858, 0.72419146]]) A = np.linalg.solve(T_true, A_true)*T_true B = np.linalg.solve(T_true, B_true) C = C_true*T_true D = D_true # Create a state space system and convert it to the observable canonical form sys_check, T_check = canonical_form(ss(A, B, C, D), "observable") # Check against the true values np.testing.assert_array_almost_equal(sys_check.A, A_true) np.testing.assert_array_almost_equal(sys_check.B, B_true) np.testing.assert_array_almost_equal(sys_check.C, C_true) np.testing.assert_array_almost_equal(sys_check.D, D_true) np.testing.assert_array_almost_equal(T_check, T_true) # Observable form only supports SISO sys = tf([[ [1], [1] ]], [[ [1, 2, 1], [1, 2, 1] ]]) np.testing.assert_raises(ControlNotImplemented, observable_form, sys)
def test_tf_input_with_int_element_works(self): num = 1 den = np.convolve([1.0, 2, 1], [1, 1, 1]) sys = ctl.tf(num, den) self.assertAlmostEqual(1.0, ctl.dcgain(sys))
def test_tf_num_with_numpy_int_element(self): num = np.convolve([1], [1, 1]) den = np.convolve([1, 2, 1], [1, 1, 1]) sys = ctl.tf(num, den) self.assertAlmostEqual(1.0, ctl.dcgain(sys))
def test_eval(self): sys_tf = ct.tf([1], [1, 2, 1]) frd_tf = FRD(sys_tf, np.logspace(-1, 1, 3)) np.testing.assert_almost_equal(sys_tf.evalfr(1), frd_tf.eval(1)) # Should get an error if we evaluate at an unknown frequency self.assertRaises(ValueError, frd_tf.eval, 2)
def plot_zero_pole_diagram(num,den,title): try: sys = control.tf(num,den) control.matlab.pzmap(sys,True,title) except: print('Error al Introducir los Datos, Vuelva a Intentarlo')
def siso_almost_equal(self,g,h): """siso_almost_equal(g,h) -> None Raises AssertionError if g and h, two SISO LTI objects, are not almost equal""" from control import tf, minreal gmh = tf(minreal(g-h,verbose=False)) if not (gmh.num[0][0]<self.TOL).all(): maxnum = max(abs(gmh.num[0][0])) raise AssertionError('systems not approx equal; max num. coeff is {}\nsys 1:\n{}\nsys 2:\n{}'.format(maxnum,g,h))
def Q2_perfFCN(Kp, Ti, Td): G = Kp * tf([Ti * Td, Ti, 1.0], [Ti, 0]) sys = feedback(series(G,F),1) res = step_response(sys, t) ISE = sum((val - 1)**2 for val in res[1]) return (ISE,) + stepinfo(res)
def weighting(wb, m, a): """weighting(wb,m,a) -> wf wb - design frequency (where |wf| is approximately 1) m - high frequency gain of 1/wf; should be > 1 a - low frequency gain of 1/wf; should be < 1 wf - SISO LTI object """ s = tf([1, 0], [1]) return (s / m + wb) / (s + wb * a)
def testTf2ssStaticSiso(self): """Regression: tf2ss for SISO static gain""" import control gsiso = control.tf2ss(control.tf(23, 46)) self.assertEqual(0, gsiso.states) self.assertEqual(1, gsiso.inputs) self.assertEqual(1, gsiso.outputs) # in all cases ratios are exactly representable, so assert_array_equal is fine np.testing.assert_array_equal([[0.5]], gsiso.D)
def pid_design(G, K_guess, d_tc, verbose=False, use_P=True, use_I=True, use_D=True): """ :param G: transfer function :param K_guess: gain matrix guess :param d_tc: time constant for derivative :param use_P: use p gain in design :param use_I: use i gain in design :param use_D: use d gain in design :return: (K, G_comp, Gc_comp) K: gain matrix G_comp: open loop compensated plant Gc_comp: closed loop compensated plant """ # compensator transfer function H = [] if use_P: H += [control.tf(1, 1)] if use_I: H += [control.tf((1), (1, 0))] if use_D: H += [control.tf((1, 0), (d_tc, 1))] H = np.array([H]).T H_num = [[H[i][j].num[0][0] for i in range(H.shape[0])] for j in range(H.shape[1])] H_den = [[H[i][j].den[0][0] for i in range(H.shape[0])] for j in range(H.shape[1])] H = control.tf(H_num, H_den) # print('G', G) # print('H', H) ss_open = control.tf2ss(G*H) if verbose: print('optimizing controller') K = lqr_ofb_design(K_guess, ss_open, verbose) if verbose: print('done') # print('K', K) # print('H', H) G_comp = control.series(G, H*K) Gc_comp = control.feedback(G_comp, 1) return K, G_comp, Gc_comp
def control_design_ulog(raw_data, do_plot=False, rolling_mean_window=100, verbose=False): """ Design a PID controller from log file. """ data, dt = setup_data(raw_data) d_tc = 1.0/125 # nyquist frequency of derivative in PID, (250 Hz/2) K_guess_att = np.matrix([[1.0]]).T roll_acc = data.ATT_RollRate.diff()/dt K_rollrate, G_cl_rollrate = attitude_control_design( 'roll rate', roll_acc, data.ATTC_Roll, rolling_mean_window=rolling_mean_window, do_plot=do_plot, verbose=verbose, d_tc=d_tc) tf_integrator = control.tf((1), (1, 0)) K_roll, G_roll, G_cl_roll = pid_design( G_cl_rollrate*tf_integrator, K_guess_att, d_tc, verbose=verbose, use_I=False, use_D=False) if do_plot: plt.figure() plot_loops('roll', G_roll, G_cl_roll) pitch_acc = data.ATT_PitchRate.diff()/dt K_pitchrate, G_cl_pitchrate = attitude_control_design( 'pitch rate', pitch_acc, data.ATTC_Pitch, rolling_mean_window=rolling_mean_window, do_plot=do_plot, d_tc=d_tc, verbose=verbose) K_pitch, G_pitch, G_cl_pitch = pid_design( G_cl_pitchrate*tf_integrator, K_guess_att, d_tc, verbose=verbose, use_I=False, use_D=False) if do_plot: plt.figure() plot_loops('pitch', G_pitch, G_cl_pitch) if verbose: print('G_roll', G_roll) print('G_cl_roll', G_cl_roll) print('G_cl_rollrate', G_cl_rollrate) print('G_pitch', G_pitch) print('G_cl_pitch', G_cl_pitch) print('G_cl_pitchrate', G_cl_pitchrate) return OrderedDict([ ('MC_ROLL_P', round(K_roll[0, 0], 3)), ('MC_ROLLRATE_P', round(K_rollrate[0, 0], 3)), ('MC_ROLLRATE_I', round(K_rollrate[1, 0], 3)), ('MC_ROLLRATE_D', round(K_rollrate[2, 0], 3)), ('MC_PITCH_P', round(K_pitch[0, 0], 3)), ('MC_PITCHRATE_P', round(K_pitchrate[0, 0], 3)), ('MC_PITCHRATE_I', round(K_pitchrate[1, 0], 3)), ('MC_PITCHRATE_D', round(K_pitchrate[2, 0], 3)), ]), locals()
def draw_step_response_feedback(K,sys): # Defining feedback system C = control.tf(K,1) # Using feedback according to http://nl.mathworks.com/help/control/examples/using-feedback-to-close-feedback-loops.html controled_sys = control.feedback(C*sys,1) poles = control.pole(controled_sys) y,t = control.step(controled_sys) plt.plot(t,y) plt.xlabel('t') plt.ylabel('y(t)')
def plant(): """plant() -> g g - LTI object; 2x2 plant with a RHP zero, at s=0.5. """ den = [0.2, 1.2, 1] gtf = tf([[[1], [1]], [[2, 1], [2]]], [[den, den], [den, den]]) return ss(gtf)
def testTf2ssStaticMimo(self): """Regression: tf2ss for MIMO static gain""" import control # 2x3 TFM gmimo = control.tf2ss(control.tf([[ [23], [3], [5] ], [ [-1], [0.125], [101.3] ]], [[ [46], [0.1], [80] ], [ [2], [-0.1], [1] ]])) self.assertEqual(0, gmimo.states) self.assertEqual(3, gmimo.inputs) self.assertEqual(2, gmimo.outputs) d = np.matrix([[0.5, 30, 0.0625], [-0.5, -1.25, 101.3]]) np.testing.assert_array_equal(d, gmimo.D)
def ramp_response_BF(num, den, feedback=1, t=None): if t == None: t = np.linspace(0, 10, 101) u = t # calcul de la BF sys = ctl.tf(num, den) sys_BF = g = ctl.feedback(sys, feedback) lti_BF = sys_BF.returnScipySignalLti()[0][0] t, s, x = signal.lsim2(lti_BF, u, t) return t, s
def test_tf2ss_robustness(self): """Unit test to make sure that tf2ss is working correctly. Source: https://github.com/python-control/python-control/issues/240 """ import control num = [ [[0], [1]], [[1], [0]] ] den1 = [ [[1], [1,1]], [[1,4], [1]] ] sys1tf = control.tf(num, den1) sys1ss = control.tf2ss(sys1tf) # slight perturbation den2 = [ [[1], [1e-10, 1, 1]], [[1,4], [1]] ] sys2tf = control.tf(num, den2) sys2ss = control.tf2ss(sys2tf) # Make sure that the poles match for StateSpace and TransferFunction np.testing.assert_array_almost_equal(np.sort(sys1tf.pole()), np.sort(sys1ss.pole())) np.testing.assert_array_almost_equal(np.sort(sys2tf.pole()), np.sort(sys2ss.pole()))
def weight_function(option, w_B, M=2, A=0): """ This function should return a transfer function object of a weight function as described in Skogestad pg. 58 (section 2.7.2) Options: 2.72 roll off of 1 for |S|. 2.73 forces a steeper slope on |S| 5.37 is good for tight control at high frequencies for S. 5.45 is good for T where the roll off of |T| is at least 1 at high frequencies, |T| is less than M at low frequencies and the cross over frequency is w_B. Parameters: option => the weight function (enter the equation ref in skogestad) w_B => minimum bandwidth (not optional) M => maximum peak of transfer function (optional at M = 2) A => steady state offset of less than A < 1 (optional at A = 0) """ if option == 2.72: wp = cn.tf([1, M * w_B], [M, M * A * w_B]) elif option == 2.73: wp = cn.tf([1, 2 * (M ** 0.5) * w_B, M * (w_B ** 2)], [M, w_B * M * 2 * (A ** 0.5), A * M * (w_B**2)]) elif option == 5.37: wp = cn.tf([1], [M]) + cn.tf([1, 0], [0, w_B]) elif option == 5.45: wp = cn.tf([1, 0], [w_B]) + cn.tf([1], [M]) return wp
def step_response_BF_pert(num1, den1, num2, den2, feedback=1, pert=0.25, pert_time=1, t=None): if t == None: t = np.linspace(0, 10, 101) sys1 = ctl.tf(num1, den1) sys2 = ctl.tf(num2, den2) sys_BO1 = ctl.series(sys1, sys2) sys_BF1 = g = ctl.feedback(sys_BO1, feedback) lti_BF1 = sys_BF1.returnScipySignalLti()[0][0] sys_BF2 = g = ctl.feedback(sys2, sys1) lti_BF2 = sys_BF2.returnScipySignalLti()[0][0] u = pert * (t > pert_time) t, s1 = signal.step2(lti_BF1, None, t) t, s2, trash = signal.lsim2(lti_BF2, u, t) s = s1 + s2 return t, s
def testSiso(self): "mixsyn with SISO system" from control import tf, augw, hinfsyn, mixsyn from control import ss # Skogestad+Postlethwaite, Multivariable Feedback Control, 1st Ed., Example 2.11 s = tf([1, 0], 1) # plant g = 200/(10*s+1)/(0.05*s+1)**2 # sensitivity weighting M = 1.5 wb = 10 A = 1e-4 w1 = (s/M+wb)/(s+wb*A) # KS weighting w2 = tf(1, 1) p = augw(g, w1, w2) kref, clref, gam, rcond = hinfsyn(p, 1, 1) ktest, cltest, info = mixsyn(g, w1, w2) # check similar to S+P's example np.testing.assert_allclose(gam, 1.37, atol = 1e-2) # mixsyn is a convenience wrapper around augw and hinfsyn, so # results will be exactly the same. Given than, use the lazy # but fragile testing option. np.testing.assert_allclose(ktest.A, kref.A) np.testing.assert_allclose(ktest.B, kref.B) np.testing.assert_allclose(ktest.C, kref.C) np.testing.assert_allclose(ktest.D, kref.D) np.testing.assert_allclose(cltest.A, clref.A) np.testing.assert_allclose(cltest.B, clref.B) np.testing.assert_allclose(cltest.C, clref.C) np.testing.assert_allclose(cltest.D, clref.D) np.testing.assert_allclose(gam, info[0]) np.testing.assert_allclose(rcond, info[1])
def testTf2SsDuplicatePoles(self): """Tests for "too few poles for MIMO tf #111" """ import control try: import slycot num = [ [ [1], [0] ], [ [0], [1] ] ] den = [ [ [1,0], [1] ], [ [1], [1,0] ] ] g = control.tf(num, den) s = control.ss(g) np.testing.assert_array_equal(g.pole(), s.pole()) except ImportError: print("Slycot not present, skipping")
def instantiate (self, specs = None): if specs is not None: self.specs = specs; self.Kc = insert (1, self.specs, 'Kc'); self.Ti = insert (np.inf, self.specs, 'Ti'); self.Td = insert (0, self.specs, 'Td'); self.b = insert (1.0, self.specs, 'b'); self.c = insert (1.0, self.specs, 'c'); self.N = insert (np.inf, self.specs, 'N'); num_p = self.Kc * self.b; den_p = 1; Cpff = ctrl.tf (num_p, den_p); Cpc = ctrl.tf (self.Kc, 1); if (self.Ti != np.inf): num_i = self.Kc; den_i = [self.Ti, 0]; Ci = ctrl.tf (num_i, den_i); else: Ci = 0; if (self.Td != 0): num_d = [self.Kc * self.Td * self.c, 0]; den_d = [(self.Td / self.N), 1]; Cdff = ctrl.tf (num_d, 1); Cdc = ctrl.tf ([self.Kc * self.Td], 1); else: Cdff = 0; Cdc = 0; self.Gff = Cpff + Ci + Cdff; self.Gc = Cpc + Ci + Cdc;
def add_control_lpf(C, p): LPF = tf(p, [1, p]) return C * LPF
def test_similarity(self): """Test similarty transform""" # Single input, single output systems siso_ini = tf2ss(tf([1, 1], [1, 1, 1])) for form in 'reachable', 'observable': # Convert the system to one of the canonical forms siso_can, T_can = canonical_form(siso_ini, form) # Use a similarity transformation to transform it back siso_sim = similarity_transform(siso_can, np.linalg.inv(T_can)) # Make sure everything goes back to the original form np.testing.assert_array_almost_equal(siso_sim.A, siso_ini.A) np.testing.assert_array_almost_equal(siso_sim.B, siso_ini.B) np.testing.assert_array_almost_equal(siso_sim.C, siso_ini.C) np.testing.assert_array_almost_equal(siso_sim.D, siso_ini.D) # Multi-input, multi-output systems mimo_ini = ss( [[-1, 1, 0, 0], [0, -2, 1, 0], [0, 0, -3, 1], [0, 0, 0, -4]], [[1, 0], [0, 0], [0, 1], [1, 1]], [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]], np.zeros((3, 2))) # Simple transformation: row/col flips + scaling mimo_txf = np.array( [[0, 1, 0, 0], [2, 0, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) # Transform the system and transform it back mimo_sim = similarity_transform(mimo_ini, mimo_txf) mimo_new = similarity_transform(mimo_sim, np.linalg.inv(mimo_txf)) np.testing.assert_array_almost_equal(mimo_new.A, mimo_ini.A) np.testing.assert_array_almost_equal(mimo_new.B, mimo_ini.B) np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C) np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D) # Make sure rescaling by identify does nothing mimo_new = similarity_transform(mimo_ini, np.eye(4)) np.testing.assert_array_almost_equal(mimo_new.A, mimo_ini.A) np.testing.assert_array_almost_equal(mimo_new.B, mimo_ini.B) np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C) np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D) # Time rescaling mimo_tim = similarity_transform(mimo_ini, np.eye(4), timescale=0.3) mimo_new = similarity_transform(mimo_tim, np.eye(4), timescale=1/0.3) np.testing.assert_array_almost_equal(mimo_new.A, mimo_ini.A) np.testing.assert_array_almost_equal(mimo_new.B, mimo_ini.B) np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C) np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D) # Time + transformation, in one step mimo_sim = similarity_transform(mimo_ini, mimo_txf, timescale=0.3) mimo_new = similarity_transform(mimo_sim, np.linalg.inv(mimo_txf), timescale=1/0.3) np.testing.assert_array_almost_equal(mimo_new.A, mimo_ini.A) np.testing.assert_array_almost_equal(mimo_new.B, mimo_ini.B) np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C) np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D) # Time + transformation, in two steps mimo_sim = similarity_transform(mimo_ini, mimo_txf, timescale=0.3) mimo_tim = similarity_transform(mimo_sim, np.eye(4), timescale=1/0.3) mimo_new = similarity_transform(mimo_tim, np.linalg.inv(mimo_txf)) np.testing.assert_array_almost_equal(mimo_new.A, mimo_ini.A) np.testing.assert_array_almost_equal(mimo_new.B, mimo_ini.B) np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C) np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D)
# Inverted Pendulum Parameter File import sys sys.path.append('..') # add parent directory import VTOLParam as P sys.path.append('../hw7') # add parent directory import VTOLParamHW7 as P7 import numpy as np # from scipy import signal from control import TransferFunction as tf import matplotlib.pyplot as plt import control as cnt # Compute plant transfer functions th_e = 0 Plant_long = tf([1.0/(P.mc + 2*P.mr)], [1, 0.0, 0.0]) Plant_lat_inner = tf([(-P.g)],[1, (P.mu/(P.mc+2*P.mr)), 0]) Plant_lat_outer = tf([(1/(P.Jc + 2*P.mr*P.d**2))], [1, 0, 0]) # Compute transfer function of controller C_pid_long = tf([(P7.kd_h+P7.kp_h*P.sigma), (P7.kp_h+P7.ki_h*P.sigma), P7.ki_h], [P.sigma, 1, 0]) C_pid_lat_inner = tf([(P7.kd_th+P7.kp_th*P.sigma), P7.kp_th, 0], [P.sigma, 1, 0]) C_pid_lat_outer = tf([(P7.kd_z+P7.kp_z*P.sigma), (P7.kp_z+P7.ki_z*P.sigma), P7.ki_z], [P.sigma, 1, 0]) # display bode plots of transfer functions cnt.bode(Plant_long, dB=False, margins=False) cnt.bode(Plant_long*C_pid_long, dB=False, margins=False)
import control as cn import matplotlib.pyplot as plt import numpy as np import control_add_on as cadd import siso_controllability as scont """ Scaling: x* refers to the unscaled perturbation variable q = q*/1 T_0 = T_0*/10 T = T*/10 """ G = cn.tf([0.8],[60, 1])*cn.tf([1],[12, 1]) #this is the only tf which changes due to scaling => equates to *0.1 Gd = cn.tf([20*0.6, 0.6],[60, 1])*cn.tf([1],[12,1]) # remember the dead time in measurement which is 3 seconds freqs = np.arange(0.001, 1, 0.001) plt.figure(1) scont.rule_three_four(G, Gd, R = 2, perfect = True, freq = freqs) plt.show() """ Performance: The plant will perform well to input (R) and disturbance tracking as it is self regulating i.e. |Gd| < 1 for all frequencies. Controllability: It is self regulating so... this is awkward... """
def add_control_proportional(C, kp): proportional = tf([kp], [1]) return C * proportional
# import matplotlib import control import tensorflow as tf from RM import ReplayMemory # matplotlib.use("TkAgg") # import matplotlib.pyplot as plt steps = 1000 episodes = 500 y_set = np.random.uniform(0, 1) eps = 0.01 # Tolerance του reward function c = 100 # reward value # Το περιβάλλον του προβλήματος g = control.tf([0.05, 0], [-0.6, 1]) sys = control.tf2ss(g) number_of_states = 2 number_of_actions = 1 def reward(state): if abs(state - y_set) < eps: return c else: return -np.power(abs(state - y_set), 2) def output(system, T, U, init_cond): return control.forced_response(system, T, U, init_cond)[1][49]
#!/usr/bin/env python from matplotlib.pyplot import plot, legend, savefig, gcf from numpy import logspace, sqrt, log from control import bode, tf k = 3 G = tf([k], [1]) om = logspace(-2, 1, 500) bode(G, dB=True, omega=om) f = gcf() r, mag, fas = f.get_children() mag.set_yticks([20, 8.75 * log(k), 0, -20]) fas.set_yticks([90, 0, -90]) mag.set_xticklabels([]) mag.set_yticklabels(['20.00', r'$k_{dB}$', '0.00', '-20.00']) fas.set_xticklabels([]) #a.set_yticklabels([]) # Se guarda la figura en la misma carpeta savefig("bodeordencero.pdf", bbox_inches='tight', pad_inches=0, transparent="True")
def set_plant(self, **kwargs): self.num = kwargs['num'] self.den = kwargs['den'] self.tf_plant = control.tf(self.num, self.den)
@author: St Elmo Wilken ''' import control as cn import matplotlib.pyplot as plt import numpy as np import siso_controllability as scont # Scaling: x* refers to the unscaled perturbation variable # q = q*/1 # T_0 = T_0*/10 # T = T*/10 # this is the only tf which changes due to scaling => equates to *0.1 G = cn.tf([0.8], [60, 1]) * cn.tf([1], [12, 1]) Gd = cn.tf([20 * 0.6, 0.6], [60, 1]) * cn.tf([1], [12, 1]) # remember the dead time in measurement which is 3 seconds freqs = np.arange(0.001, 1, 0.001) plt.figure(1) scont.rule_three_four(G, Gd, R=2, perfect=True, freq=freqs) plt.show() # Performance: # The plant will perform well to input (R) and disturbance tracking as it # is self regulating i.e. |Gd| < 1 for all frequencies. # # Controllability: # It is self regulating so... this is awkward...
plantWn11 = 3 * hz2rps plantD11 = 0.2 plantK12 = 0.5 plantWn12 = 5 * hz2rps plantD12 = 0.3 plantK21 = 0.25 plantWn21 = 4 * hz2rps plantD21 = 0.1 plantK22 = 1.0 plantWn22 = 6 * hz2rps plantD22 = 0.4 sysPlant = control.tf( [[[0, 0, plantK11 * plantWn11**2], [0, 0, plantK21 * plantWn21**2]], [[0, 0, plantK12 * plantWn12**2], [0, 0, plantK22 * plantWn22**2]]], [[[1, 2.0 * plantD11 * plantWn11, plantWn11**2], [1, 2.0 * plantD21 * plantWn21, plantWn21**2]], [[1, 2.0 * plantD12 * plantWn12, plantWn12**2], [1, 2.0 * plantD22 * plantWn22, plantWn22**2]]]) sysK = control.ss([], [], [], [[0.75, 0.25], [0.25, 0.5]]) elif False: # SP Example #3.8 freqLin_hz = np.linspace(1e-2 * rps2hz, 1e2 * rps2hz, 200) freqRate_hz = 50 plantK11 = 1.0 plantWn11 = np.sqrt(5) plantD11 = 6 / (2 * plantWn11) plantK12 = 1.0 plantWn12 = 1.0 * plantWn11
import control import matplotlib.pyplot as plt import numpy as np #if using termux import subprocess import shlex #end if #coefficents of open loop transfer function den = [4, 5, 1, 0] num1 = [1.25] num2 = [2] num3 = [1] #Creating a transfer function G = num/den G1 = control.tf(num1, den) G2 = control.tf(num2, den) G3 = control.tf(num3, den) #plotting the nyquist plot for three different transfer functions for a variable K control.nyquist(G1, label='K=1.25') control.nyquist(G2, label='K=2') control.nyquist(G3, label='K=1') plt.xlim(-4, 0) plt.ylim(-2, 2) plt.legend() plt.annotate("-1+0j", (-1, 0)) plt.xlabel('Re(s)') plt.ylabel('Im(s)') #if using termux
for filename in filenames: df = pd.read_csv(filename, index_col=None, header=0) df = np.array(df) df = np.concatenate(df) dfs.append(df) return dfs dfs = read_profiles() tinitial = 0 tfinal = 59 N = len(dfs[0]) print(N) times = np.linspace(tinitial, tfinal, N) s = tf([1, 0], [0, 1]) Gp = 1 / (s**2 + s + 1) Gd = (s + 1) / (s**2 + s + 1) def calc_average_perf(s, Gp, Gd, Kc, tauI, tauD, tauC, times): Perf = np.linspace(0, 1, 100) Gc = Kc * (1 + 1 / (tauI * s) + tauD * s / (tauC * s + 1)) sys_D = Gd / (1 + Gp * Gc) average_Perf = [] for j in range(0, 100): Ti = dfs[j] _, Y, _ = control.forced_response(sys_D, times, Ti) sys_U = Gc _, U, _ = control.forced_response(sys_U, times, -Y) Perf[j] = sum(abs(Y)) + 0.2 * sum(abs(U))
# Single link arm Parameter File import sys sys.path.append('..') # add parent directory import massParam as P import numpy as np import control as cnt from control import TransferFunction as tf import matplotlib.pyplot as plt # from mpldatacursor import datacursor # Compute plant transfer functions th_e = 0 Plant = tf([1.0 / P.m], [1, P.b / P.m, P.k / P.m]) # Bode plot of the plant plt.figure(3), cnt.bode_plot(Plant, dB=False, margins=False) # if you want specific values at specific frequencies, you can do the following: mag, phase, omega = cnt.bode(Plant, plot=False, dB=False, omega=[0.3, 10.0, 100.0]) # Closes plot windows when the user presses a button. plt.pause(0.0001) # not sure why this is needed for both figures to display print('Close window to end program') plt.show()
def test_modal_form_MIMO(self): """Test error because modal form only supports SISO""" sys = tf([[[1], [1]]], [[[1, 2, 1], [1, 2, 1]]]) with pytest.raises(ControlNotImplemented): modal_form(sys)
def add_control_lag(C, z, M): Lag = tf([1, z], [1, z/M]) return C * Lag
import control as co import matplotlib.pyplot as plt import numpy as np s = co.tf('s') g = (s**2 + 5 * s + 4) / (s**2 - 5 * s - 24) """a) οχι ευσταθές""" print(co.pole(g)) co.root_locus(g, ylim=(-0.25, 0.25)) """b) απο root_locus μια καλή τιμή για ευστάθεια είναι k=8""" g_gained = co.feedback(8 * g, sign=-1) plt.figure(2) #co.root_locus(g_gained, ylim=(-0.025, 0.025)) """c) """ print(g_gained) co.pzmap(g_gained) plt.show()
def add_control_integral(C, ki): integrator = tf([1, ki], [1, 0]) return C * integrator
def test_modal_form(self): """Test the modal canonical form""" # Create a system in the modal canonical form A_true = np.diag([4.0, 3.0, 2.0, 1.0]) # order from the largest to the smallest B_true = np.matrix("1.1 2.2 3.3 4.4").T C_true = np.matrix("1.3 1.4 1.5 1.6") D_true = 42.0 # Perform a coordinate transform with a random invertible matrix T_true = np.matrix( [[-0.27144004, -0.39933167, 0.75634684, 0.44135471], [-0.74855725, -0.39136285, -0.18142339, -0.50356997], [-0.40688007, 0.81416369, 0.38002113, -0.16483334], [-0.44769516, 0.15654653, -0.50060858, 0.72419146]]) A = np.linalg.solve(T_true, A_true) * T_true B = np.linalg.solve(T_true, B_true) C = C_true * T_true D = D_true # Create a state space system and convert it to the modal canonical form sys_check, T_check = canonical_form(ss(A, B, C, D), "modal") # Check against the true values # TODO: Test in respect to ambiguous transformation (system characteristics?) np.testing.assert_array_almost_equal(sys_check.A, A_true) #np.testing.assert_array_almost_equal(sys_check.B, B_true) #np.testing.assert_array_almost_equal(sys_check.C, C_true) np.testing.assert_array_almost_equal(sys_check.D, D_true) #np.testing.assert_array_almost_equal(T_check, T_true) # Check conversion when there are complex eigenvalues A_true = np.array([[-1, 1, 0, 0], [-1, -1, 0, 0], [0, 0, -2, 0], [0, 0, 0, -3]]) B_true = np.array([[0], [1], [0], [1]]) C_true = np.array([[1, 0, 0, 1]]) D_true = np.array([[0]]) A = np.linalg.solve(T_true, A_true) * T_true B = np.linalg.solve(T_true, B_true) C = C_true * T_true D = D_true # Create state space system and convert to modal canonical form sys_check, T_check = canonical_form(ss(A, B, C, D), 'modal') # Check A and D matrix, which are uniquely defined np.testing.assert_array_almost_equal(sys_check.A, A_true) np.testing.assert_array_almost_equal(sys_check.D, D_true) # B matrix should be all ones (or zero if not controllable) # TODO: need to update modal_form() to implement this if np.allclose(T_check, T_true): np.testing.assert_array_almost_equal(sys_check.B, B_true) np.testing.assert_array_almost_equal(sys_check.C, C_true) # Make sure Hankel coefficients are OK from numpy.linalg import matrix_power for i in range(A.shape[0]): np.testing.assert_almost_equal( np.dot(np.dot(C_true, matrix_power(A_true, i)), B_true), np.dot(np.dot(C, matrix_power(A, i)), B)) # Reorder rows to get complete coverage (real eigenvalue cxrtvfirst) A_true = np.array([[-1, 0, 0, 0], [0, -2, 1, 0], [0, -1, -2, 0], [0, 0, 0, -3]]) B_true = np.array([[0], [0], [1], [1]]) C_true = np.array([[0, 1, 0, 1]]) D_true = np.array([[0]]) A = np.linalg.solve(T_true, A_true) * T_true B = np.linalg.solve(T_true, B_true) C = C_true * T_true D = D_true # Create state space system and convert to modal canonical form sys_check, T_check = canonical_form(ss(A, B, C, D), 'modal') # Check A and D matrix, which are uniquely defined np.testing.assert_array_almost_equal(sys_check.A, A_true) np.testing.assert_array_almost_equal(sys_check.D, D_true) # B matrix should be all ones (or zero if not controllable) # TODO: need to update modal_form() to implement this if np.allclose(T_check, T_true): np.testing.assert_array_almost_equal(sys_check.B, B_true) np.testing.assert_array_almost_equal(sys_check.C, C_true) # Make sure Hankel coefficients are OK from numpy.linalg import matrix_power for i in range(A.shape[0]): np.testing.assert_almost_equal( np.dot(np.dot(C_true, matrix_power(A_true, i)), B_true), np.dot(np.dot(C, matrix_power(A, i)), B)) # Modal form only supports SISO sys = tf([[[1], [1]]], [[[1, 2, 1], [1, 2, 1]]]) np.testing.assert_raises(ControlNotImplemented, modal_form, sys)
import control as co import numpy as np import matplotlib.pyplot as plt import sympy as sp num = [-25.95] den = [1, 7.25, 9.77] sys = co.tf(num, den) co.rlocus(sys) plt.title('K->+inf') plt.figure(2) sys = co.tf(num, den) co.rlocus(-sys) plt.title('K->-inf') plt.grid() plt.show()
ISE = tdiv + max(yc) else: # Realizando a Integral do erro quadrado t1 = feedback(F, 1) _, y1 = step_response(t1, time) SE = np.square(np.subtract(1, y1)) ISE = np.sum(SE) return (ISE) #Paramentros da Planta K = 2 T = 4 #Função Transferência da Planta G = tf(K, [T, 1]) #Criando vetor de tempo para ser o mesmo em todas as análises tdiv = 201 time = np.linspace(0, 6, tdiv) #Fator indesejado, Sinal de controle >max maxcontrolador = 3 Kpi = 0 Kii = 0 Kdi = 0 faixa = 5 * 100 # Faixa de valores para as variáveis do GA varbound = np.array([[-Kpi, faixa - Kpi], [-Kii, faixa - Kii], [-Kdi, faixa - Kdi]])
# p: lpf cutoff frequency def add_control_lpf(C, p): LPF = tf(p, [1, p]) return C * LPF # phase lead: increase PM (stability) # w_L: location of maximum frequency bump # M: separation between zero and pole def add_control_lead(C, w_L, M): gain = (1.0+np.sqrt(M))/(1.0+1.0/np.sqrt(M)) Lead = tf([gain * 1.0, gain * w_L / np.sqrt(M)], [1.0, w_L * np.sqrt(M)]) return C * Lead # Compute plant transfer functions Plant = cnt.tf([1.0 / (P.m * P.ell**2)], [1, P.b/(P.m*P.ell**2), P.k1/(P.m*P.ell**2)]) C_pid = cnt.tf([(P.kd+P.kp*P.sigma), (P.kp+P.ki*P.sigma), P.ki], [P.sigma, 1, 0]) PLOT = True # PLOT = False # calculate bode plot and gain and phase margin mag, phase, omega = cnt.bode(Plant, dB=True, omega=np.logspace(-3, 5), Plot=False) gm, pm, Wcg, Wcp = cnt.margin(Plant*C_pid) print(" pm: ", pm, " Wcp: ", Wcp, "gm: ", gm, " Wcg: ", Wcg) if PLOT: plt.figure(3), plt.clf() plt.subplot(211), plt.grid(True) plantMagPlot, = plt.semilogx(omega, 20*np.log10(mag), label='Plant') plt.subplot(212), plt.grid(True)
def simulate_pendulum_MPC(sim_options): seed_val = get_parameter(sim_options, 'seed_val') if seed_val is not None: np.random.seed(seed_val) Ac = get_parameter(sim_options, 'Ac') Bc = get_parameter(sim_options, 'Bc') Cc = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.]]) Dc = np.zeros((2, 1)) [nx, nu] = Bc.shape # number of states and number or inputs ny = np.shape(Cc)[0] Ts_slower_loop = get_parameter(sim_options, 'Ts_slower_loop') ratio_Ts = int(Ts_slower_loop // Ts_PID) # Brutal forward euler discretization Ad = np.eye(nx) + Ac * Ts_slower_loop Bd = Bc * Ts_slower_loop Cd = Cc Dd = Dc # Standard deviation of the measurement noise on position and angle std_npos = get_parameter(sim_options, 'std_npos') std_nphi = get_parameter(sim_options, 'std_nphi') # Force disturbance std_dF = get_parameter(sim_options, 'std_dF') # Disturbance power spectrum w_F = get_parameter(sim_options, 'w_F') # bandwidth of the force disturbance tau_F = 1 / w_F Hu = control.TransferFunction([1], [1 / w_F, 1]) Hu = Hu * Hu Hud = control.matlab.c2d(Hu, Ts_PID) N_sim_imp = tau_F / Ts_PID * 20 t_imp = np.arange(N_sim_imp) * Ts_PID t, y = control.impulse_response(Hud, t_imp) y = y[0] std_tmp = np.sqrt(np.sum(y**2)) # np.sqrt(trapz(y**2,t)) Hu = Hu / (std_tmp) * std_dF N_skip = int(20 * tau_F // Ts_PID) # skip initial samples to get a regime sample of d t_sim_d = get_parameter(sim_options, 'len_sim') # simulation length (s) N_sim_d = int(t_sim_d // Ts_PID) N_sim_d = N_sim_d + N_skip + 1 e = np.random.randn(N_sim_d) te = np.arange(N_sim_d) * Ts_PID _, d, _ = control.forced_response(Hu, te, e) d = d.ravel() angle_ref = d[N_skip:] # Initialize simulation system t0 = 0 phi0 = 0.0 * 2 * np.pi / 360 x0 = np.array([0, 0, phi0, 0]) # initial state system_dyn = ode(f_ODE_wrapped).set_integrator('vode', method='bdf') # dopri5 # system_dyn = ode(f_ODE_wrapped).set_integrator('dopri5') system_dyn.set_initial_value(x0, t0) system_dyn.set_f_params(0.0) # Default controller parameters - P = -100.0 I = -1 D = -10 N = 100.0 kP = control.tf(P, 1, Ts_PID) kI = I * Ts_PID * control.tf([0, 1], [1, -1], Ts_PID) kD = D * control.tf([N, -N], [1.0, Ts_PID * N - 1], Ts_PID) PID_tf = kP + kD + kI PID_ss = control.ss(PID_tf) k_PID = LinearStateSpaceSystem(A=PID_ss.A, B=PID_ss.B, C=PID_ss.C, D=PID_ss.D) # Simulate in closed loop len_sim = get_parameter(sim_options, 'len_sim') # simulation length (s) nsim = int( len_sim // Ts_slower_loop ) #int(np.ceil(len_sim / Ts_slower_loop)) # simulation length(timesteps) # watch out! +1 added, is it correct? t_vec = np.zeros((nsim, 1)) t_calc_vec = np.zeros( (nsim, 1)) # computational time to get MPC solution (+ estimator) status_vec = np.zeros((nsim, 1)) x_vec = np.zeros((nsim, nx)) x_ref_vec = np.zeros((nsim, nx)) y_vec = np.zeros((nsim, ny)) y_meas_vec = np.zeros((nsim, ny)) u_vec = np.zeros((nsim, nu)) nsim_fast = int(len_sim // Ts_PID) t_vec_fast = np.zeros((nsim_fast, 1)) x_vec_fast = np.zeros( (nsim_fast, nx)) # finer integration grid for performance evaluation ref_angle_vec_fast = np.zeros((nsim_fast, 1)) y_meas_vec_fast = np.zeros((nsim_fast, ny)) x_ref_vec_fast = np.zeros( (nsim_fast, nx)) # finer integration grid for performance evaluatio u_vec_fast = np.zeros( (nsim_fast, nu)) # finer integration grid for performance evaluatio Fd_vec_fast = np.zeros((nsim_fast, nu)) # t_int_vec_fast = np.zeros((nsim_fast, 1)) emergency_vec_fast = np.zeros((nsim_fast, 1)) # t_step = t0 x_step = x0 u_PID = None for idx_fast in range(nsim_fast): ## Determine step type: fast simulation only or MPC step idx_inner_controller = idx_fast // ratio_Ts run_inner_controller = (idx_fast % ratio_Ts) == 0 y_step = Cd.dot(x_step) # y[i] from the system ymeas_step = np.copy(y_step) ymeas_step[0] += std_npos * np.random.randn() ymeas_step[1] += std_nphi * np.random.randn() y_meas_vec_fast[idx_fast, :] = ymeas_step # Output for step i # Ts_slower_loop outputs if run_inner_controller: # it is also a step of the simulation at rate Ts_slower_loop if idx_inner_controller < nsim: t_vec[idx_inner_controller, :] = t_step y_vec[idx_inner_controller, :] = y_step y_meas_vec[idx_inner_controller, :] = ymeas_step u_vec[idx_inner_controller, :] = u_PID # PID angle CONTROLLER ref_angle = angle_ref[idx_fast] error_angle = ref_angle - ymeas_step[1] u_PID = k_PID.output(error_angle) u_PID[u_PID > 10.0] = 10.0 u_PID[u_PID < -10.0] = -10.0 u_TOT = u_PID # Ts_fast outputs t_vec_fast[idx_fast, :] = t_step x_vec_fast[idx_fast, :] = x_step #system_dyn.y u_vec_fast[idx_fast, :] = u_TOT Fd_vec_fast[idx_fast, :] = 0.0 ref_angle_vec_fast[idx_fast, :] = ref_angle ## Update to step i+1 k_PID.update(error_angle) # Controller simulation step at rate Ts_slower_loop if run_inner_controller: pass # System simulation step at rate Ts_fast time_integrate_start = time.perf_counter() system_dyn.set_f_params(u_TOT) system_dyn.integrate(t_step + Ts_PID) x_step = system_dyn.y #x_step = x_step + f_ODE_jit(t_step, x_step, u_TOT)*Ts_fast #x_step = x_step + f_ODE(0.0, x_step, u_TOT) * Ts_fast t_int_vec_fast[ idx_fast, :] = time.perf_counter() - time_integrate_start # Time update t_step += Ts_PID simout = { 't': t_vec, 'x': x_vec, 'u': u_vec, 'y': y_vec, 'y_meas': y_meas_vec, 'x_ref': x_ref_vec, 'status': status_vec, 'Fd_fast': Fd_vec_fast, 't_fast': t_vec_fast, 'x_fast': x_vec_fast, 'x_ref_fast': x_ref_vec_fast, 'u_fast': u_vec_fast, 'y_meas_fast': y_meas_vec_fast, 'emergency_fast': emergency_vec_fast, 'K': k_PID, 'nsim': nsim, 'Ts_slower_loop': Ts_slower_loop, 't_calc': t_calc_vec, 'ref_angle_fast': ref_angle_vec_fast, 't_int_fast': t_int_vec_fast } return simout
Created on Mon Feb 22 21:22:49 2016 @author: Charles """ from __future__ import division import numpy as np from control import tf, ss from matplotlib import pyplot as plot import scipy # Exact Process Model K = 2 tau = 5 numer = [K] denom = [tau,1] sys1 = tf(numer,denom) #Transformation to State-Space sys = ss(sys1) A, B, C, D = np.asarray(sys.A), np.asarray(sys.B), np.asarray(sys.C), \ np.asarray(sys.D) Nstates = A.shape[0] z = np.zeros((Nstates, 1)) t_start = 0 t_end = 100 dt = 0.01 tspan = np.arange(t_start, t_end, dt) npoints = len(tspan)
def add_control_lead(C, w_L, M): gain = (1.0+np.sqrt(M))/(1.0+1.0/np.sqrt(M)) Lead = tf([gain * 1.0, gain * w_L / np.sqrt(M)], [1.0, w_L * np.sqrt(M)]) return C * Lead
def test_dcgain(self): sys = tf(84, [1, 2]) np.testing.assert_allclose(sys.dcgain(), 42) np.testing.assert_allclose(dcgain(sys), 42)
def test_observable_form_MIMO(self): """Test error as Observable form only supports SISO""" sys = tf([[[1], [1] ]], [[[1, 2, 1], [1, 2, 1]]]) with pytest.raises(ControlNotImplemented): observable_form(sys)
def system(K_P): num = [10] den = [10, 25, 11, 2] G = ctl.tf(num, den) G_0 = K_P * G return ctl.feedback(G_0)
if e_mode == "two_added_sines": e = 1 * np.sin(t + np.pi / 4) + 2 * np.sin(0.25 * t + 0) # ################# initialize controllers T_s = 0.5 # T_sample K_p = 2 K_I = 0.5 K_D = 2 K_N = 0.5 # P-controller # apparently the control toolbox cant simulate this simple system due to some strange bug # PI-controller pi_controller = ctrl.tf([K_p + K_I * T_s / 2, K_I * T_s / 2 - K_p], [1, -1], T_s) # PID-controller a1 = K_p + K_I * T_s / 2 a0 = K_I * T_s / 2 - K_p num = [ a1 + K_D * K_N, -a1 + K_N * T_s * a1 + a0 - 2 * K_D * K_N, -a0 + K_N * T_s * a0 + K_D * K_N ] den = [1, K_N * T_s - 2, -K_N * T_s + 1] pid_controller = ctrl.tf(num, den, T_s) print("PI-controller: " + str(pi_controller)) print("PID-controller: " + str(pid_controller)) # ################# simulate systems
""" Spyder Editor This is a temporary script file. """ import control import carla from scipy import signal import matplotlib.pyplot as plt import numpy as np from collections import deque num = [1.0] den = [1., 1., 0.5] sys1 = control.tf(num, den, 0.002) t = np.linspace(0, 1, 500, endpoint=False) #sig = np.sin(2 * np.pi * t) pwm = signal.square(2 * np.pi * 5 * t) num_low_pass = [1.0] den_low_pass = [1.0, 10.0] low_pass_c = control.tf(num_low_pass, den_low_pass) low_pass_d = control.sample_system(low_pass_c, 0.002) sys = low_pass_d sys = control.tf2ss(sys) T, yout, xout = control.forced_response(sys, U=pwm, X0=0)
import numpy as np import matplotlib.pyplot as plt import control from scipy import signal #if using termux import subprocess import shlex #end if k = 96 num = [k] den = [1, 12, 44, 48, 0] sys = control.tf(num, den) gm, pm, wg, wp = control.margin(sys) s = signal.lti(num, den) w, mag, phase = signal.bode(s) gain_y = np.full((len(w)), -4.81) phase_y = np.full((len(w)), -180) idx = np.argwhere(np.diff(np.sign(mag - gain_y))).flatten() print("The Freqency of intersection:", w[idx]) plt.figure() plt.semilogx(w, mag) plt.plot(w, gain_y) plt.plot(2.06, -4.81, 'ro') plt.ylabel("Gain") plt.xlabel("Frequency") plt.grid()