def test_identification_frequency_domain_discrete():
m = 12
n = 12
system0 = signal.TransferFunction(num, den)
freq = linspace(0, 200, 801)
freq[0] = 1e-3
_freq, mag, phase = signal.bode((system0.num, system0.den), freq * 2 * pi)
mag0 = 10**(mag / 20)
phase0 = phase / 180 * pi
system1 = identification_frequency_domain_discrete(freq,
mag0 * exp(1j * phase0),
1 / FS,
m,
n,
weight='h')
_freq, mag, phase = signal.dbode(system1, freq / FS * 2 * pi)
mag1 = 10**(mag / 20)
phase1 = phase / 180 * pi
figure()
semilogy(freq, mag0, lw=5)
semilogy(freq, mag1)
grid()
figure()
plot(freq, phase0, lw=5)
plot(freq, phase1)
grid()
return system1
def show_bode():
# Show the bode plot for the targeted system
import numpy as np
FREQ_RANGE = np.linspace(0, 100, 501)
w, mag_c, phase_c = signal.bode(SYS_C, FREQ_RANGE * 2 * np.pi)
w, mag_d, phase_d = signal.dbode(SYS_D, FREQ_RANGE / (1 / T) * 2 * np.pi)
fig = figure(figsize=(12, 9))
ax = fig.add_subplot(211)
ax.semilogx(FREQ_RANGE, mag_c, lw=5, label='continuous')
ax.semilogx(FREQ_RANGE, mag_d, label='discrete')
ax.grid(which='both')
ax.legend()
ax.set_ylabel('Magnitude (dB)')
ax = fig.add_subplot(212, sharex=ax)
ax.semilogx(FREQ_RANGE, phase_c, lw=5, label='continuous')
ax.semilogx(FREQ_RANGE, phase_d, label='discrete')
ax.grid(which='both')
ax.legend()
ax.set_ylabel('Phase (deg)')
ax.set_xlabel('Frequency (Hz)\nNormalized Frequency')
ax.set_xticklabels([
'{freq}\n{Omega}'.format(freq=i, Omega=2 * i * T)
for i in ax.get_xticks()
])
show()
def bode(self, wmin=1e-3, tikz=False):
"""Plot a bode graph
return tikz code if tikz is True, otherwise display it"""
# get the frequency, magnitude and phase
w, mag, phase = dbode(self.toTransferFunction(),
linspace(wmin, 3.14159, 500))
import matplotlib.pyplot as plt
plt.figure(3)
plt.subplot(2, 1, 1)
plt.semilogx(w, mag, 'b-', linewidth=1)
plt.xlim((wmin, 3.5))
plt.grid(b=True, which='major', color='gray', linestyle='-')
plt.grid(b=True, which='minor', color='silver', linestyle='--')
plt.ylabel('Magnitude (dB)')
plt.axvline(x=3.14159, color='grey', zorder=2)
plt.subplot(2, 1, 2)
plt.semilogx(w, phase, 'b-', linewidth=1)
plt.xlim((wmin, 3.5))
plt.grid(b=True, which='major', color='gray', linestyle='-')
plt.grid(b=True, which='minor', color='silver', linestyle='--')
plt.axvline(x=3.14159, color='grey', zorder=2)
plt.ylabel('Phase')
plt.xlabel('Frequency ($rad.s^{-1}$)')
if tikz:
import matplotlib2tikz
return matplotlib2tikz.get_tikz_code(figurewidth='15cm',
figureheight='7cm')
else:
plt.show()
def wiener(a, random_state):
s = random_state.choice([1., -1.], size=(1000, ))
channel = signal.TransferFunction([1, a], [1, 0], dt=1)
t, x = signal.dlsim(channel, s)
R = linalg.toeplitz(np.r_[[1 + a**2, a], np.zeros(7)])
p = np.r_[[1], np.zeros(8)]
w_wiener = np.linalg.inv(R) @ p
with open(f'prova_01/tex/03/{sanitize_path(f"Wiener_{a}")}.tex',
mode='w') as tex_file:
tex_file.write(sympy.latex(sympy.Matrix(w_wiener).n(3)))
equalizer = signal.TransferFunction(w_wiener[:2], [1, 0], dt=1)
t, s_est = signal.dlsim(equalizer, x, t=t)
HW = signal.TransferFunction(np.convolve(channel.num, equalizer.num),
np.convolve(channel.den, equalizer.den),
dt=1)
w, mag, phase = signal.dbode(HW)
fig, axes = plt.subplots(2, 1, sharex=True)
axes[0].set_ylabel('Magnitude')
axes[0].semilogx(w, mag)
axes[1].set_ylabel('Fase')
axes[1].set_xlabel('$\omega$')
axes[1].semilogx(w, phase)
plt.tight_layout()
fig.savefig(f'prova_01/img/03/{sanitize_path(f"Wiener_Bode_{a}")}.png')
def plot(firTable, ylim):
fig, ax = plt.subplots(3, 1)
fig.set_size_inches(6, 8)
fig.set_tight_layout(True)
ax[0].set_title("Amplitude Response")
ax[0].set_ylabel("Amplitude [dB]")
ax[1].set_title("Phase Response")
ax[1].set_ylabel("Phase [degree]")
ax[2].set_title("Group Delay")
ax[2].set_ylabel("Group Delay [sample]")
ax[2].set_xlabel("Frequency [rad/sample]")
ax[2].set_ylim(ylim)
cmap = plt.get_cmap("plasma")
for index, fir in enumerate(firTable):
freq, mag, phase = signal.dbode((fir, 1, 1), n=1024)
freq, gd = signal.group_delay((fir, 1), w=1024)
cmapValue = index / len(firTable)
ax[0].plot(freq, mag, alpha=0.75, color=cmap(cmapValue), label=f"{index}")
ax[1].plot(freq, phase, alpha=0.75, color=cmap(cmapValue), label=f"{index}")
ax[2].plot(freq, gd, alpha=0.75, color=cmap(cmapValue), label=f"{index}")
for axis in ax:
axis.grid()
axis.legend()
plt.show()
def task3c():
sys1_num = [1, 2, 1]
sys1_denom = [1]
sys2_num = [1, 0]
sys2_denom = [1, 0.9]
sys1_gain = 1
sys1_zeros = [-1, -1]
sys1_poles = []
sys1 = sig.dlti(sys1_num, sys1_denom)
sys2 = sig.dlti(sys2_num, sys2_denom)
w1, mag1, phase1 = sig.dbode(sys1, n=100000)
w2, mag2, phase2 = sig.dbode(sys2, n=100000)
plt.semilogx(w1, mag1)
plt.title("magnitude response of first system")
plt.savefig("task3c mag1.pdf")
plt.show()
plt.semilogx(w1, phase1)
plt.title("phase response of first system")
plt.savefig("task3c phase1.pdf")
plt.show()
plt.semilogx(w2, mag2)
plt.title("magnitude response of second system")
plt.savefig("task3c mag2.pdf")
plt.show()
plt.semilogx(w2, phase2)
plt.title("phase response of second system")
plt.savefig("task3c phase2.pdf")
plt.show()
sys1 = sig.ZerosPolesGain(sys1_zeros, )
n = np.arange(1000)
x = 1 / 2 * np.sin(np.pi * n + np.pi / 4)
out1 = sig.dlsim(sys1, x)
out2 = sig.dlsim(sys2, x)
plt.plot(n, out1)
plt.title("output of system 1")
plt.savefig("task3e output1.pdf")
plt.show()
plt.plot(n, out2)
plt.title("output of system 2")
plt.savefig("task3e output2.pdf")
plt.show()
def test_pole_one(self):
# Test that freqresp() doesn't fail on a system with a pole at 0.
# integrator, pole at zero: H(s) = 1 / s
system = TransferFunction([1], [1, -1], dt=0.1)
with warnings.catch_warnings():
warnings.simplefilter("ignore", RuntimeWarning)
w, mag, phase = dbode(system, n=2)
assert_equal(w[0], 0.) # a fail would give not-a-number
def test_pole_one(self):
# Test that freqresp() doesn't fail on a system with a pole at 0.
# integrator, pole at zero: H(s) = 1 / s
system = TransferFunction([1], [1, -1], dt=0.1)
with warnings.catch_warnings():
warnings.simplefilter("ignore", RuntimeWarning)
w, mag, phase = dbode(system, n=2)
assert_equal(w[0], 0.) # a fail would give not-a-number
def bandpass(fs):
r1 = 47e3
r2 = 1e6
c1 = .33e-6
c2 = 22e-9
num = [c1 * r2, 0]
den = [c1 * c2 * r1 * r2, c1 * r2 + c2 * r1 - c1 * r1, 1]
h_s = sg.TransferFunction(num, den)
w_s = np.linspace(.1 * 2 * math.pi, 300 * 2 * math.pi, 5000)
w, mag, phase = sg.bode(h_s, w=w_s)
plt.figure(2)
plt.title("Magnitude")
plt.semilogx(w_s / (2 * math.pi), mag)
plt.axvline(x=.5, color='orange', linestyle='--', label='fc_1(.5 Hz)')
plt.axvline(x=153, color='orange', linestyle='--', label='fc_2(153 Hz)')
plt.legend(loc='upper right')
plt.xlabel("Freq. [Hz]")
plt.ylabel("Mag. [dB]")
plt.figure(3)
plt.title("Phase")
plt.semilogx(w_s / (2 * math.pi), phase)
plt.axvline(x=.5, color='orange', linestyle='--', label='fc_1(.5 Hz)')
plt.axvline(x=153, color='orange', linestyle='--', label='fc_2(153 Hz)')
plt.legend(loc='upper right')
plt.xlabel("Freq. [Hz]")
plt.ylabel("[deg]")
#Discrete conversion
w_d = np.linspace(.1 * 2 * math.pi, 300 * 2 * math.pi, 5000)
num_d = [.6307, -.6307]
den_d = [1, -1.395, .3967]
h_s_d = sg.TransferFunction(num_d, den_d, dt=1 / fs)
w_d, mag_d, phase_d = sg.dbode(h_s_d, w=w_d / fs)
#z,p,k = sg.tf2zpk(num,den)
#w_d,mag_d,phase_d = sg.dbode((z,p,k,1/Fs),w=w_d/Fs)
plt.figure(4)
plt.title("Magnitude discrete")
plt.semilogx(w_d / (2 * math.pi), mag_d)
plt.axvline(x=.5, color='orange', linestyle='--', label='fc_1(.5 Hz)')
plt.axvline(x=153, color='orange', linestyle='--', label='fc_2(153 Hz)')
plt.legend(loc='upper right')
plt.xlabel("Freq. [Hz]")
plt.ylabel("Mag. [dB]")
plt.figure(5)
plt.title("Phase discrete")
plt.semilogx(w_d / (2 * math.pi), phase_d)
plt.axvline(x=.5, color='orange', linestyle='--', label='fc_1(.5 Hz)')
plt.axvline(x=153, color='orange', linestyle='--', label='fc_2(153 Hz)')
plt.legend(loc='upper right')
plt.xlabel("Freq. [Hz]")
plt.ylabel("[deg]")
plt.show()
return h_s_d, num_d, den_d
def test_range(self):
# Test that bode() finds a reasonable frequency range.
# 1st order low-pass filter: H(s) = 0.3 / (z - 0.2),
dt = 0.1
system = TransferFunction(0.3, [1, -0.2], dt=0.1)
n = 10
# Expected range is from 0.01 to 10.
expected_w = np.linspace(0, np.pi, n, endpoint=False) / dt
w, mag, phase = dbode(system, n=n)
assert_almost_equal(w, expected_w)
def test_pole_one(self):
# Test that freqresp() doesn't fail on a system with a pole at 0.
# integrator, pole at zero: H(s) = 1 / s
system = TransferFunction([1], [1, -1], dt=0.1)
with suppress_warnings() as sup:
sup.filter(RuntimeWarning, message="divide by zero")
sup.filter(RuntimeWarning, message="invalid value encountered")
w, mag, phase = dbode(system, n=2)
assert_equal(w[0], 0.) # a fail would give not-a-number
def test_pole_one(self):
# Test that freqresp() doesn't fail on a system with a pole at 0.
# integrator, pole at zero: H(s) = 1 / s
system = TransferFunction([1], [1, -1], dt=0.1)
with suppress_warnings() as sup:
sup.filter(RuntimeWarning, message="divide by zero")
sup.filter(RuntimeWarning, message="invalid value encountered")
w, mag, phase = dbode(system, n=2)
assert_equal(w[0], 0.) # a fail would give not-a-number
def test_range(self):
# Test that bode() finds a reasonable frequency range.
# 1st order low-pass filter: H(s) = 0.3 / (z - 0.2),
dt = 0.1
system = TransferFunction(0.3, [1, -0.2], dt=0.1)
n = 10
# Expected range is from 0.01 to 10.
expected_w = np.linspace(0, np.pi, n, endpoint=False) / dt
w, mag, phase = dbode(system, n=n)
assert_almost_equal(w, expected_w)
def makeCallibration(sos):
coef = 1
for filter in sos:
#print "filter " + str(filter)
b = [filter[0], filter[1], filter[2]]
a = [filter[3], filter[4], filter[5]]
sys = signal.TransferFunction(b, a, dt=0.000001)
w, mag, phase = signal.dbode(sys)
maximum = np.amax(mag)
#print "max " + str(maximum)
if (maximum > 5):
divide = abs(maximum) / 5
coef = coef * divide
filter[0] = filter[0] / divide
filter[1] = filter[1] / divide
filter[2] = filter[2] / divide
#print "/" + str(divide)
else:
if (abs(maximum) > 5):
divide = abs(maximum) / 5
coef = coef * divide
filter[0] = filter[0] / divide
filter[1] = filter[1] / divide
filter[2] = filter[2] / divide
#print "*" + str(divide)
#print "coef" + str(coef)
b = [sos[0][0], sos[0][1], sos[0][2]]
a = [sos[0][3], sos[0][4], sos[0][5]]
#print b
#print a
sys = signal.TransferFunction(b, a, dt=0.000001)
w, mag, phase = signal.dbode(sys)
#print np.amax(mag)
#print "*" + str(coef)
sos[0][0] = sos[0][0] * coef
sos[0][1] = sos[0][1] * coef
sos[0][2] = sos[0][2] * coef
def bode(num, den, fs):
w, mag, phase = sig_p.dbode((num, den, 1 / fs), n=1000)
plt.figure(5)
plt.subplot(121)
plt.title('Mag')
plt.plot((w / (2 * np.pi)), mag)
plt.xlabel('Freq [Hz]')
plt.ylabel('Amplitude [dB]')
plt.subplot(122)
plt.title('Phase')
plt.plot((w / (2 * np.pi)), phase)
plt.xlabel('Freq [Hz]')
plt.ylabel('Phase [deg]')
def filt_resp(b, a, fs):
plt.figure(2)
w, mag, phase = sg.dbode((b, a, 1 / fs), n=1000)
plt.subplot(2, 1, 1)
plt.title("WMA Mag. and Phase Response")
plt.plot(w / (2 * np.pi), 10**(mag / 20))
plt.ylabel('Magnitude')
plt.xlabel('Frequency [Hz]')
plt.xlim(0, np.pi)
plt.subplot(2, 1, 2)
plt.plot(w / (2 * np.pi), np.unwrap(phase))
plt.ylabel('Angle (degrees)')
plt.xlabel('Frequency [Hz]')
plt.xlim(0, np.pi)
def bode(num,den,fs):
#w,mag,phase = sig_p.dbode((b,a,1/fs),n=1000)
w,mag,phase = sig_p.dbode((num,den,1/fs), n=1000)
plt.figure(5)
plt.title('Mag')
plt.plot((w/(2*math.pi)),mag)
plt.xlabel('Freq [Hz]')
plt.ylabel('Amplitude [dB]')
plt.figure(6)
plt.title('Phase')
plt.plot((w/(2*math.pi)),phase)
plt.xlabel('Freq [Hz]')
plt.ylabel('Phase [deg]')
def test_auto(self):
# Test bode() magnitude calculation.
# 1st order low-pass filter: H(s) = 0.3 / (z - 0.2),
system = TransferFunction(0.3, [1, -0.2], dt=0.1)
w = np.array([0.1, 0.5, 1, np.pi])
w2, mag, phase = dbode(system, w=w)
jw = np.exp(w * 1j)
y = np.polyval(system.num, jw) / np.polyval(system.den, jw)
# Test mag
expected_mag = 20.0 * np.log10(abs(y))
assert_almost_equal(mag, expected_mag)
# Test phase
expected_phase = np.rad2deg(np.angle(y))
assert_almost_equal(phase, expected_phase)
def test_auto(self):
# Test bode() magnitude calculation.
# 1st order low-pass filter: H(s) = 0.3 / (z - 0.2),
system = TransferFunction(0.3, [1, -0.2], dt=0.1)
w = np.array([0.1, 0.5, 1, np.pi])
w2, mag, phase = dbode(system, w=w)
jw = np.exp(w * 1j)
y = np.polyval(system.num, jw) / np.polyval(system.den, jw)
# Test mag
expected_mag = 20.0 * np.log10(abs(y))
assert_almost_equal(mag, expected_mag)
# Test phase
expected_phase = np.rad2deg(np.angle(y))
assert_almost_equal(phase, expected_phase)
def test_manual(self):
# Test bode() magnitude calculation (manual sanity check).
# 1st order low-pass filter: H(s) = 0.3 / (z - 0.2),
dt = 0.1
system = TransferFunction(0.3, [1, -0.2], dt=dt)
w = [0.1, 0.5, 1, np.pi]
w2, mag, phase = dbode(system, w=w)
# Test mag
expected_mag = [-8.5329, -8.8396, -9.6162, -12.0412]
assert_almost_equal(mag, expected_mag, decimal=4)
# Test phase
expected_phase = [-7.1575, -35.2814, -67.9809, -180.0000]
assert_almost_equal(phase, expected_phase, decimal=4)
# Test frequency
assert_equal(np.array(w) / dt, w2)
def test_manual(self):
# Test bode() magnitude calculation (manual sanity check).
# 1st order low-pass filter: H(s) = 0.3 / (z - 0.2),
dt = 0.1
system = TransferFunction(0.3, [1, -0.2], dt=dt)
w = [0.1, 0.5, 1, np.pi]
w2, mag, phase = dbode(system, w=w)
# Test mag
expected_mag = [-8.5329, -8.8396, -9.6162, -12.0412]
assert_almost_equal(mag, expected_mag, decimal=4)
# Test phase
expected_phase = [-7.1575, -35.2814, -67.9809, -180.0000]
assert_almost_equal(phase, expected_phase, decimal=4)
# Test frequency
assert_equal(np.array(w) / dt, w2)
def filt_resp(b, a, fs):
plt.figure(2)
w, mag, phase = sg.dbode((b, a, 1 / fs), n=1000)
#w, h = sg.freqz(b,a,fs=fs)
plt.subplot(2, 1, 1)
plt.plot(w / (2 * np.pi), 10**(mag / 20))
plt.ylabel('Magnitude [dB]')
plt.xlabel('Frequency [Hz]')
plt.xlim(0, np.pi)
plt.subplot(2, 1, 2)
#angles = np.unwrap(np.angle(h))
plt.plot(w / (2 * np.pi), phase)
plt.ylabel('Angle (radians)')
plt.xlabel('Frequency [Hz]')
plt.xlim(0, np.pi)
#impulse_resp(b,a,fs)
zplane(b, a)
def __init__(self, fsig, fcut, order=20, Fs=44100):
"""
@parameters:
- fsig : frequencies of the signal used for compensation
- fcut : cutoff frequency for the lowpass filter
- order : order of the lowpass filter
- fs : sampling frequency of the system
"""
self.fcut = fcut
self.order = order
self.Fs = Fs
self.sos = signal.butter(order,
self.fcut,
'low',
fs=self.Fs,
output='sos')
b, a = signal.butter(order, self.fcut, 'low', fs=self.Fs)
if fsig[0] == 0:
fsig = fsig[1:]
w, mag, phase = signal.dbode((b, a, 1 / Fs),
w=fsig / Fs) # compute mag
self.K = np.mean(1 / (10**(mag / 20)))
w, gd = signal.group_delay((b, a), w=fsig / Fs, fs=Fs) # compute delay
self.lag = int(np.mean(gd))
def test_imaginary(self):
# bode() should not fail on a system with pure imaginary poles.
# The test passes if bode doesn't raise an exception.
system = TransferFunction([1], [1, 0, 100], dt=0.1)
dbode(system, n=2)
def filt_resp_iter(b,a,fs):
plt.figure(2)
w, mag, phase = sg.dbode((b,a,1/fs), n=1000)
wc = np.where((10**(mag/20)) >= .707)
print("Mag cutoff:", 10**(mag[wc[0][-1]]/20))
return (w[wc[0][-1]]/(2*np.pi))
from scipy import signal import matplotlib.pyplot as plt # Transfer function: H(z) = 1 / (z^2 + 2z + 3) sys = signal.TransferFunction([1], [1, 2, 3], dt=0.05) # Equivalent: sys.bode() w, mag, phase = signal.dbode(sys) plt.figure() plt.semilogx(w, mag) # Bode magnitude plot plt.figure() plt.semilogx(w, phase) # Bode phase plot plt.show()
k1 = kp_dig + ki_dig + kd_dig
k2 = -kp_dig - 2 * kd_dig
k3 = kd_dig
#este es el pid formado
b_pid = [k1, k2, k3]
a_pid = [1, -1]
td = 1 / Fsampling
pid_dig_tf = TransferFunction(b_pid, a_pid, dt=td)
####################
# Bode PID Digital #
####################
w, mag, phase = dbode(pid_dig_tf, n=10000)
fig, (ax1, ax2) = plt.subplots(2, 1)
ax1.semilogx(w / (2 * np.pi), mag, 'y')
ax1.set_title('Digital PID')
ax1.set_ylabel('Amplitude [dB]', color='g')
ax1.set_xlabel('Frequency [Hz]')
# ax1.set_ylim(ymin=-40, ymax=40)
ax2.semilogx(w / (2 * np.pi), phase, 'y')
ax2.set_ylabel('Phase', color='g')
ax2.set_xlabel('Frequency [Hz]')
plt.tight_layout()
plt.show()
#############################
td = 1 / Fsampling
controller_tf = TransferFunction(cont_zpk_b, cont_zpk_a, dt=td)
print("Digital Controller:")
print(controller_tf)
#####################################
# Polos y Ceros del Control Digital #
#####################################
plot_argand(controller_tf)
########################
# Bode Control Digital #
########################
w, mag, phase = dbode(controller_tf, n=10000)
fig, (ax1, ax2) = plt.subplots(2, 1)
ax1.semilogx(w / (2 * np.pi), mag, 'c')
ax1.set_title('Digital Controller TF')
ax1.set_ylabel('Amplitude [dB]', color='c')
ax1.set_xlabel('Frequency [Hz]')
ax2.semilogx(w / (2 * np.pi), phase, 'c')
ax2.set_ylabel('Phase', color='c')
ax2.set_xlabel('Frequency [Hz]')
plt.tight_layout()
plt.show()
#--- end of file ---#
#normalizo con TransferFunction
print("Planta en Digital")
planta_dig_tustin = TransferFunction(planta_dig_tustin_n,
planta_dig_tustin_d,
dt=td)
print(planta_dig_tustin)
planta_dig_euler = TransferFunction(planta_dig_euler_n,
planta_dig_euler_d,
dt=td)
print(planta_dig_euler)
#dbode devuelve w = pi / dt, 100 puntos
f_eval = np.arange(0, 0.5, 0.0001) #de 0 a 1 en saltos de 0.01 de fsampling
w_tustin, mag_tustin, phase_tustin = dbode(planta_dig_tustin, n=1000)
w_euler, mag_euler, phase_euler = dbode(planta_dig_euler, n=1000)
fig, (ax1, ax2) = plt.subplots(2, 1)
ax1.semilogx(w_tustin / (np.pi), mag_tustin, 'b')
ax1.semilogx(w_euler / (np.pi), mag_euler, 'r')
ax1.set_title('Planta digital Tustin blue; Euler red')
ax1.set_ylabel('Amplitude [dB]', color='b')
ax1.set_xlabel('Frequency [Hz]')
ax1.set_ylim([-20, 20])
ax2.semilogx(w_tustin / (np.pi), phase_tustin, 'b')
ax2.semilogx(w_euler / (np.pi), phase_euler, 'r')
ax2.set_ylabel('Phase', color='r')
ax2.set_xlabel('Frequency [Hz]')
def test_imaginary(self):
# bode() should not fail on a system with pure imaginary poles.
# The test passes if bode doesn't raise an exception.
system = TransferFunction([1], [1, 0, 100], dt=0.1)
dbode(system, n=2)
Tsampling = 1 / Fsampling
cont_n, cont_d, td = cont2discrete((controller.num, controller.den),
Tsampling,
method='euler')
#normalizo con TransferFunction
print(cont_n)
print(cont_d)
controller_d = TransferFunction(cont_n, cont_d, dt=td)
print(controller_d)
#dbode devuelve w = pi / dt, 100 puntos
f_eval = np.arange(0, 0.5, 0.0001) #de 0 a 1 en saltos de 0.01 de fsampling
# w, mag, phase = dbode(controller_d, w=f_eval*np.pi)
w, mag, phase = dbode(controller_d, n=1000)
# w, mag, phase = dbode(controller_d)
# print (w)
fig, (ax1, ax2) = plt.subplots(2, 1)
ax1.semilogx(w / (np.pi), mag, 'b')
ax1.set_title('PID Euler')
ax1.set_ylabel('Amplitude P D2 [dB]', color='b')
ax1.set_xlabel('Frequency [Hz]')
ax1.set_ylim([0, 65])
ax2.semilogx(w / (np.pi), phase, 'r')
ax2.set_ylabel('Phase', color='r')
ax2.set_xlabel('Frequency [Hz]')
## este es el pid digital
b_pid = [k1, k2, k3]
a_pid = [1, -1]
print ("")
print ("kp_dig: " + str(kp_dig) + " ki_dig: " + str(ki_dig) + " kd_dig: " + str(kd_dig))
print ("")
#este es custom controller
# b_pid = [1.03925, -0.96075, 0]
# a_pid = [1, -1]
pid_dig = TransferFunction(b_pid, a_pid, dt=td)
print ("PID Digital:")
print (pid_dig)
if show_digital_pid_bode:
w, mag, phase = dbode(pid_dig, n = 10000)
fig, (ax1, ax2) = plt.subplots(2,1)
ax1.semilogx(w/(2*np.pi), mag, 'y')
ax1.set_title('PID Digital')
ax1.set_ylabel('Amplitude P D2 [dB]')
ax1.set_xlabel('Frequency [Hz]')
ax2.semilogx(w/(2*np.pi), phase, 'y')
ax2.set_ylabel('Phase')
ax2.set_xlabel('Frequency [Hz]')
plt.tight_layout()
plt.show()
##################################################
# Convierto Planta por ZOH a una frecuencia alta #
# para que no afecte polos o ceros #
##################################################
Fsampling = 20
Tsampling = 1 / Fsampling
planta_dig_zoh_n, planta_dig_zoh_d, td = cont2discrete(
(planta_TF.num, planta_TF.den), Tsampling, method='zoh')
#normalizo con TransferFunction
print("Planta Digital Zoh:")
planta_dig_zoh = TransferFunction(planta_dig_zoh_n, planta_dig_zoh_d, dt=td)
print(planta_dig_zoh)
w, mag_zoh, phase_zoh = dbode(planta_dig_zoh, n=10000)
if Bode_Planta_Digital == True:
fig, (ax1, ax2) = plt.subplots(2, 1)
ax1.semilogx(w / (2 * np.pi), mag_zoh, 'y')
ax1.set_title('Digital ZOH')
ax1.set_ylabel('Amplitude P D2 [dB]', color='g')
ax1.set_xlabel('Frequency [Hz]')
ax2.semilogx(w / (2 * np.pi), phase_zoh, 'y')
ax2.set_ylabel('Phase', color='g')
ax2.set_xlabel('Frequency [Hz]')
plt.tight_layout()
plt.show()
def plot_dbode_ML(num_samples,
den_samples,
num_true,
den_true,
num_ML,
den_ML,
num_ML2,
den_ML2,
Ts,
omega,
no_plot=300,
max_samples=1000,
save=False):
"""plot bode diagram from estimated discrete time system samples and true sys"""
no_samples = np.shape(num_samples)[0]
no_eval = min(no_samples, max_samples)
sel = np.random.choice(np.arange(no_samples), no_eval, False)
omega_res = max(np.shape(omega))
mag_samples = np.zeros((omega_res, no_eval))
phase_samples = np.zeros((omega_res, no_eval))
count = 0
for s in sel:
den_sample = np.concatenate(([1.0], den_samples[s, :]), 0)
num_sample = num_samples[s, :]
w, mag_samples[:, count], phase_samples[:, count] = signal.dbode(
(num_sample, den_sample, Ts), omega)
count = count + 1
# calculate the true bode diagram
# plot the true bode diagram
w, mag_true, phase_true = signal.dbode(
(num_true.flatten(), den_true.flatten(), Ts), omega)
w, mag_ML, phase_ML = signal.dbode(
(num_ML.flatten(), den_ML.flatten(), Ts), omega)
w, mag_ML2, phase_ML2 = signal.dbode(
(num_ML2.flatten(), den_ML2.flatten(), Ts), omega)
# # convert back from decibal
# mag_true = np.power(10,mag_true/10)
# mag_ML = np.power(10, mag_ML / 10)
# mag_ML2 = np.power(10, mag_ML2 / 10)
# mag_samples = np.power(10, mag_samples / 10)
# plot the samples
plt.subplot(2, 1, 1)
h2, = plt.semilogx(w.flatten(),
mag_samples[:, 0],
color='green',
alpha=0.1,
label='hmc samples') # Bode magnitude plot
plt.semilogx(w.flatten(),
mag_samples[:, 1:no_plot],
color='green',
alpha=0.1) # Bode magnitude plot
h1, = plt.semilogx(w.flatten(),
mag_true,
color='blue',
label='True system') # Bode magnitude plot
# hml, = plt.semilogx(w.flatten(), mag_ML,'--', color='purple', label='ML estimate') # Bode magnitude plot
hml2, = plt.semilogx(w.flatten(),
mag_ML2,
'--',
color='red',
label='ML reg estimate') # Bode magnitude plot
hm, = plt.semilogx(w.flatten(),
np.mean(mag_samples, 1),
'-.',
color='orange',
label='hmc mean') # Bode magnitude plot
# hu, = plt.semilogx(w.flatten(), np.percentile(mag_samples, 97.5, axis=1),'--',color='orange',label='Upper CI') # Bode magnitude plot
plt.legend(handles=[h1, h2, hm])
plt.legend()
plt.title('Bode diagram')
plt.ylabel('Magnitude (dB)')
plt.ylim((-45, 10))
# plt.xlim((min(w.flatten()),min(max(omega),1/Ts*3.14)))
plt.xlim((1e-1, min(max(omega), 1 / Ts * 3.14)))
plt.subplot(2, 1, 2)
plt.semilogx(w.flatten(),
phase_samples[:, :no_plot],
color='green',
alpha=0.1) # Bode phase plot
plt.semilogx(w.flatten(), phase_true, color='blue') # Bode phase plot
# hml, = plt.semilogx(w.flatten(), phase_ML, '--', color='purple') # Bode magnitude plot
hml, = plt.semilogx(w.flatten(), phase_ML2, '--',
color='red') # Bode magnitude plot
plt.semilogx(w.flatten(),
np.mean(phase_samples, 1),
'-.',
color='orange',
label='mean') # Bode magnitude plot
plt.ylabel('Phase (deg)')
plt.xlabel('Frequency (rad/s)')
# plt.xlim((min(w.flatten()), min(max(omega),1/Ts*3.14)))
plt.xlim((1e-1, min(max(omega), 1 / Ts * 3.14)))
plt.ylim((-500, 50))
if save:
plt.savefig('figures/bode_plot_oe.png', format='png')
plt.show()
#normalizo con TransferFunction
planta_dig_tustin = TransferFunction(planta_dig_tustin_n,
planta_dig_tustin_d,
dt=td)
print(planta_dig_tustin)
planta_dig_euler = TransferFunction(planta_dig_euler_n,
planta_dig_euler_d,
dt=td)
print(planta_dig_euler)
#dbode devuelve w = pi / dt, 100 puntos
f_eval = np.arange(0, 0.5, 0.0001) #de 0 a 1 en saltos de 0.01 de fsampling
w_tustin, mag_tustin, phase_tustin = dbode(planta_dig_tustin, n=1000)
w_euler, mag_euler, phase_euler = dbode(planta_dig_euler, n=1000)
fig, (ax1, ax2) = plt.subplots(2, 1)
ax1.semilogx(w_tustin / (np.pi), mag_tustin, 'b')
ax1.semilogx(w_euler / (np.pi), mag_euler, 'r')
ax1.set_title('Planta digital Tustin blue; Euler red')
ax1.set_ylabel('Amplitude P D2 [dB]', color='b')
ax1.set_xlabel('Frequency [Hz]')
ax1.set_ylim([-20, 20])
ax2.semilogx(w_tustin / (np.pi), phase_tustin, 'b')
ax2.semilogx(w_euler / (np.pi), phase_euler, 'r')
ax2.set_ylabel('Phase', color='r')
ax2.set_xlabel('Frequency [Hz]')
def plot_firfreq(b_samples,
num_true,
den_true,
b_ML,
Ts=1.0,
no_plot=300,
max_samples=1000,
save=False):
"""plot bode diagram from estimated discrete time system samples and true sys"""
no_samples = np.shape(b_samples)[0]
no_eval = min(no_samples, max_samples)
sel = np.random.choice(np.arange(no_samples), no_eval, False)
# omega_res = max(np.shape(omega))
omega_res = 512
mag_samples = np.zeros((omega_res, no_eval))
phase_samples = np.zeros((omega_res, no_eval))
count = 0
for s in sel:
b_sample = b_samples[s, :]
w_hmc, h = signal.freqz(b_sample)
mag_samples[:, count] = 20 * np.log10(abs(h))
phase_samples[:, count] = np.unwrap(np.angle(h)) * 180.0 / 3.14
count = count + 1
# calculate the true bode diagram
# plot the true bode diagram
w, mag_true, phase_true = signal.dbode(
(num_true.flatten(), den_true.flatten(), Ts))
w_ML, h = signal.freqz(b_ML)
mag_ML = 20 * np.log10(abs(h))
phase_ML = np.unwrap(np.angle(h)) * 180.0 / 3.14
# plot the samples
plt.subplot(2, 1, 1)
h2, = plt.semilogx(w_hmc.flatten(),
mag_samples[:, 0],
color='green',
alpha=0.1,
label='hmc samples') # Bode magnitude plot
plt.semilogx(w_hmc.flatten(),
mag_samples[:, 1:no_plot],
color='green',
alpha=0.1) # Bode magnitude plot
h1, = plt.semilogx(w.flatten(),
mag_true,
color='blue',
label='True system') # Bode magnitude plot
hml, = plt.semilogx(w_ML.flatten(),
mag_ML,
'--',
color='purple',
label='ML Estimate') # Bode magnitude plot
hm, = plt.semilogx(w_hmc.flatten(),
np.mean(mag_samples, 1),
'-.',
color='orange',
label='hmc mean') # Bode magnitude plot
# hu, = plt.semilogx(w.flatten(), np.percentile(mag_samples, 97.5, axis=1),'--',color='orange',label='Upper CI') # Bode magnitude plot
plt.legend(handles=[h1, h2, hm, hml])
plt.legend()
plt.title('Bode diagram')
plt.ylabel('Magnitude (dB)')
# plt.xlim((min(omega),min(max(omega),1/Ts*3.14)))
plt.subplot(2, 1, 2)
plt.semilogx(w_hmc.flatten(),
phase_samples[:, :no_plot],
color='green',
alpha=0.1) # Bode phase plot
plt.semilogx(w.flatten(), phase_true, color='blue') # Bode phase plot
plt.semilogx(w_ML.flatten(), phase_ML, '--',
color='purple') # Bode magnitude plot
plt.semilogx(w_hmc.flatten(),
np.mean(phase_samples, 1),
'-.',
color='orange',
label='mean') # Bode magnitude plot
plt.ylabel('Phase (deg)')
plt.xlabel('Frequency (rad/sample)')
# plt.xlim((min(omega), min(max(omega),1/Ts*3.14)))
if save:
plt.savefig('fir_bode_plot.png', format='png')
plt.show()
