def testMargin(self, siso): """Test margin()""" #! TODO: check results to make sure they are OK gm, pm, wg, wp = margin(siso.tf1) gm, pm, wg, wp = margin(siso.tf2) gm, pm, wg, wp = margin(siso.ss1) gm, pm, wg, wp = margin(siso.ss2) gm, pm, wg, wp = margin(siso.ss2 * siso.ss2 * 2) np.testing.assert_array_almost_equal( [gm, pm, wg, wp], [1.5451, 75.9933, 1.2720, 0.6559], decimal=3)
def testCombi01(self): """Test from a "real" case, combines tf, ss, connect and margin. This is a type 2 system, with phase starting at -180. The margin command should remove the solution for w = nearly zero. """ # Example is a concocted two-body satellite with flexible link Jb = 400 Jp = 1000 k = 10 b = 5 # can now define an "s" variable, to make TF's s = tf([1, 0], [1]) hb1 = 1 / (Jb * s) hb2 = 1 / s hp1 = 1 / (Jp * s) hp2 = 1 / s # convert to ss and append sat0 = append(ss(hb1), ss(hb2), k, b, ss(hp1), ss(hp2)) # connection of the elements with connect call Q = [ [1, -3, -4], # link moment (spring, damper), feedback to body [2, 1, 0], # link integrator to body velocity [3, 2, -6], # spring input, th_b - th_p [4, 1, -5], # damper input [5, 3, 4], # link moment, acting on payload [6, 5, 0] ] inputs = [1] outputs = [1, 2, 5, 6] sat1 = connect(sat0, Q, inputs, outputs) # matched notch filter wno = 0.19 z1 = 0.05 z2 = 0.7 Hno = (1 + 2 * z1 / wno * s + s**2 / wno**2) / (1 + 2 * z2 / wno * s + s**2 / wno**2) # the controller, Kp = 1 for now Kp = 1.64 tau_PD = 50. Hc = (1 + tau_PD * s) * Kp # start with the basic satellite model sat1, and get the # payload attitude response Hp = tf(np.array([0, 0, 0, 1]) * sat1) # total open loop Hol = Hc * Hno * Hp gm, pm, wg, wp = margin(Hol) # print("%f %f %f %f" % (gm, pm, wg, wp)) np.testing.assert_allclose(gm, 3.32065569155) np.testing.assert_allclose(pm, 46.9740430224) np.testing.assert_allclose(wg, 0.176469728448) np.testing.assert_allclose(wp, 0.0616288455466)
def servo_loop(gain, tau): # + # r----->O------C------G------->y # - ^ | # | | # |-------------H---| g = ctrl.series( ctrl.tf(np.array([1]), np.array([1, 0])), # Integrator ctrl.tf(np.array([1]), np.array([0.1, 1]))) # Actuator lag h = ctrl.tf(np.array([1]), np.array([0.01, 1])) # Sensor lag c = ctrl.tf(np.array([tau, 1]), np.array([0.008, 1])) * gain # Control law fbsum = mat.ss([], [], [], [1, -1]) sys_ol = mat.append(mat.tf2ss(c), mat.tf2ss(g), mat.tf2ss(h)) sys_cl = mat.append(mat.tf2ss(c), mat.tf2ss(g), mat.tf2ss(h), fbsum) q_ol = np.array([[2, 1], [3, 2]]) q_cl = np.array([[1, 4], [2, 1], [3, 2], [5, 3]]) sys_ol = mat.connect(sys_ol, q_ol, [1], [3]) sys_cl = mat.connect(sys_cl, q_cl, [4], [2]) return mat.margin(sys_ol), sys_ol, sys_cl
from control.matlab import linspace, logspace from control.matlab import bode, rlocus, nyquist, nichols, ngrid, pzmap from control.matlab import freqresp, evalfr from control.matlab import hsvd, balred, modred, minreal from control.matlab import place, place_varga, acker from control.matlab import lqr, ctrb, obsv, gram from control.matlab import pade from control.matlab import unwrap, c2d, isctime, isdtime from control.matlab import connect, append from control.exception import ControlArgument from control.frdata import FRD from control.tests.conftest import slycotonly # for running these through Matlab or Octave ''' siso_ss1 = ss([1. -2.; 3. -4.], [5.; 7.], [6. 8.], [0]) siso_tf1 = tf([1], [1, 2, 1]) siso_tf2 = tf([1, 1], [1, 2, 3, 1]) siso_tf3 = tf(siso_ss1) siso_ss2 = ss(siso_tf2) siso_ss3 = ss(siso_tf3) siso_tf4 = tf(siso_ss2) A =[ 1. -2. 0. 0.; 3. -4. 0. 0.; 0. 0. 1. -2.; 0. 0. 3. -4. ] B = [ 5. 0.;
plt.axhline(1, color="b", linestyle="--") plt.xlim(0, 3) plt.show() # impulse response yout, T = matlab.impulse(sys, t) plt.plot(T, yout) plt.axhline(0, color="b", linestyle="--") plt.xlim(0, 3) #plt.show() # nyquist diagram matlab.nyquist(sys) #plt.show() # bode diagram matlab.bode(sys) #plt.show() # root locus matlab.rlocus(sys) #plt.show() # pole matlab.pole(sys) #plt.show() # margin matlab.margin(sys) #plt.show()
import matplotlib.pyplot as plt import control.matlab as ml from math import * k = 86.8753439248997 #Defining the transfer function T(s) #Since the transfer function T(s) is (s+2)/(s^4 + 12s^3 + 47s^2 + 60s) s1 = signal.lti([1 * k, 2 * k], [1, 12, 47, 60, 0]) #signal.bode takes transfer function as input and returns frequency,magnitude and phase arrays w, mag, phase = signal.bode(s1) #Wgc using builtin functions s = ml.tf('s') gm, pm, Wpc, Wgc = ml.margin( (s * k + 2 * k) / (s**4 + 12 * s**3 + 47 * s**2 + 60 * s)) print('Wgc = ', Wgc, ' which matches the graphical solution') print('PM =', pm, 'which is close to 40 degrees') # phase of G(s) at Wgc phase_at_Wgc = -180 + pm.round() plt.subplot(2, 1, 1) plt.semilogx(w, mag, 'b', label='_nolegend_') # Bode Magnitude plot plt.plot(Wgc, 0, 'o', label='_nolegend_') plt.xlabel('Frequency(rad/s)') plt.ylabel('Mag') plt.axhline(y=0, xmin=0, xmax=1, color='b', linestyle='dashed') plt.axvline(x=Wgc, ymin=0, ymax=0.6, color='k', linestyle='dashed') plt.text(1, -30, '({}, {})'.format(Wgc.round(2), 0)) plt.legend(['0 dB line'], loc='lower left')