def plot_bode(self, fig):
tf = self.tf if self.loop_type == 'ol' else self.tf_plant * self.tf_comp
if self.settings.is_bode_default():
mag, phase, omega = control.bode(tf, dB=True, Plot=False)
else:
fmin = self.settings.get_bode_freq_min()
fmax = self.settings.get_bode_freq_max()
mag, phase, omega = control.bode(tf,
omega_limits=[fmin, fmax],
dB=True,
Plot=False)
mag = 20 * np.log10(mag)
phase = phase * 180.0 / np.pi
fig.clf()
ax1 = fig.add_subplot(2, 1, 1)
ax1.semilogx(omega, mag)
ax1.grid(which="both")
ax1.set_xlabel('Frequency (rad/s)')
ax1.set_ylabel('Magnitude (dB)')
ax2 = fig.add_subplot(2, 1, 2)
ax2.semilogx(omega, phase)
ax2.grid(which="both")
ax2.set_xlabel('Frequency (rad/s)')
ax2.set_ylabel('Phase (deg)')
return fig
def plot_loops(name, G_ol, G_cl):
# type: (str, control.tf, control.tf) -> None
"""
Plot loops
:param name: Name of axis
:param G_ol: open loop transfer function
:param G_cl: closed loop transfer function
"""
plt.figure()
plt.plot(*control.step_response(G_cl, np.linspace(0, 1, 1000)))
plt.title(name + ' step response')
plt.grid()
plt.figure()
control.bode(G_ol)
print('margins', control.margin(G_ol))
plt.subplot(211)
plt.title(name + ' open loop bode plot')
plt.figure()
control.rlocus(G_ol, np.logspace(-2, 0, 1000))
for pole in G_cl.pole():
plt.plot(np.real(pole), np.imag(pole), 'rs')
plt.title(name + ' root locus')
plt.grid()
def plot_attitude_rate_design(name, G_ol, G_cl):
import matplotlib.pyplot as plt
plt.figure()
plt.plot(*control.step_response(G_cl, np.linspace(0, 1, 1000)))
plt.title(name + ' rate step resposne')
plt.figure()
control.bode(G_ol)
print(control.margin(G_ol))
plt.figure()
control.rlocus(G_ol, np.logspace(-2, 0, 1000))
for pole in G_cl.pole():
plt.plot(np.real(pole), np.imag(pole), 'rs')
plt.title(name + ' rate step root locus')
def Bode_Margin(sys, omega):
w = omega
mag, phaserad, w = ctrl.bode(sys, w)
magdb = 20 * log(mag)
phasedeg = phaserad * 180 / pi
gm, pm, wg, wp = ctrl.margin(sys)
a1 = f.add_subplot(221)
a1.set_xscale("log")
a1.plot(w, magdb)
a1.plot(wp, 0, '.y')
a1.text(wp, 0 + 25, "wc=%.2f" % wp)
# a1.title = "Bode"
# a1.ylabel = "Magnitude dB "
# a1.xlabel = "Angular frequency"
a2 = f.add_subplot(223)
a2.set_xscale("log")
a2.plot(w, phasedeg)
a2.plot(wp, pm - 180, '.y')
a2.text(wp, pm - 180 + 25, "phase=%.2f" % (pm - 180))
# a2.ylabel = "Phase (degree) "
# a2.xlabel = "Angular frequency"
a3 = f.add_subplot(122)
sysF = sys / (1 + sys)
T = np.arange(0, 30, 0.5)
T, yout = ctrl.step_response(sysF, T)
a3.plot(T, yout)
# a3.title = "step response"
# a3.xlabel = "time"
# a3.ylabel = "Magnitude"
return wp, pm
def zpk(z, p, k):
Gs = control.tf(36, [1, 3.6, 0])
# Define Compensator transfer function
num, den = matlab.zpk2tf(z, p, k)
Ds = matlab.tf(num, den)
print(Ds)
# Draw the open-loop frequency resp. for the comp. sys
DsGs = Ds*Gs
gm, pm, wg, wp = control.margin(DsGs)
print(f"Gain margin = {gm} dB")
print(f"Phase margin = {round(pm, 2)} degrees")
print(f"Frequency for Gain Margin = {wg} radians/sec")
print(f"Frequecny for Phase Margin = {wp} radians/sec")
omega_comp = np.logspace(-2,2,2000)
mag_comp, phase_comp, omega_comp = control.bode(DsGs, omega=omega_comp)
mag_comp = 20 * np.log10(mag_comp)
phase_comp = np.degrees(phase_comp)
omega_comp = omega_comp.T
phase_comp = phase_comp.T
mag_comp = mag_comp.T
omega_comp = list(omega_comp)
phase_comp = list(phase_comp)
mag_comp = list(mag_comp)
return omega_comp, mag_comp, phase_comp, gm, pm, wp, wg
def PI_analyze(self, system, controller):
self.S = system
self.C = controller
self.C.sysOpenCE(self.S.CE)
print("Open Loop: %s" % self.C.sysOCE)
self.C.sysClosedCE()
print("Closed Loop: %s" % self.C.sysCCE)
self.C.sysRecomp(P=10, I=30)
OC1 = self.C.sysOpenCE(self.S.CE)
self.C.sysRecomp(P=50, I=150)
OC2 = self.C.sysOpenCE(self.S.CE)
self.C.sysRecomp(P=200, I=600)
OC3 = self.C.sysOpenCE(self.S.CE)
control.bode([OC1, OC2, OC3])
def plot(self):
G = control.TransferFunction((1, 1.5), (1, 11, 10, 0))
mag, phase, omega = control.bode(G)
plt.title('Bode Plots', y= 2.20)
plt.gcf().canvas.set_window_title('Bode Plots')
plt.grid()
plt.show()
def pid(Kp, Ki, Kd):
# PID Controller
s = matlab.tf('s')
Ds = Kp + Ki/s + Kd*s
# Draw the open-loop frequency resp. for the comp. sys
Gs = control.tf(36, [1, 3.6, 0])
DsGs = Ds*Gs
gm, pm, wg, wp = control.margin(DsGs)
print(f"Gain margin = {gm} dB")
print(f"Phase margin = {round(pm, 2)} degrees")
print(f"Frequency for Gain Margin = {wg} radians/sec")
print(f"Frequecny for Phase Margin = {wp} radians/sec")
omega_comp = np.logspace(-2,2,2000)
mag_comp, phase_comp, omega_comp = control.bode(DsGs, omega=omega_comp)
mag_comp = 20 * np.log10(mag_comp)
phase_comp = np.degrees(phase_comp)
omega_comp = omega_comp.T
phase_comp = phase_comp.T
mag_comp = mag_comp.T
omega_comp = list(omega_comp)
phase_comp = list(phase_comp)
mag_comp = list(mag_comp)
return omega_comp, mag_comp, phase_comp, gm, pm, wp, wg
def bode_pid(sys, Kp, Ki, Kd):
# open-loop system transfer function
try:
num, den = model(sys)
except:
# for error detection
print("Err: system in not defined")
return
Gs = control.tf(num, den)
# PID Controller
s = matlab.tf('s')
Ds = Kp + Ki/s + Kd*s
# Compensated open-loop transfer function
DsGs = Ds*Gs
# bode plot arrays
omega = np.logspace(-2, 2, 2000)
mag, phase, omega = control.bode(DsGs, omega=omega)
mag = 20 * np.log10(mag) # mag in db
phase = np.degrees(phase) # phase in degrees
# convert numpy arrays to lists
omega = list(omega)
phase = list(phase)
mag = list(mag)
# round lists to 6 decimal floating digits
ndigits = 6
omega = [round(num, ndigits) for num in omega]
phase = [round(num, ndigits) for num in phase]
mag = [round(num, ndigits) for num in mag]
return omega, mag, phase
def Gs():
# Define plant transfer function
Gs = control.tf(36, [1, 3.6, 0])
[n, d] = control.tfdata(Gs)
b = str(control.tf(d,1 ))
print(b[5])
num = "36"
den = "s^2 + 3.6s"
# Draw open-loop frequency response for the planet
gm, pm, wg, wp = control.margin(Gs)
# print(f"Gain margin = {gm} dB")
# print(f"Phase margin = {round(pm, 2)} degrees")
# print(f"Frequency for Gain Margin = {wg} radians/sec")
# print(f"Frequecny for Phase Margin = {wp} radians/sec")
omega = np.logspace(-2,2,2000)
mag, phase, omega = control.bode(Gs, omega=omega)
mag = 20 * np.log10(mag)
phase= np.degrees(phase)
omega = omega.T
phase = phase.T
mag = mag.T
omega= list(omega)
phase= list(phase)
mag= list(mag)
#mag = 20*np.log(mag,10)
return num, den, omega, mag, phase,gm, pm, wg, wp
def bode_sys(sys):
# open-loop system transfer function
try:
num, den = model(sys)
except:
# for error detection
print("Err: system in not defined")
return
Gs = control.tf(num, den)
# bode plot arrays
omega = np.logspace(-2, 2, 2000)
mag, phase, omega = control.bode(Gs, omega=omega)
mag = 20 * np.log10(mag) # mag in db
phase = np.degrees(phase) # phase in degrees
# convert numpy arrays to lists
omega = list(omega)
phase = list(phase)
mag = list(mag)
# round lists to 6 decimal floating digits
ndigits = 6
omega = [round(num, ndigits) for num in omega]
phase = [round(num, ndigits) for num in phase]
mag = [round(num, ndigits) for num in mag]
return omega, mag, phase
def bode(name, sys, omega, margins=False):
mag, phase, omega = control.bode(sys, omega, Plot=False)
mag_dB = 20 * np.log10(mag)
if margins:
gm, pm, sm, wg, wp, ws = control.stability_margins(sys)
plt.subplot(211)
if margins:
plt.hlines(0, omega[0], omega[-1], linestyle='--')
plt.vlines([wp, wg], np.min(mag_dB), np.max(mag_dB), linestyle='--')
plt.semilogx(omega, mag_dB)
plt.xlabel('rad')
plt.ylabel('dB')
plt.grid()
if margins:
plt.title(
name +
' bode pm: {:0.2f} deg @{:0.2f} rad/s gm: {:0.2f} @{:0.2f} rad/s'.
format(pm, wp, gm, wg))
else:
plt.title(name + ' bode')
plt.subplot(212)
phase_deg = np.rad2deg(phase)
plt.semilogx(omega, phase_deg)
if margins:
plt.vlines([wp, wg],
np.min(phase_deg),
np.max(phase_deg),
linestyle='--')
plt.hlines(-180, omega[0], omega[-1], linestyle='--')
plt.grid()
def bode(self):
"""
Display bode plot of the system.
uses the bode module of control.
Plots **magnitude in dB** and **phase in degrees** with respect to the
**Frequency in rad/s**.
"""
mag, phase, omega = control.bode(self.sys, dB=True)
plt.clf()
plt.figure(1)
plt.subplot(2, 1, 1)
plt.title("Magnitude Response")
plt.grid()
plt.plot(omega, mag, color='g')
plt.ylabel('Magnitude in dB')
plt.xlabel('Frequency in rad/s')
plt.subplot(2, 1, 2)
plt.title("Phase Response")
plt.plot(omega, phase, color='r')
plt.ylabel('Phase in degrees')
plt.xlabel('Frequency in rad/s')
plt.grid()
plt.tight_layout()
plt.show()
def LoadModelTriggered(self, q):
options = QFileDialog.Options()
filename, _ = QFileDialog.getOpenFileName(
self,
"Select linearized model file:",
"U:/Loads/i115_gl2012_2B12_IC/Linearization/LinMod/",
"Matlab Files (*.mat)",
options=options)
azimuth_index = 0
wtg = WTG(filename, azimuth_index)
kvect = np.linspace(0, 2e10, 1000)
start = time.time()
rlist, klist = ctrl.root_locus(wtg.T_G_1, kvect, Plot=False)
omega_vect = np.logspace(-2, 2, 1000)
mag, phase, omega = ctrl.bode(wtg.T_G_1, omega_vect, Plot=False)
magdb = 20 * np.log10(mag)
phasedeg = phase * 180 / np.pi
omegahz = omega / 2 / np.pi
self.frame_plot_1_2.plot_bode(phase, magdb, omegahz)
self.frame_plot_2_1.plot_bode(phase, magdb, omegahz)
self.frame_plot_2_2.plot_bode(phase, magdb, omegahz)
for ii in range(0, rlist.shape[1]):
locs = rlist[:, ii]
x = locs.real
y = locs.imag
self.frame_plot_1_1.plot_rlocus.plot(
x, y, pen=pg.mkPen('k', width=1, style=QtCore.Qt.SolidLine))
plt.show()
end = time.time()
print(end - start)
print('done')
def stability_analysis(self):
""" Perform a stability analysis of the loop with
settings defined in the constructor """
# create the phase detector transfer function
K_pd = control.tf([1], [1])
# create the low pass filter transfer function
iir_taps = self.iir.read_coeffs()
iir_b = iir_taps[0:2]
iir_a = iir_taps[3:5]
K_lpf = control.tf(iir_b, iir_a)
# create the PI filter transfer function
K_pi = control.tf([self.proportional_gain, self.integral_gain], [1, 0])
# create the VCO transfer function
K_vco = control.tf([1], [1, 0])
# create the final transfer function
K_ol = K_pd * K_lpf * K_pi * K_vco
# find the bode plot
mag, phase, omega = control.bode(K_ol)
# plot the bode plot
plt.plot(omega / (2 * np.pi), mag)
plt.plot(omega / (2 * np.pi), phase)
plt.show()
# find the gain and phase margins
gm, pm, wg, wp = control.margin(K_ol)
print("Gain margin = ", gm, "Phase margin = ", pm)
print(K_ol)
def plot_margins(sys):
mag, phase, omega = ctl.bode(sys, dB=True, Plot=False)
magdB = 20 * np.log10(mag)
phase_deg = phase * 180.0 / np.pi
Gm, Pm, Wcg, Wcp = ctl.margin(sys)
GmdB = 20 * np.log10(Gm)
##Plot Gain and Phase
f, (ax1, ax2) = plt.subplots(2, 1)
ax1.semilogx(omega, magdB)
ax1.grid(which="both")
ax1.set_xlabel('Frequency (rad/s)')
ax1.set_ylabel('Magnitude (dB)')
ax2.semilogx(omega, phase_deg)
ax2.grid(which="both")
ax2.set_xlabel('Frequency (rad/s)')
ax2.set_ylabel('Phase (deg)')
ax1.set_title('Gm = ' + str(np.round(GmdB, 2)) + ' dB (at ' +
str(np.round(Wcg, 2)) + ' rad/s), Pm = ' +
str(np.round(Pm, 2)) + ' deg (at ' + str(np.round(Wcp, 2)) +
' rad/s)')
###Plot the zero dB line
ax1.plot(omega, 0 * omega, 'k--', lineWidth=2)
###Plot the -180 deg lin
ax2.plot(omega, -180 + 0 * omega, 'k--', lineWidth=2)
##Plot the vertical line from -180 to 0 at Wcg
ax2.plot([Wcg, Wcg], [-180, 0], 'r--', lineWidth=2)
##Plot the vertical line from -180+Pm to 0 at Wcp
ax2.plot([Wcp, Wcp], [-180, -180 + Pm], 'g--', lineWidth=2)
##Plot the vertical line from min(magdB) to 0-GmdB at Wcg
ax1.plot([Wcg, Wcg], [-GmdB, 0], 'r--', lineWidth=2)
##Plot the vertical line from min(magdB) to 0db at Wcp
ax1.plot([Wcp, Wcp], [np.min(magdB), 0], 'g--', lineWidth=2)
return Gm, Pm, Wcg, Wcp
def bode_open(self,
omega: list = None) -> Tuple[np.float, np.float, np.float]:
mag, phase, omega = ct.bode(self.open_system(),
omega=omega,
dB=True,
deg=True,
Plot=False)
return mag, phase, omega
def P_analyze(self, system, controller):
self.S = system
self.C = controller
self.C.sysOpenCE(self.S.CE)
print("Open Loop: %s" % self.C.sysOCE)
self.C.sysClosedCE()
print("Closed Loop: %s" % self.C.sysCCE)
# plot open loop Bode
self.C.sysRecomp(P=100)
OC1 = self.C.sysOpenCE(self.S.CE)
self.C.sysRecomp(P=500)
OC2 = self.C.sysOpenCE(self.S.CE)
self.C.sysRecomp(P=2000)
OC3 = self.C.sysOpenCE(self.S.CE)
control.bode([OC1, OC2, OC3])
def bode_close(self, omega: list = None) -> Tuple[list, list, list]:
mag, phase, omega_returned = ct.bode(self.full_system(),
omega=omega,
dB=True,
Plot=False) # dB=True, deg=True,
phase = phase * 180. / np.pi
mag = 20. * np.log10(mag)
return mag, phase, omega_returned
def mybode(*args, **kwargs):
freq = kwargs.pop('freq', None)
kwargs['omega'] = freq * 2.0 * np.pi
mag, phase, omega = bode(*args, **kwargs)
phase = degwrap(phase)
mag = FrequencySeries(mag, frequencies=freq)
phase = FrequencySeries(phase, frequencies=freq)
return mag, phase
def test_discrete_bode(self, tsys):
# Create a simple discrete time system and check the calculation
sys = TransferFunction([1], [1, 0.5], 1)
omega = [1, 2, 3]
mag_out, phase_out, omega_out = bode(sys, omega)
H_z = list(map(lambda w: 1./(np.exp(1.j * w) + 0.5), omega))
np.testing.assert_array_almost_equal(omega, omega_out)
np.testing.assert_array_almost_equal(mag_out, np.absolute(H_z))
np.testing.assert_array_almost_equal(phase_out, np.angle(H_z))
def test_initial_phase(TF, initial_phase, default_phase, expected_phase):
# Check initial phase of standard transfer functions
mag, phase, omega = ctrl.bode(TF)
assert (abs(phase[0] - default_phase) < 0.1)
# Now reset the initial phase to +180 and see if things work
mag, phase, omega = ctrl.bode(TF, initial_phase=initial_phase)
assert (abs(phase[0] - expected_phase) < 0.1)
# Make sure everything works in rad/sec as well
if initial_phase:
plt.xscale('linear') # avoids xlim warning on next line
plt.clf() # clear previous figure (speeds things up)
mag, phase, omega = ctrl.bode(TF,
initial_phase=initial_phase / 180. *
math.pi,
deg=False)
assert (abs(phase[0] - expected_phase) < 0.1)
def add_spec_input_disturbance(gamma_d, omega_d, system, dB_flag=False):
fig = plt.gcf()
w = np.logspace(np.log10(omega_d)-2, np.log10(omega_d))
mag, phase, omega = bode(system, dB=dB_flag, omega=w, Plot=False)
if dB_flag == False:
fig.axes[0].loglog(w, 1. / gamma_d * np.ones(len(w))*mag,
'--', color=[1, 0, 0], label='$d_{in}$ spec')
else:
fig.axes[0].semilogx(w, 20 * np.log10(1 / gamma_r) * ones(size(w)),
'--', color=[1, 0, 0], label='$d_{in}$ spec')
def urCheck(sys,wctar,pmtar,omega):
thres=0.5
R.urOffset = 1
gm, pm, wg, wp = ctrl.margin(sys)
mag, phaserad, w = ctrl.bode(sys, omega)
phasedeg = phaserad * 180 / pi
if pm < pmtar:
for i in range(len(omega)):
if phasedeg[i] < pmtar-180+thres and phasedeg[i] > pmtar-180-thres and mag[i]>1:
R.urOffset=mag[i]**-1
break
return sys * R.urOffset
def speed():
vmax = 1
kl = 3.1
lmax = (vmax / kl)**2
lrange = np.arange(-lmax, lmax, 0.0001)[np.newaxis].T
vrange = (np.sign(lrange) * kl * (np.abs(lrange))**0.5)
k1 = np.linalg.pinv(lrange) @ vrange
print(k1)
#plt.plot(lrange,vrange)
#plt.plot(lrange,k1*lrange)
#plt.show()
#plt.clf()
Hs = ct.tf([kl], [1])
Hz = ct.matlab.c2d(Hs, 1, method='zoh')
Fs = 1
Tp = 100
Tz = 1000
p1 = np.exp(-1 / Tp / Fs)
z1 = np.exp(-1 / Tz / Fs)
p = [-.5, p1] #pole locations
z = [-.5, z1] #zero loations
gain = 0.002 #gain
freq = 0.1 #at frequency
Fs = 1 #sample rate
#fn.zplane(p,z)
#plt.show()
k = gain / np.abs((1 - z[0] * np.exp(-freq / Fs * 1j)) *
(1 - z[1] * np.exp(-freq / Fs * 1j)) /
((1 - p[0] * np.exp(-freq / Fs * 1j)) *
(1 - p[1] * np.exp(-freq / Fs * 1j))))
b = [k, -k * (z[0] + z[1]), k * z[0] * z[1]]
a = [1, -(p[0] + p[1]), p[0] * p[1]]
#print(a,b)
Kz = ct.tf(b, a, 1 / Fs)
ct.bode(Kz * Hz, np.logspace(-5, 0))
plt.show()
def updateClosedLoop(self):
if not self.is_system_identified:
return
# Simulate closed-loop system with generated PID
num = self.num
den = self.den
dt = self.dt
kc = self.kc
ki = self.ki
kd = self.kd
Gz2 = ctrl.TransferFunction(num, den, dt)
(pid_num, pid_den) = gainsToNumDen(kc, ki, kd, dt)
PID = ctrl.TransferFunction(pid_num, pid_den, dt)
Gcl = ctrl.feedback(PID * Gz2, 1)
t_out, y_out = ctrl.step_response(Gcl, T=np.arange(0, 1, dt))
self.plotClosedLoop(t_out, y_out)
w = np.logspace(-1, 3, 40).tolist()
mag, phase, omega = ctrl.bode(Gz2, omega=np.asarray(w), plot=False)
mag_cl, phase_cl, omega_cl = ctrl.bode(Gcl,
omega=np.asarray(w),
plot=False)
self.plotBode(omega, mag, omega_cl, mag_cl)
def frequency_requirements(g, gain_margin=None, phase_margin=None):
gains = []
gm, pm, _, _ = ct.margin(g)
if gain_margin is not None:
gains.append(10**(-(gain_margin - gm) / 20))
if phase_margin is not None:
mag, phase, omega = ct.bode(g, Plot=False)
arg = np.argmin(abs(phase - (phase_margin - np.pi)))
m = 20 * np.log10(mag[arg])
gains.append(10**(-m / 20))
return np.prod(gains)
def lag_compensator(g,
err_step=None,
err_ramp=None,
err_para=None,
pm_desired=None,
psi=None):
s = ct.TransferFunction([1, 0], [1])
if psi is not None:
pm_desired = 100 * psi * np.pi / 180
if pm_desired > 2 * np.pi:
print("remember, I need phase in radians")
kc, ess = ss_error(g, err_step, err_ramp, err_para)
if abs(ess) == np.inf or abs(ess) == np.nan:
ess = None
if ess is None or kc is None:
assert "Inconsistent system"
kc = kc / np.real(ess)
print("kc=", kc)
phi = (pm_desired + 7 * np.pi / 180) - np.pi
print("phi=", phi)
mag, phase, omega = ct.bode(kc * g, Plot=False)
arg = np.argmin(abs(phase - phi))
wcg = omega[arg]
print(f"wcg= {wcg}")
gain = 20 * np.log10(mag[arg])
a = 0 - gain
print(f"A= {a}")
alpha = 10**(a / 20)
tau = 10 / alpha / wcg
lag = kc * (alpha * tau * s + 1) / (tau * s + 1)
return lag
def lead_compensator(g,
err_step=None,
err_ramp=None,
err_para=None,
pm_desired=None,
psi=None):
s = ct.TransferFunction([1, 0], [1])
# all radians policy
if psi is not None:
pm_desired = 100 * psi * np.pi / 180
if pm_desired > 2 * np.pi:
print("remember, I need phase in radians")
kc, ess = ss_error(g, err_step, err_ramp, err_para)
if abs(ess) == np.inf or abs(ess) == np.nan:
ess = None
if ess is None or kc is None:
assert "Inconsistent system"
kc = kc / np.real(ess)
print(f"kc= {kc}")
_, pm, _, wpm = ct.margin(kc * g)
print(pm, wpm)
k_angle = (pm_desired - pm * np.pi / 180 + 5 * np.pi / 180) # 5deg extra
alpha = (1 + np.sin(k_angle)) / (1 - np.sin(k_angle))
print(f"alpha= {alpha}")
a = 10 * np.log10(alpha)
print(f"A= {a}")
mag, phase, omega = ct.bode(kc * g, Plot=False)
arg = np.argmin(abs(20 * np.log10(mag) - -a))
wcg = omega[arg]
tau = 1 / np.sqrt(alpha) / wcg
print(f"tau= {tau}")
lead = kc * (alpha * tau * s + 1) / (tau * s + 1)
return lead
def bode(tf: Union[control.TransferFunction, control.StateSpace],
deg: bool = True,
dB: bool = True,
Hz: bool = True,
Plot: bool = True):
"""Computer bode plot, to fix issues with bode from python control"""
mag, phase, w = control.bode(tf, deg=deg, dB=dB, Hz=Hz, Plot=False)
if Plot:
plt.subplot(211)
plt.semilogx(w, mag)
plt.grid(which='both')
plt.ylabel('magnitude (dB)')
plt.subplot(212)
plt.semilogx(w, phase)
plt.xlabel('Hz')
plt.ylabel('phase (deg)')
plt.grid(which='both')
return mag, phase, w
def testConvert(self):
"""Test state space to transfer function conversion."""
verbose = self.debug
from control.statesp import _mimo2siso
#print __doc__
# Machine precision for floats.
eps = np.finfo(float).eps
for states in range(1, self.maxStates):
for inputs in range(1, self.maxIO):
for outputs in range(1, self.maxIO):
# start with a random SS system and transform to TF then
# back to SS, check that the matrices are the same.
ssOriginal = matlab.rss(states, outputs, inputs)
if (verbose):
self.printSys(ssOriginal, 1)
# Make sure the system is not degenerate
Cmat = control.ctrb(ssOriginal.A, ssOriginal.B)
if (np.linalg.matrix_rank(Cmat) != states):
if (verbose):
print(" skipping (not reachable)")
continue
Omat = control.obsv(ssOriginal.A, ssOriginal.C)
if (np.linalg.matrix_rank(Omat) != states):
if (verbose):
print(" skipping (not observable)")
continue
tfOriginal = matlab.tf(ssOriginal)
if (verbose):
self.printSys(tfOriginal, 2)
ssTransformed = matlab.ss(tfOriginal)
if (verbose):
self.printSys(ssTransformed, 3)
tfTransformed = matlab.tf(ssTransformed)
if (verbose):
self.printSys(tfTransformed, 4)
# Check to see if the state space systems have same dim
if (ssOriginal.states != ssTransformed.states):
print("WARNING: state space dimension mismatch: " + \
"%d versus %d" % \
(ssOriginal.states, ssTransformed.states))
# Now make sure the frequency responses match
# Since bode() only handles SISO, go through each I/O pair
# For phase, take sine and cosine to avoid +/- 360 offset
for inputNum in range(inputs):
for outputNum in range(outputs):
if (verbose):
print("Checking input %d, output %d" \
% (inputNum, outputNum))
ssorig_mag, ssorig_phase, ssorig_omega = \
control.bode(_mimo2siso(ssOriginal, \
inputNum, outputNum), \
deg=False, Plot=False)
ssorig_real = ssorig_mag * np.cos(ssorig_phase)
ssorig_imag = ssorig_mag * np.sin(ssorig_phase)
#
# Make sure TF has same frequency response
#
num = tfOriginal.num[outputNum][inputNum]
den = tfOriginal.den[outputNum][inputNum]
tforig = control.tf(num, den)
tforig_mag, tforig_phase, tforig_omega = \
control.bode(tforig, ssorig_omega, \
deg=False, Plot=False)
tforig_real = tforig_mag * np.cos(tforig_phase)
tforig_imag = tforig_mag * np.sin(tforig_phase)
np.testing.assert_array_almost_equal( \
ssorig_real, tforig_real)
np.testing.assert_array_almost_equal( \
ssorig_imag, tforig_imag)
#
# Make sure xform'd SS has same frequency response
#
ssxfrm_mag, ssxfrm_phase, ssxfrm_omega = \
control.bode(_mimo2siso(ssTransformed, \
inputNum, outputNum), \
ssorig_omega, \
deg=False, Plot=False)
ssxfrm_real = ssxfrm_mag * np.cos(ssxfrm_phase)
ssxfrm_imag = ssxfrm_mag * np.sin(ssxfrm_phase)
np.testing.assert_array_almost_equal( \
ssorig_real, ssxfrm_real)
np.testing.assert_array_almost_equal( \
ssorig_imag, ssxfrm_imag)
#
# Make sure xform'd TF has same frequency response
#
num = tfTransformed.num[outputNum][inputNum]
den = tfTransformed.den[outputNum][inputNum]
tfxfrm = control.tf(num, den)
tfxfrm_mag, tfxfrm_phase, tfxfrm_omega = \
control.bode(tfxfrm, ssorig_omega, \
deg=False, Plot=False)
tfxfrm_real = tfxfrm_mag * np.cos(tfxfrm_phase)
tfxfrm_imag = tfxfrm_mag * np.sin(tfxfrm_phase)
np.testing.assert_array_almost_equal( \
ssorig_real, tfxfrm_real)
np.testing.assert_array_almost_equal( \
ssorig_imag, tfxfrm_imag)
import control
from matplotlib import pyplot as plt
from utils import feedback
w = np.logspace(-1, 2, 1000)
s = control.tf([1, 0], 1)
G = 4 /((s - 1)*(0.02*s + 1)**2)
Kc = 1.25
tau1 = 1.5
K = Kc*(1+1/(tau1*s))
L = K*G
S = feedback(1, L)
T = feedback(L, 1)
mag, phase, omega = control.bode(L, w)
magS, phaseS, omega = control.bode(S, w)
magT, phaseT, omega = control.bode(T, w)
plt.legend(["L", "S", "T"],
bbox_to_anchor=(0, 1.01, 1, 0), loc=3, ncol=3)
Ms = max(magS)
Mt = max(magT)
gm, pm, wg, wp = control.margin(mag, phase, omega)
Lu_180 = 1/np.abs(control.evalfr(L, wg))
P = np.angle(control.evalfr(L, wp)) + np.pi
print "Lower GM:", Lu_180
print "PM:", np.round(P*180/np.pi, 1), "deg or", np.round(P, 2), "rad"
from matplotlib.pyplot import * from scipy import * import control import bode_utils G = control.TransferFunction(100.0,[1,0,0])#100.0/s**2 f = logspace(-2,2,1000) w = 2*pi*f db, phase, f2 = control.bode(G, omega=w, dB=True, Hz=True, Plot=False) bode_utils.bode_plot(f, db, phase, clear=True, fignum=1, label='$G$') #Choose freq and design a lead comp. f_c = 10.0 w_c = 2*pi*f_c factor = 5.0 z = w_c/factor p = w_c*factor G_lead = control.TransferFunction([1,z],[1,p])*p/z gain = 6.0 db2, phase2, f2 = control.bode(G*G_lead*gain, omega=w, dB=True, Hz=True, Plot=False) bode_utils.bode_plot(f, db2, phase2, clear=False, fignum=1, label='$G_c \\cdot G$') subplot(211) legend(loc=3)
#!/usr/bin/env python
from matplotlib.pyplot import plot, legend, savefig, gcf
from numpy import logspace
from control import bode, tf
T=1
G = tf([0, 1], [T, 1])
om = logspace(-2, 2, 100)
bode(G, dB=True, omega=om)
f=gcf()
r, mag, fas = f.get_children()
p1, = mag.plot([0, 0.1, 1, 10, 100], [0, 0, 0, -20, -40], '--')
fas.plot([0, 0.1, 1, 10, 100], [0, 0, -45, -90, -90], '--')
mag.set_yticks([0, -3, -10, -20, -40])
fas.set_yticks([0, -45, -90])
mag.legend([p1], ['asintotas'])
mag.set_xticklabels([])
fas.set_xticklabels(['',r'$\frac{1}{100 T}$', r'$\frac{1}{10 T}$', r'$\frac{1}{T}$', r'$\frac{10}{T}$', r'$\frac{100}{T}$'])
#a.set_yticklabels([])
# Se guarda la figura en la misma carpeta
savefig("bodeprimerorden.pdf", bbox_inches='tight', pad_inches=0, transparent="True")
def plot_loops(name, G_ol, G_cl):
"""
Plot loops
:param name: Name of axis
:param G_ol: open loop transfer function
:param G_cl: closed loop transfer function
"""
plt.figure()
plt.plot(*control.step_response(G_cl, np.linspace(0, 1, 1000)))
plt.title(name + ' step resposne')
plt.grid()
plt.figure()
control.bode(G_ol)
print('margins', control.margin(G_ol))
plt.subplot(211)
plt.title(name + ' open loop bode plot')
plt.figure()
control.rlocus(G_ol, np.logspace(-2, 0, 1000))
for pole in G_cl.pole():
plt.plot(np.real(pole), np.imag(pole), 'rs')
plt.title(name + ' root locus')
plt.grid()
