def test_dstep(self):
a = np.asarray([[0.9, 0.1], [-0.2, 0.9]])
b = np.asarray([[0.4, 0.1, -0.1], [0.0, 0.05, 0.0]])
c = np.asarray([[0.1, 0.3]])
d = np.asarray([[0.0, -0.1, 0.0]])
dt = 0.5
# Because b.shape[1] == 3, dstep should result in a tuple of three
# result vectors
yout_step_truth = (np.asarray([
0.0, 0.04, 0.052, 0.0404, 0.00956, -0.036324, -0.093318,
-0.15782348, -0.226628324, -0.2969374948
]),
np.asarray([
-0.1, -0.075, -0.058, -0.04815, -0.04453,
-0.0461895, -0.0521812, -0.061588875,
-0.073549579, -0.08727047595
]),
np.asarray([
0.0, -0.01, -0.013, -0.0101, -0.00239, 0.009081,
0.0233295, 0.03945587, 0.056657081, 0.0742343737
]))
tout, yout = dstep((a, b, c, d, dt), n=10)
assert_equal(len(yout), 3)
for i in range(0, len(yout)):
assert_equal(yout[i].shape[0], 10)
assert_array_almost_equal(yout[i].flatten(), yout_step_truth[i])
# Check that the other two inputs (tf, zpk) will work as well
tfin = ([1.0], [1.0, 1.0], 0.5)
yout_tfstep = np.asarray([0.0, 1.0, 0.0])
tout, yout = dstep(tfin, n=3)
assert_equal(len(yout), 1)
assert_array_almost_equal(yout[0].flatten(), yout_tfstep)
zpkin = tf2zpk(tfin[0], tfin[1]) + (0.5, )
tout, yout = dstep(zpkin, n=3)
assert_equal(len(yout), 1)
assert_array_almost_equal(yout[0].flatten(), yout_tfstep)
# Raise an error for continuous-time systems
system = lti([1], [1, 1])
assert_raises(AttributeError, dstep, system)
def test_more_step_and_impulse(self):
lambda1 = 0.5
lambda2 = 0.75
a = np.array([[lambda1, 0.0],
[0.0, lambda2]])
b = np.array([[1.0, 0.0],
[0.0, 1.0]])
c = np.array([[1.0, 1.0]])
d = np.array([[0.0, 0.0]])
n = 10
# Check a step response.
ts, ys = dstep((a, b, c, d, 1), n=n)
# Create the exact step response.
stp0 = (1.0 / (1 - lambda1)) * (1.0 - lambda1 ** np.arange(n))
stp1 = (1.0 / (1 - lambda2)) * (1.0 - lambda2 ** np.arange(n))
assert_allclose(ys[0][:, 0], stp0)
assert_allclose(ys[1][:, 0], stp1)
# Check an impulse response with an initial condition.
x0 = np.array([1.0, 1.0])
ti, yi = dimpulse((a, b, c, d, 1), n=n, x0=x0)
# Create the exact impulse response.
imp = (np.array([lambda1, lambda2]) **
np.arange(-1, n + 1).reshape(-1, 1))
imp[0, :] = 0.0
# Analytical solution to impulse response
y0 = imp[:n, 0] + np.dot(imp[1:n + 1, :], x0)
y1 = imp[:n, 1] + np.dot(imp[1:n + 1, :], x0)
assert_allclose(yi[0][:, 0], y0)
assert_allclose(yi[1][:, 0], y1)
# Check that dt=0.1, n=3 gives 3 time values.
system = ([1.0], [1.0, -0.5], 0.1)
t, (y,) = dstep(system, n=3)
assert_allclose(t, [0, 0.1, 0.2])
assert_array_equal(y.T, [[0, 1.0, 1.5]])
t, (y,) = dimpulse(system, n=3)
assert_allclose(t, [0, 0.1, 0.2])
assert_array_equal(y.T, [[0, 1, 0.5]])
def test_more_step_and_impulse(self):
lambda1 = 0.5
lambda2 = 0.75
a = np.array([[lambda1, 0.0],
[0.0, lambda2]])
b = np.array([[1.0, 0.0],
[0.0, 1.0]])
c = np.array([[1.0, 1.0]])
d = np.array([[0.0, 0.0]])
n = 10
# Check a step response.
ts, ys = dstep((a, b, c, d, 1), n=n)
# Create the exact step response.
stp0 = (1.0 / (1 - lambda1)) * (1.0 - lambda1 ** np.arange(n))
stp1 = (1.0 / (1 - lambda2)) * (1.0 - lambda2 ** np.arange(n))
assert_allclose(ys[0][:, 0], stp0)
assert_allclose(ys[1][:, 0], stp1)
# Check an impulse response with an initial condition.
x0 = np.array([1.0, 1.0])
ti, yi = dimpulse((a, b, c, d, 1), n=n, x0=x0)
# Create the exact impulse response.
imp = (np.array([lambda1, lambda2]) **
np.arange(-1, n + 1).reshape(-1, 1))
imp[0, :] = 0.0
# Analytical solution to impulse response
y0 = imp[:n, 0] + np.dot(imp[1:n + 1, :], x0)
y1 = imp[:n, 1] + np.dot(imp[1:n + 1, :], x0)
assert_allclose(yi[0][:, 0], y0)
assert_allclose(yi[1][:, 0], y1)
# Check that dt=0.1, n=3 gives 3 time values.
system = ([1.0], [1.0, -0.5], 0.1)
t, (y,) = dstep(system, n=3)
assert_allclose(t, [0, 0.1, 0.2])
assert_array_equal(y.T, [[0, 1.0, 1.5]])
t, (y,) = dimpulse(system, n=3)
assert_allclose(t, [0, 0.1, 0.2])
assert_array_equal(y.T, [[0, 1, 0.5]])
def rise_time(self): """Return 90 percent rise time of step response in terms of samples""" s = sig.dstep((self.b, self.a, 1))[1][0] n = 0 for ii in range(len(s)): if s[ii] > 0.9: n = ii break return n
def test_dstep(self):
a = np.asarray([[0.9, 0.1], [-0.2, 0.9]])
b = np.asarray([[0.4, 0.1, -0.1], [0.0, 0.05, 0.0]])
c = np.asarray([[0.1, 0.3]])
d = np.asarray([[0.0, -0.1, 0.0]])
dt = 0.5
# Because b.shape[1] == 3, dstep should result in a tuple of three
# result vectors
yout_step_truth = (np.asarray([0.0, 0.04, 0.052, 0.0404, 0.00956,
-0.036324, -0.093318, -0.15782348,
-0.226628324, -0.2969374948]),
np.asarray([-0.1, -0.075, -0.058, -0.04815,
-0.04453, -0.0461895, -0.0521812,
-0.061588875, -0.073549579,
-0.08727047595]),
np.asarray([0.0, -0.01, -0.013, -0.0101, -0.00239,
0.009081, 0.0233295, 0.03945587,
0.056657081, 0.0742343737]))
tout, yout = dstep((a, b, c, d, dt), n=10)
assert_equal(len(yout), 3)
for i in range(0, len(yout)):
assert_equal(yout[i].shape[0], 10)
assert_array_almost_equal(yout[i].flatten(), yout_step_truth[i])
# Check that the other two inputs (tf, zpk) will work as well
tfin = ([1.0], [1.0, 1.0], 0.5)
yout_tfstep = np.asarray([0.0, 1.0, 0.0])
tout, yout = dstep(tfin, n=3)
assert_equal(len(yout), 1)
assert_array_almost_equal(yout[0].flatten(), yout_tfstep)
zpkin = tf2zpk(tfin[0], tfin[1]) + (0.5,)
tout, yout = dstep(zpkin, n=3)
assert_equal(len(yout), 1)
assert_array_almost_equal(yout[0].flatten(), yout_tfstep)
# Raise an error for continuous-time systems
system = lti([1], [1, 1])
assert_raises(AttributeError, dstep, system)
def MotorControl():
J = .01 # kgm**2
b = .1 # N.m.s
Ke = .01 # V/rad/sec
Kt = .01 # N.m/Amp
R = 1 # Electrical resistance
L = .5 # Henries
K = Kt # or Ke, Ke == Kt here
# State Space Model
A = np.matrix([[-b / J, K / J], [-K / L, -R / L]])
B = np.matrix([[0], [1 / L]])
C = np.matrix([1, 0])
D = np.matrix([0])
dt = .025 # Discrete Sample Time
ss = sig.StateSpace(A, B, C, D)
ssd = sig.cont2discrete((A, B, C, D), dt)
Ad = ssd[0]
Bd = ssd[1]
Cd = ssd[2]
evc, evecc = np.linalg.eig(A)
evd, evecd = np.linalg.eig(Ad)
# Create Bode
#w, mag, phase = sig.bode(ss)
#plot_SISO(w,mag, "Freq Response Mag vs w")
#plot_SISO(w, phase, "Freq Response Phase vs w")
#discrete_time_frequency_repsonse()
# Transfer Function
tf = ss.to_tf()
print(tf)
t, y = sig.step(ss)
td, yd = sig.dstep(ssd)
yd = np.reshape(yd, -1)
#plot_SISO(t,y)
#plot_SISO(td, yd)
#pole_zeros(A, B, C, plot=True)
#pole_zeros(Ad, Bd, Cd, plot=True)
T = 10
N = T / dt
times = np.arange(0, T, dt)
U = np.ones(int(N))
x0 = np.matrix([0, 0]).T
xr = np.matrix([2.5, 0])
kp = 15
kd = 4
ki = 100
Q = Cd.T * Cd
R = np.matrix([.0005])
discrete_time_response(Ad, Bd, Cd, U, x0, dt, plot=True)
discrete_time_frequency_repsonse(Ad, Bd, Cd, 20, dt)
continous_time_frequency_repsonse(A, B, C)
discrete_pid_response((Ad, Bd, Cd), x0, xr, times, kp, ki, kd)
sim_dlqr(Ad, Bd, Cd, Q, R, x0, xr, dt, T)
x = 9
def step_response(self, n=100):
"""
Calculates the step response of the filter (n steps, default = 100)
"""
step = []
for number in range(0, n):
step.append(1)
_times, step_resp = signal.dstep((self.b, self.a, 1), n=n)
step_response = step_resp[0].flatten()
return step, step_response
def addFilter(b, a, sampling_freq, name=""):
w, h = signal.freqz(b, a, fs=sampling_freq)
t, y = signal.dstep((b, a, 1.0 / fs))
w_gd, gd = signal.group_delay((b, a))
frequencies.append(w)
amplitudes.append(h)
times.append(t)
step_responses.append(y)
group_delays.append(gd)
names.append(name)
def get_step_response(self, dt=None, n=None):
'''
Converts the 2nd order section to a transfer function (b, a) and then computes the SR of the discrete time
system.
:param dt: the time delta.
:param n: the no of samples to output, defaults to 1s.
:return: t, y
'''
t, y = signal.dstep(signal.dlti(*signal.sos2tf(np.array(self.get_sos())),
dt=1 / self.fs if dt is None else dt),
n=self.fs if n is None else n)
return t, y
def plot_the_thing(T):
# reset the current figure
plt.figure()
# create two arrays using T as a base for an exponential function
num = np.array([10 - 10 * np.exp(-T)])
den = np.array([1, 10 - 11 * np.exp(-T)])
# turn the two arrays above into a transfer function
dtf = sig.dlti(num, den)
# generate axes for plot
t, y = sig.dstep((dtf), n=1000)
# generate values for lower circle plot
tCirc = np.linspace(0, 2 * np.pi, 100)
xCirc = np.cos(tCirc)
yCirc = np.sin(tCirc)
# obtain the roots of the function to determine the poles
poleLocation = np.roots(den)
# style the indicators for the pole
colorIndicator = "b"
markerIndicator = "o"
# restyle the pole locator if outside the unit circle
if np.abs(poleLocation) > 1:
colorIndicator = "r"
markerIndicator = "x"
# tell user location of pole in the terminal
print("pole location", poleLocation)
# get axes objects to draw the two plots to
f, xarr = plt.subplots(2)
# plot the upper graph showing the output function
xarr[0].set_title("T = %s" % T)
xarr[0].plot(t, y[:][0])
# plot the lower graph showing the pole relative to the unit circle
xarr[1].plot(1, 0, c="g", marker="o")
xarr[1].plot(xCirc, yCirc)
xarr[1].scatter(poleLocation, 0, c=colorIndicator, marker=markerIndicator)
xarr[1].set_xlabel("pole magnitude is %s" % poleLocation)
xarr[1].set_xlim([-2, 2])
# return the figure object to save images of plots
return f
def discrete_siso_step_response(self, n_tc=7):
if self.is_siso() is False:
raise ValueError('This function is for SISO systems.')
if self.is_delta() is False and self.is_shift() is False:
raise ValueError('This function is for discrete time systems.')
if self.is_delta() is True:
sys = self.delta2shift()
else:
sys = self
mat_a, mat_b, mat_c, mat_d = sys.cofs
tc = sys.time_constant()
n = round(n_tc * tc / sys.dt)
t, y = signal.dstep((mat_a, mat_b, mat_c, mat_d, sys.dt), n=n)
return t, np.squeeze(y)
def step_response(sys,sysd,Ts): #Draw the step response for the continuous-time and discrete-time systems
sysnum = asarray(sys.num)[0][0]
sysden = asarray(sys.den)[0][0]
sysdnum = asarray(sysd.num)[0][0]
sysdden = asarray(sysd.den)[0][0]
x = arange(0,100,0.1)
syslti = signal.lti(sysnum,sysden)
sysdlti = signal.lti(sysdnum,sysdden)
t,s = signal.step(syslti,T=x)
t2,s2 = signal.dstep((sysdnum,sysdden,Ts),t=x)
ax = plt.subplot(3,2,4)
plt.plot(t, s,color='blue',label='Continuous-time')
plt.plot(t2, s2[0],color='red',label='Discrete-time')
plt.title('Step response')
plt.xlabel('Time [sec]')
plt.ylabel('Amplitude')
plt.tight_layout(w_pad = 3.0)
plt.show()
def parse(z, p, k):
b, a = zpk2tf(z, p, k)
global plot_cnt
subplot(2, 6, plot_cnt)
title("Sistem " + str(plot_cnt))
plot(real(z), imag(z), 'o')
plot(real(p), imag(p), 'x')
plot(circ_x, circ_y, '--')
axis([-1.5, 1.5, -1.5, 1.5])
axis("equal")
grid()
t, y = dstep((b, a, 1))
subplot(2, 6, plot_cnt + 6)
plot(t, y[0])
grid()
plot_cnt += 1
def plot_time(analog_system, discrete_system, response='step', num_samples=100, sample_rate=48e3, plot_error=False):
n = np.arange(num_samples)
t = n/sample_rate
fig, ax1 = plt.subplots()
if response == 'impulse':
ta, ya = signal.impulse2(analog_system, T=t)
_, yd = signal.dimpulse((*discrete_system,1/sample_rate), t=t)
ax1.set_title('Resposta ao impulso')
elif response == 'step':
ta, ya = signal.step2(analog_system, T=t)
_, yd = signal.dstep((*discrete_system,1/sample_rate), t=t)
ax1.set_title('Resposta ao degrau')
elif response == 'ramp':
ramp_factor = 1
ta = np.arange(int(num_samples*ramp_factor))/(ramp_factor*sample_rate)
print(ta.shape)
ta, ya, xa = signal.lsim2(analog_system, U=ta, T=ta, printmessg=True)
_, yd = signal.dlsim((*discrete_system,1/sample_rate), u=t, t=t, x0=None)
ax1.set_title('Resposta a rampa')
ax1.plot(ta*sample_rate,ya,'b')
ax1.plot(n,np.ravel(yd),'k.')
ax1.set_ylabel('Amplitude', color='b')
ax1.set_xlabel('Amostras')
ax1.grid(True)
ax1.legend(['Analógico', 'Discreto'])
if plot_error:
ax2 = ax1.twinx()
ax2.plot(n, np.abs(ya-np.ravel(yd)), 'r')
ax2.set_ylabel('Erro', color='r')
ax2.axis('tight')
for T in Tx: # Making a for loop. Iterator is T. and we are going to loop through Tx. (5 numbers between 0 and 0.22)
#Notice the indent to designate the forloops code block
num = np.array(
[10 - 10 * np.exp(-T)]
) # Calling the numpy library. Asking for the function Array. We want to make an array.
# So we are building an array which will be 5 numbers because we are using T to loop through Tx
# num stands for numerator
den = np.array([1, 10 - 11 * np.exp(-T)]) # making a tuple
dtf = sig.dlti(
num, den
) # some discrete time linear time invariant system base class. Like a multi dimensional array.
t, y = sig.dstep(
(dtf), n=1000
) # dstep returns a tuple. Returns two things. We store the discription of dtf in a tuple.
# So it is a 1D array, and the step response of the system
tCirc = np.linspace(
0, 2 * np.pi,
100) # making 100 evenly spaced number between 0 to 2*3.14
xCirc = np.cos(
tCirc
) # Using Numpy, asking for the cosign function, giving it the 100 numbers we just made.
yCirc = np.sin(tCirc) # Same thing as xCirc but for the y direction.
poleLocation = np.roots(den) # Finding what makes the denominator zero.
def step(self, dt=1): """Return discrete step response of filter""" return sig.dstep((self.b, self.a, dt))
# at the end of the for loop
i = 0
# Here be a for loop with its weird not c like syntax
for T in Tx:
# set num to be an array using numpy and populate that array
# with the expression that is given in the brackets
num = np.array([10 - 10 * np.exp(-T)])
# den to be an array using numpy and populate that array
# with the expression that is given in the brackets
den = np.array([1, 10 - 11 * np.exp(-T)])
# set dtf to be a line that gets made using the two expression given
# in the lines above
dtf = sig.dlti(num, den)
# this defines the timestep used in the graphs
t, y = sig.dstep((dtf), n=1000)
# define tCirc as a line that creates a circle
tCirc = np.linspace(0, 2 * np.pi, 100)
# create the x components of the circle
xCirc = np.cos(tCirc)
# create the y components of the circle
yCirc = np.sin(tCirc)
# poleLocation is being assigned to the roots of the
# equation that was defined for den
poleLocation = np.roots(den)
# Set colorIndicator to be blue
colorIndicator = "b"
# Set markerIndicator to be the letter o
markerIndicator = "o"
n1 = Cc.shape[1] n_in = Bc.shape[1] ## Make A Augmented Model Ae = np.eye(n1+m1,n1+m1) Ae[0:n1,0:n1] = Ac Ae[n1:n1+m1,0:n1] = np.matmul(Cc,Ac) ## Make B Augmented Model Be = np.zeros(((n1+m1),n_in)) Be[0:n1,:] = Bc Be[n1:n1+m1,:] = np.matmul(Cc,Bc) print(Be) ## Make B Augmented Model Ce = np.zeros((m1,n1+m1)) Ce[:,n1:n1+m1] = np.eye(m1,m1) print(Ce) sys = signal.StateSpace(Ac,Bc,Cc,Dc,dt=1) AugSys = signal.StateSpace(Ae, Be, Ce, dt = 1) # print(sys) # print(AugSys) import matplotlib.pyplot as plt t,result = signal.dstep(sys) # print(result) plt.plot(t,result[0]) plt.show()
# x_0 = np.zeros(A.shape[0])
# 0.01745 is degree in radians. this is psi here
# x_0 = np.array([0., 0., 0., 0., 0., 0., 0., 0., 0.01745])
x_0 = np.array([0., 0., 0., 0., 0., 0., 0.01745, 0.01745, 0.01745])
# x_0 = np.array([0., 0., 0., 0., 0., 0., 0., 0.01745, 0.])
newA = A - np.matmul(B, K)
newB = np.matmul(B, K)
newC = np.identity(newA.shape[0])
newD = np.zeros(newB.shape)
new_sys = signal.dlti(newA, newB, newC, newD, dt=0.1)
# step the new response, from t0 = 0 to tf = 2s
sim = signal.dstep(new_sys, x0=x_0, n=21)
times = np.linspace(0, 2, 21)
# build plots
philist = []
thetalist = []
psilist = []
for i in range(len(sim[1][0])):
philist.append(sim[1][0][i][-3])
thetalist.append(sim[1][0][i][-2])
psilist.append(sim[1][0][i][-1])
# plot the responses for the sim
fig = go.Figure()
fig.add_trace(go.Scatter(x=times, y=philist, mode='lines', name='phi'))
fig.add_trace(go.Scatter(x=times, y=thetalist, mode='lines', name='theta'))
def test_step_invariant(self, sys, sample_time, samples_number):
time = np.arange(samples_number) * sample_time
_, yout_cont = step2(sys, T=time, **self.tolerances)
_, yout_disc = dstep(c2d(sys, sample_time, method='zoh'), n=len(time))
assert_allclose(yout_cont.ravel(), yout_disc[0].ravel())
ax1.set_ylabel('Amplitude P D2 [dB]', color='b')
ax1.set_xlabel('Frequency [Hz]')
ax2.semilogx(w / (np.pi), phase, 'r')
ax2.set_ylabel('Phase', color='r')
ax2.set_xlabel('Frequency [Hz]')
plt.tight_layout()
plt.show()
print("Resultado sistema: ")
print(closed_loop)
### Desde aca hago pruebas temporales
t = np.linspace(0, 0.01, num=2000)
tout, yout = dstep([closed_loop.num, closed_loop.den, td], t=t)
yout1 = np.transpose(yout)
yout0 = yout1[0]
yout = yout0[:tout.size]
fig, ax = plt.subplots()
ax.set_title(
'Respuesta escalon de la función transferencia luego del PID realimentado')
ax.set_ylabel('Corriente')
ax.set_xlabel('Tiempo [s]')
ax.grid()
ax.stem(tout, yout)
plt.tight_layout()
plt.show()
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()
#############################################
# Verifico Respuesta Escalon Planta Digital #
#############################################
tiempo_de_simulacion = 3.0
t = np.linspace(0,
tiempo_de_simulacion,
num=(tiempo_de_simulacion * Fsampling))
tout, yout_zoh = dstep([planta_dig_zoh.num, planta_dig_zoh.den, td], t=t)
yout1 = np.transpose(yout_zoh)
yout0 = yout1[0]
yout_zoh = yout0[:tout.size]
if Escalon_Planta_Digital == True:
fig, ax = plt.subplots()
ax.set_title('Step Planta Digital ZOH Yellow')
ax.set_ylabel('Salida Planta')
ax.set_xlabel('Tiempo [s]')
ax.grid()
ax.plot(tout, yout_zoh, 'y')
plt.tight_layout()
plt.show()
plt.grid()
plt.savefig("bode.svg")
plt.show()
# Plot the impulse response
t, y_sma = dimpulse((b_sma, a_sma, 1 / f_s), n=2 * N)
t, y_ema = dimpulse((b_ema, a_ema, 1 / f_s), n=2 * N)
#plt.suptitle('Impulse Response')
plt.plot(t, y_sma[0], 'o-', color=col1, label='SMA')
plt.plot(t, y_ema[0], 'o-', color=col2, label='EMA')
plt.xlabel('Time \n (seconds)')
plt.ylabel('Output')
plt.xlim(-1 / f_s, 2 * N / f_s)
plt.legend()
plt.grid()
plt.show()
# Plot the step response
t, y_sma = dstep((b_sma, a_sma, 1 / f_s), n=2 * N)
t, y_ema = dstep((b_ema, a_ema, 1 / f_s), n=2 * N)
#plt.suptitle('Step Response')
plt.plot(t, y_sma[0], 'o-', color=col1, label='SMA')
plt.plot(t, y_ema[0], 'o-', color=col2, label='EMA')
plt.xlabel('Time \n (seconds)')
plt.ylabel('Output')
plt.xlim(-1 / f_s, 2 * N / f_s)
plt.legend()
plt.grid()
plt.savefig("step.svg")
plt.show()
def overshoot(self): """Return overshoot as decimal""" s = sig.dstep((self.b, self.a, 1))[1][0] m = max(s) return round(float(m - 1), 3)
# OpenLoop_sim = OpenLoop.simplify()
System_sim = system.simplify(
) # Esta función me permite ver como queda acomodada la función transferencia de H*Gp
print(System_sim)
# print (OpenLoop_sim)
zeroes = [0]
poles = [-0.308035714285714]
#paso H(z) con potencias positivas b0 zn b1 zn-1, etc; a0 zn a1 zn-1
b = np.array([Kt, 0])
a = np.array([1, Kt * 0.022])
zplane(b, a)
#respuesta escalon
t = np.arange(0, 100, 1)
tout, yout = signal.dstep([b, a, 1], t=t)
yout1 = np.transpose(yout)
yout0 = yout1[0]
yout = yout0[:tout.size]
plt.figure(1)
plt.clf()
plt.title('digital Step Response')
# print (yout)
plt.stem(tout, yout)
plt.show()
print "\nxfer f'n's DC gain" print control.matlab.dcgain(T_ideal) #T_ideal_tf = (num, den, T_ideal.dt)#control.tf(T_ideal.num, T_ideal.den, T_ideal.dt) #T_ideal_tf = control.matlab.tf(T_ideal) T = np.linspace(0, 500, 1000) #T, y = control.matlab.step(T_ideal, T = np.linspace(0, 100, 500), input=0, output=0)#, T=None, input=0, output=0) dsys = (num, den, Ts) print "Dsys:" print dsys T, y = dstep(dsys, t=T)#, T=None, input=0, output=0) plt.figure(1) plt.plot(T, y[0]) plt.show() #create implemented model #hz_actual = tf([tau_gain_fp, 0], [1, -tau_gain_fp*tau_fp], Ts) #create PID with no derivative filter #ctrl_actual = pid(kp_fp, ki_fp, kd_fp, 0, Ts) """ T_actual = control.matlab.feedback(ctrl_actual*delay*hz_actual, 1)
def plot_dtlti_analysis(z, p, k, fs=1, Nf=2**10, Nt=2**5):
"""Plot linear, time-invariant, discrete-time system.
Impulse, step, frequency response (level/phase/group delay) and z-plane
of transfer function H(z) given as zeros/poles/gain description
Note that we use fs only for the frequency responses
"""
# still hard coded, TBD:
# figure size
# group_delay discontinuities and automatic ylim, yticks is not
# useful sometimes
plt.figure(figsize=(9, 9), tight_layout=True)
mu = np.arange(Nf)
df = fs / Nf
dW = 2 * np.pi / Nf
f = mu * df # frequency vector [0...fs)
W = mu * dW # digital angular frequency [0...2pi)
sys = signal.dlti(z, p, k, dt=True)
sys_ba = signal.TransferFunction(sys)
b = sys_ba.num # we need coeff b,a for group_delay()
a = sys_ba.den
[W, H] = signal.dlti.freqresp(sys, W)
[W, gd] = signal.group_delay((b, a), w=W) # gd in samples
h = signal.dimpulse(sys, n=Nt)
he = signal.dstep(sys, n=Nt)
# plot frequency response: level
ax1 = plt.subplot(3, 2, 1)
ax1t = ax1.twiny()
ax1.grid(True, color='lavender', which='major')
# plotted below grid :-(
# ax1t.grid(True, color='mistyrose', which='minor')
ax1.plot(W / (2 * np.pi), 20 * np.log10(np.abs(H)), color='C0', lw=2)
ax1.set_xscale('linear')
ax1.set_xlim([0, 1])
ax1.tick_params(axis='x', labelcolor='C0')
ax1.set_xlabel(r'$\frac{\Omega}{2\pi} = \frac{f}{f_s}$', color='C0')
ax1.set_xticks(np.arange(11) / 10)
ax1.set_ylabel('level in dB or 20 lg|H| / dB', color='k')
ax1.set_axisbelow(True)
ax1t.plot(f, 20 * np.log10(np.abs(H)), color='C3', lw=2)
ax1t.set_xscale('log')
ax1t.set_xlim([f[1], f[-1]])
ax1t.tick_params(axis='x', labelcolor='C3')
ax1t.set_xlabel('frequency in Hz or f / Hz for $f_s$ = ' +
'{0:5.2f}'.format(fs) + ' Hz',
color='C3')
ax1t.set_axisbelow(True)
# plot impulse response
ax2 = plt.subplot(3, 2, 2)
ax2.stem(np.squeeze(h[0]),
np.squeeze(h[1]),
use_line_collection=True,
linefmt='C0:',
markerfmt='C0o',
basefmt='C0:')
ax2.xaxis.set_major_locator(MaxNLocator(integer=True))
ax2.grid(True)
ax2.set_xlabel(r'$k$')
ax2.set_ylabel(r'impulse response $h[k]$')
# plot frequency response: phase
ax3 = plt.subplot(3, 2, 3)
ax3.plot(W, np.unwrap(np.angle(H)))
ax3.set_xlabel(
r'digital angular frequency in radian or $\Omega$ / rad')
ax3.set_ylabel(r'phase in radian or $\angle$ H / rad')
ax3.set_xlim([0, 2 * np.pi])
ax3.set_xticks(np.arange(9) * np.pi / 4)
ax3.set_xticklabels([
r'0', r'$\pi/4$', r'$\pi/2$', r'$3/4\pi$', r'$\pi$', r'$5/4\pi$',
r'$3/2\pi$', r'$7/4\pi$', r'$2\pi$'
])
# ax3.set_ylim([-180, +180])
# ax3.set_yticks(np.arange(-180, +180+45, 45))
ax3.grid(True)
# plot step response
ax4 = plt.subplot(3, 2, 4)
ax4.stem(np.squeeze(he[0]),
np.squeeze(he[1]),
use_line_collection=True,
linefmt='C0:',
markerfmt='C0o',
basefmt='C0:')
ax4.xaxis.set_major_locator(MaxNLocator(integer=True))
ax4.grid(True)
ax4.set_xlabel(r'$k$')
ax4.set_ylabel(r'step response $h_\epsilon[k]$')
# plot frequency response: group delay
ax5 = plt.subplot(3, 2, 5)
ax5.plot(W / np.pi, gd / fs)
ax5.set_xscale('linear')
ax5.set_xlim([0, 2])
ax5.set_xlabel(r'$\frac{\Omega}{\pi}$')
ax5.set_ylabel(r'group delay in seconds or $\tau_\mathrm{GD}$ / s')
ax5.grid(True, which='both')
# zplane
ax6 = plt.subplot(3, 2, 6)
plot_zplane(sys.zeros, sys.poles, sys.gain) # see function above
delay = 446e-6
D = int(delay / (1 / fs))
# constantes filtro
Aa = 70
fp = 1.2e3
fa = 2 * fp
N = 25
# N, beta = sgn.kaiserord(Aa, (fa-fp)/(fs/2))
taps = sgn.firwin(N, fp / (fs / 2), window=("chebwin", 55))
gen_filter_coeffs(taps, N)
# taps = sgn.firwin(N, fp/(fs/2), window='hann')
# print(taps)
sistema = sgn.dlti(taps, [1] + [0 for i in range(len(taps) - 1)])
tout, step = sgn.dstep(sistema)
step = step[0]
step = [step[i][0] for i in range(len(step))]
plt.plot(tout, step)
plt.show()
# for tap in taps:
# print('{0:.30e}'.format(tap))
w, h = sgn.freqz(taps, worN=8000)
plt.plot((w / np.pi) * (fs / 2), 20 * np.log10(np.absolute(h)), linewidth=2)
plt.xlabel('Frequency (Hz)')
plt.ylabel('Gain')
plt.title('Frequency Response')
# plt.ylim(-0.05, 1.05)
plt.grid(True)
########################################
# Poles and Zeros on the Digital Plant #
########################################
if Poles_Zeros_Digital_Plant == True:
plot_argand(filter_dig_zoh)
##############################
# Step Response verification #
##############################
simulation_time = 0.1
t = np.linspace(0, simulation_time, num=(simulation_time*Fsampling))
if Step_Digital_Plant == True:
tout, yout_zoh = dstep([filter_dig_zoh.num, filter_dig_zoh.den, td], t=t)
yout1 = np.transpose(yout_zoh)
yout0 = yout1[0]
yout_zoh = yout0[:tout.size]
fig, ax = plt.subplots()
ax.set_title('Step Filter Digital ZOH')
ax.set_ylabel('Plant Voltage')
ax.set_xlabel('Time [s]')
ax.grid()
ax.plot(tout, yout_zoh, 'y')
# ax.set_ylim(ymin=-20, ymax=100)
plt.tight_layout()
plt.show()
Ts = 1. / 25000
#sysd = signal.cont2discrete ((num, den), Ts)
#w1, h1 = signal.freqz(sysd)
numd, dend, dt = signal.cont2discrete((num, den), Ts)
numd1 = numd[0, :]
w1, h1 = signal.freqz(numd1, dend, worN=None, whole=0, plot=None)
plt.figure(3)
plt.clf()
plt.plot(w1, 20 * np.log10(abs(h1)), 'b')
plt.ylabel('Amplitude [dB]', color='b')
plt.xlabel('Frequency [rad/sample]')
plt.show()
#u = np.ones(1000)
#t_in = np.arange(0, 1000)
#t_out, y = dlsim(sysd, u, t=t_in)
#tout, yout = signal.dstep((numd1, dend, Ts), x0=None, t=None, n=None)
tout, yout = signal.dstep((numd1, dend, Ts), x0=None, t=None, n=25)
#split del tuple
y_out = yout[0]
plt.figure(4)
plt.clf()
plt.plot(tout, y_out)
plt.show()
print T.shape #print T #print "this is 't'" #print t #print "this is s" #print s # recalculate t and s to get smooth plot #t, s = step(tf, T ) t, s = step(tf, T=linspace(0, 5, 100)) #t, s = step(tf, T) dsys = (dnum, dden, 1 / 50.0) print "dsys type:" print type(dsys) dt, ds = dstep(dsys) print "ds:" print ds print "ds type:" print type(ds) ds_yvals = np.array(ds[0][0:, 0]) #print "continuous time values:" #print t.shape #print t print "discrete time values:"
### Desde aca sistema Digital
### Convierto Forward Euler
Fsampling = 1500
Tsampling = 1 / Fsampling
num_d1, den_d1, td = cont2discrete((planta.num, planta.den),
Tsampling,
method='euler')
#normalizo con dlti
planta_d1 = dlti(num_d1, den_d1)
print('Planta Digital sys 1')
print(str(planta_d1.num), '/(', str(planta_d1.den), ')')
#respuesta escalon
t = np.arange(0, 0.1, td)
tout, yout = dstep([planta_d1.num, planta_d1.den, td], t=t)
yout1 = np.transpose(yout)
yout0 = yout1[0]
yout = yout0[:tout.size]
plt.figure(1)
plt.clf()
plt.title('digital Step Response')
plt.stem(tout, yout)
plt.show()
###otro mas ajustado, agrega zero en 1 y compensa ganancia
num_d2 = [0.091, 0.091]
den_d2 = [1, -0.8861]
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]')
plt.tight_layout()
plt.show()
t = np.linspace(0, 0.1, num=2000)
tout_t, yout_t = dstep([planta_dig_tustin.num, planta_dig_tustin.den, td], t=t)
tout_e, yout_e = dstep([planta_dig_euler.num, planta_dig_euler.den, td], t=t)
yout1 = np.transpose(yout_t)
yout0 = yout1[0]
yout_t = yout0[:tout_t.size]
yout1 = np.transpose(yout_e)
yout0 = yout1[0]
yout_e = yout0[:tout_e.size]
fig, ax = plt.subplots()
ax.set_title('Respuesta escalon de la planta digital')
ax.set_ylabel('Corriente')
ax.set_xlabel('Tiempo [s]')
ax.grid()
ax.stem(tout_t, yout_t)
ax.stem(tout_e, yout_e, '-.')
def test_step_invariant(self, sys, sample_time, samples_number):
time = np.arange(samples_number) * sample_time
_, yout_cont = step2(sys, T=time, **self.tolerances)
_, yout_disc = dstep(c2d(sys, sample_time, method='zoh'), n=len(time))
assert_allclose(yout_cont.ravel(), yout_disc[0].ravel())
# -*- coding: utf-8 -*-
"""
Created on Wed Mar 24 08:40:22 2021
@author: cns59
"""
import numpy as np
from scipy import signal
import matplotlib.pyplot as plt
M=48
Kd=?
Kp=?
Ki=?
num=[?, ?, ? ]
den=[1.0, 0.7, 0.1475, 0.0088]
drobot=signal.cont2discrete((num, den),0.05)
t, y = signal.dstep(drobot,n=50)
plt.step(t, np.squeeze(y))
plt.grid()
plt.xlabel('n [samples]')
plt.ylabel('Amplitude')
ax1.semilogx(w / (2 * pi) * Fsampling, 20 * np.log10(abs(h)), 'b')
ax1.set_ylabel('Amplitude [dB]', color='b')
ax1.set_xlabel('Frequency [Hz]')
angles = np.unwrap(np.angle(h))
ax2.semilogx(w / (2 * pi) * Fsampling, angles * 180 / pi, 'b-', linewidth="1")
ax2.set_title('Angle')
plt.tight_layout()
plt.show()
### Ahora voy a probar la respuesta escalon del sistema digital
tfinal = 0.01
num = tfinal * Fsampling
t = np.linspace(0, tfinal, num=num)
tout, yout = dstep(planta_d, t=t)
yout1 = np.transpose(yout)
yout0 = yout1[0]
yout = yout0[:tout.size]
fig, ax = plt.subplots()
ax.set_title('Respuesta escalon de la planta d2')
ax.set_ylabel('Corriente')
ax.set_xlabel('Tiempo [s]')
ax.grid()
ax.stem(tout, yout, 'b-')
plt.tight_layout()
plt.show()
from scipy import signal
import matplotlib.pyplot as plt
butter = signal.dlti(*signal.butter(3, 0.5))
t, y = signal.dstep(butter, n=25)
plt.step(t, np.squeeze(y))
plt.grid()
plt.xlabel('n [samples]')
plt.ylabel('Amplitude')