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')
