def classic():
# PI controller
Kp = 1.5
Ki = 30.
P = tf([Kp], [1])
I = tf([Ki], [1, 0])
Gc = parallel(P, I)
# Power convertor
Gpc = synthesize_2pole_filter(50, 0.707)
# Plant
Gp = tf([50], [1, 0])
# Sensor
# если выбросить из петли становится лучше
# "remove sensor lag"
Gs = synthesize_1pole_filter(20)
# Openloop
Gol = series(series(Gc, Gpc), Gp)
# Closed loop
Gcl = feedback(Gol)
ts, xs = step_response( Gcl )
grid()
plot(ts, xs)
def testLists(self):
"""Make sure that lists of various lengths work for operations"""
sys1 = ctrl.tf([1, 1], [1, 2])
sys2 = ctrl.tf([1, 3], [1, 4])
sys3 = ctrl.tf([1, 5], [1, 6])
sys4 = ctrl.tf([1, 7], [1, 8])
sys5 = ctrl.tf([1, 9], [1, 0])
# Series
sys1_2 = ctrl.series(sys1, sys2)
np.testing.assert_array_almost_equal(sort(pole(sys1_2)), [-4., -2.])
np.testing.assert_array_almost_equal(sort(zero(sys1_2)), [-3., -1.])
sys1_3 = ctrl.series(sys1, sys2, sys3);
np.testing.assert_array_almost_equal(sort(pole(sys1_3)),
[-6., -4., -2.])
np.testing.assert_array_almost_equal(sort(zero(sys1_3)),
[-5., -3., -1.])
sys1_4 = ctrl.series(sys1, sys2, sys3, sys4);
np.testing.assert_array_almost_equal(sort(pole(sys1_4)),
[-8., -6., -4., -2.])
np.testing.assert_array_almost_equal(sort(zero(sys1_4)),
[-7., -5., -3., -1.])
sys1_5 = ctrl.series(sys1, sys2, sys3, sys4, sys5);
np.testing.assert_array_almost_equal(sort(pole(sys1_5)),
[-8., -6., -4., -2., -0.])
np.testing.assert_array_almost_equal(sort(zero(sys1_5)),
[-9., -7., -5., -3., -1.])
# Parallel
sys1_2 = ctrl.parallel(sys1, sys2)
np.testing.assert_array_almost_equal(sort(pole(sys1_2)), [-4., -2.])
np.testing.assert_array_almost_equal(sort(zero(sys1_2)),
sort(zero(sys1 + sys2)))
sys1_3 = ctrl.parallel(sys1, sys2, sys3);
np.testing.assert_array_almost_equal(sort(pole(sys1_3)),
[-6., -4., -2.])
np.testing.assert_array_almost_equal(sort(zero(sys1_3)),
sort(zero(sys1 + sys2 + sys3)))
sys1_4 = ctrl.parallel(sys1, sys2, sys3, sys4);
np.testing.assert_array_almost_equal(sort(pole(sys1_4)),
[-8., -6., -4., -2.])
np.testing.assert_array_almost_equal(sort(zero(sys1_4)),
sort(zero(sys1 + sys2 +
sys3 + sys4)))
sys1_5 = ctrl.parallel(sys1, sys2, sys3, sys4, sys5);
np.testing.assert_array_almost_equal(sort(pole(sys1_5)),
[-8., -6., -4., -2., -0.])
np.testing.assert_array_almost_equal(sort(zero(sys1_5)),
sort(zero(sys1 + sys2 +
sys3 + sys4 + sys5)))
def testLists(self):
"""Make sure that lists of various lengths work for operations"""
sys1 = ctrl.tf([1, 1], [1, 2])
sys2 = ctrl.tf([1, 3], [1, 4])
sys3 = ctrl.tf([1, 5], [1, 6])
sys4 = ctrl.tf([1, 7], [1, 8])
sys5 = ctrl.tf([1, 9], [1, 0])
# Series
sys1_2 = ctrl.series(sys1, sys2)
np.testing.assert_array_almost_equal(sort(pole(sys1_2)), [-4., -2.])
np.testing.assert_array_almost_equal(sort(zero(sys1_2)), [-3., -1.])
sys1_3 = ctrl.series(sys1, sys2, sys3);
np.testing.assert_array_almost_equal(sort(pole(sys1_3)),
[-6., -4., -2.])
np.testing.assert_array_almost_equal(sort(zero(sys1_3)),
[-5., -3., -1.])
sys1_4 = ctrl.series(sys1, sys2, sys3, sys4);
np.testing.assert_array_almost_equal(sort(pole(sys1_4)),
[-8., -6., -4., -2.])
np.testing.assert_array_almost_equal(sort(zero(sys1_4)),
[-7., -5., -3., -1.])
sys1_5 = ctrl.series(sys1, sys2, sys3, sys4, sys5);
np.testing.assert_array_almost_equal(sort(pole(sys1_5)),
[-8., -6., -4., -2., -0.])
np.testing.assert_array_almost_equal(sort(zero(sys1_5)),
[-9., -7., -5., -3., -1.])
# Parallel
sys1_2 = ctrl.parallel(sys1, sys2)
np.testing.assert_array_almost_equal(sort(pole(sys1_2)), [-4., -2.])
np.testing.assert_array_almost_equal(sort(zero(sys1_2)),
sort(zero(sys1 + sys2)))
sys1_3 = ctrl.parallel(sys1, sys2, sys3);
np.testing.assert_array_almost_equal(sort(pole(sys1_3)),
[-6., -4., -2.])
np.testing.assert_array_almost_equal(sort(zero(sys1_3)),
sort(zero(sys1 + sys2 + sys3)))
sys1_4 = ctrl.parallel(sys1, sys2, sys3, sys4);
np.testing.assert_array_almost_equal(sort(pole(sys1_4)),
[-8., -6., -4., -2.])
np.testing.assert_array_almost_equal(sort(zero(sys1_4)),
sort(zero(sys1 + sys2 +
sys3 + sys4)))
sys1_5 = ctrl.parallel(sys1, sys2, sys3, sys4, sys5);
np.testing.assert_array_almost_equal(sort(pole(sys1_5)),
[-8., -6., -4., -2., -0.])
np.testing.assert_array_almost_equal(sort(zero(sys1_5)),
sort(zero(sys1 + sys2 +
sys3 + sys4 + sys5)))
def multi_loop():
"""
Gives the transfer funtion of multi-loop system
Feedback[ R(s) -> (G1 - H2) -> G2 -> feedback(G3G4, H1) , H3]-> C(s)
"""
g1 = np.array([
[1], [1, 10]])
g2 = np.array([
[1],
[1, 1]]
)
g3 = np.array([
[1, 0, 1],
[1, 4, 4]
])
g4 = np.array([
[1, 1],
[1, 6]
])
h1 = np.array([
[1, 1],
[1, 2]
])
h2 = np.array([[2], 1])
h3 = np.array([[1], 1])
sys_g1 = get_sys(g1, sys_name='tf')
sys_g2 = get_sys(g2, 'tf')
sys_g3 = get_sys(g3, 'tf')
sys_g4 = get_sys(g4, 'tf')
sysh1 = get_sys(h1, 'tf')
sysh2 = get_sys(h2, 'tf')
sysh3 = get_sys(h3, 'tf')
td.ouput(title='Exercise One',
keys=['G1', 'G2', 'G3', 'G4', 'H1', 'H2', 'H3'],
values=[sys_g1, sys_g2, sys_g3, sys_g4, sysh1, sysh2, sysh3])
sys_t = ct.series(sys_g1 - sysh2, sys_g2)
sys_t = ct.series(sys_t,
ct.feedback(
ct.series(sys_g3, sys_g4), sysh1, sign=1))
tS = ct.feedback(sys_t, sysh3)
zeros, poles = ct.pzmap(tS, Plot=True, grid=True)
show_plot(zeros, poles)
def delay(sys, T):
"""========================================================================
Function: delay()
approximates the given delay with a PT100 element and concatenates it
with the given system.
Synopsis:
sys1 = delay(sys,T)
Input Parameter:
- sys: Control module LTI-System in transfer function or state space
formulation. The delay will be added to this system.
- T: Delay time that will be added to this system.
Output Parameter:
- sys1: The delayed input system.
========================================================================"""
# define the PT100 element
n = 100
T = float(T) / n
A = (np.diag([-1] * n, 0) + np.diag([1] * (n - 1), -1)) / T
B = np.vstack([1, [[0]] * (n - 1)]) / T
C = np.hstack([[0] * (n - 1), 1])
D = 0
# concatenate it with the system
return ctrl.series(sys, ctrl.ss(A, B, C, D))
def serify(called=False):
"""
Generates a system of transfer functions for
a series block combination
"""
# Create arrays for G(s)
numerator_g = np.array([1])
denominator_g = np.array([500, 0, 0])
# Create arrays for G(c)
numerator_h = np.array([1, 1])
denominator_h = np.array([1, 2])
# Get transfer functions
sys_g = ct.tf(numerator_g, denominator_g)
sys_h = ct.tf(numerator_h, denominator_h)
sys_tranfer_func = ct.series(sys_g, sys_h)
if called:
return sys_g, sys_h
td.ouput('Series TF',
keys=['G(s)', 'G(c)', 'sys tf'],
values=[sys_g, sys_h, sys_tranfer_func])
def calculating_params(self, populations):
calculatedParams = []
for population in populations:
kp = population[0]
ti = population[1]
td = population[2]
G = kp * control.tf([ti * td, ti, 1], [ti, 0])
F = control.tf(1, [1, 6, 11, 6, 0])
system = control.feedback(control.series(G, F), 1)
t = []
i = 0
while i < 100:
t.append(i)
i += 0.01
try:
systemInfo = control.step_info(system)
except IndexError:
calculatedParams.append([10000000, 1, 100, 200])
continue
T, output = control.step_response(system, T=t)
ISE = round(sum((output - 1)**2), 2)
timeRise = round(systemInfo['RiseTime'], 2)
timeSettle = round(systemInfo['SettlingTime'], 2)
overshoot = round(systemInfo['Overshoot'], 2)
calculatedParams.append([ISE, timeRise, timeSettle, overshoot])
return calculatedParams
def pidplot1(num, den, kp, ki, kd):
G = control.tf(num, den)
if ki != 0:
g2 = control.tf([kd, kp, ki], [1, 0])
else:
g2 = control.tf([kd, kp], [1])
k = control.series(G, g2)
v = control.feedback(k, 1)
t1, y1 = control.step_response(G)
t2, y2 = control.step_response(v)
plt.plot(t2, y2, 'r', linewidth=1, label='G(s) with PID control')
plt.grid(True)
s = '(Kp=' + str(kp) + ',Ki=' + str(ki) + ',Kd=' + str(kd) + ')'
plt.title('Time response of G(s) with PID control' + s)
plt.xlabel('Time(sec)')
plt.ylabel('Amplitude')
if not os.path.isdir('static'):
os.mkdir('static')
else:
# Remove old plot files
for filename in glob.glob(
os.path.join('static', 'pidcontrol', 'pidc1.png')):
os.remove(filename)
pidc1 = os.path.join('static', 'pidcontrol', 'pidc1.png')
plt.savefig(pidc1)
plt.clf()
plt.cla()
plt.close()
return pidc1
def fitnessFunction(Kp, Ti, Td):
G = control.tf([Ti * Td, Ti, 1], [Ti, 0])
F = control.tf(1, [1, 6, 11, 6, 0])
sys = control.feedback(control.series(G, F), 1)
y = control.step(sys)
yout = y[0]
t = y[-1]
# Integrated Square Error
error = 0
for val in y[0]:
error += (val - 1) * (val - 1)
# Overshoot
OS = (yout.max() / yout[-1] - 1) * 100
Tr = 0
Ts = 0
# Rising Time
for i in range(0, len(yout) - 1):
if yout[i] > yout[-1] * .90:
Tr = t[i] - t[0]
# Settling Time
for i in range(2, len(yout) - 1):
if abs(yout[-i] / yout[-1]) > 1.02:
Ts = t[len(yout) - i] - t[0]
return 1 / (OS + Tr + Ts + error * error * 10) * 100000
def bode_cor(num1, den1, num2, den2):
sys1 = ctl.tf(num1, den1)
sys2 = ctl.tf(num2, den2)
sys_BO = ctl.series(sys1, sys2)
sys = sys_BO.returnScipySignalLti()[0][0]
w, mag, phase = sys.bode()
return w, mag, phase
def fitnessFunction(Kp, Ti, Td):
G = control.tf([Ti*Td, Ti, 1], [Ti, 0])
F = control.tf(1,[1,6,11,6,0])
sys = control.feedback(control.series(G, F), 1)
y = control.step(sys)
yout = y[0]
t = y[-1]
# Integrated Square Error
error = 0
for val in y[0]:
error += (val - 1)*(val - 1)
# Overshoot
OS = (yout.max()/yout[-1]-1)*100
Tr = 0
Ts = 0
# Rising Time
for i in range(0, len(yout) - 1):
if yout[i] > yout[-1]*.90:
Tr = t[i] - t[0]
# Settling Time
for i in range(2, len(yout) - 1):
if abs(yout[-i]/yout[-1]) > 1.02:
Ts = t[len(yout) - i] - t[0]
return 1/(OS + Tr + Ts + error*error*10)*100000
def Controlador(X):
#Atribuição dos paramentros Kp, Ki e Kd
Kp = Kpi + X[0] / 100
Ki = Kii + X[1] / 100
Kd = Kdi + X[2] / 100
#Função Transferência dos termos do controlador
P = Kp
I = tf(Ki, [1, 0])
D = tf([Kd, 0], [0.1 * Kd, 1])
#União dos blocos PID
C = parallel(P, I, D)
# Função Transferência com o Controlador
F = series(C, G)
# Penalidade para o valor do sinal de Entrada na planta
# ou seja penalidade para o sinal de Controle alto
tc = feedback(C, G)
_, yc = step_response(tc, time)
if max(yc) > maxcontrolador:
#SE1 = np.square(np.subtract(1,yc))
ISE = tdiv + max(yc)
else:
# Realizando a Integral do erro quadrado
t1 = feedback(F, 1)
_, y1 = step_response(t1, time)
SE = np.square(np.subtract(1, y1))
ISE = np.sum(SE)
return (ISE)
def generate_lead_controller(G, testing_point):
angle_G_evaluated_at_testpoint = math.degrees(
np.angle(control.evalfr(G, testing_point)))
defficit = 180 - angle_G_evaluated_at_testpoint
if defficit < 90:
new_zero = np.real(testing_point)
angle_new_pole = 90 - defficit
else:
new_zero = 0
angle_new_zero = math.degrees(
np.angle(
control.evalfr(control.series(control.tf([1, 0], 1), G),
testing_point)))
if angle_new_zero < 0:
angle_new_pole = 180 + angle_new_zero
else:
angle_new_pole = -180 - angle_new_zero
distance_pole_zero = (np.imag(testing_point) /
math.tan(math.radians(angle_new_pole)))
new_pole = np.real(testing_point) - distance_pole_zero
if new_zero == 0:
num_and_controler_zero = 1
num_and_controler_zero = np.concatenate([[1], [-new_zero]])
den_and_controler_pole = np.concatenate([[1], [-new_pole]])
lead_controller = control.tf(num_and_controler_zero,
den_and_controler_pole)
return lead_controller
def Q2_perfFCN(Kp, Ti, Td): G = Kp * tf([Ti * Td, Ti, 1.0], [Ti, 0]) sys = feedback(series(G,F),1) res = step_response(sys, t) ISE = sum((val - 1)**2 for val in res[1]) return (ISE,) + stepinfo(res)
def test_bdalg_functions(self):
"""Test block diagram functions algebra on I/O systems"""
# Set up parameters for simulation
T = self.T
U = [np.sin(T), np.cos(T)]
X0 = 0
# Set up systems to be composed
linsys1 = self.mimo_linsys1
linio1 = ios.LinearIOSystem(linsys1)
linsys2 = self.mimo_linsys2
linio2 = ios.LinearIOSystem(linsys2)
# Series interconnection
linsys_series = ct.series(linsys1, linsys2)
iosys_series = ct.series(linio1, linio2)
lin_t, lin_y, lin_x = ct.forced_response(linsys_series, T, U, X0)
ios_t, ios_y = ios.input_output_response(iosys_series, T, U, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Make sure that systems don't commute
linsys_series = ct.series(linsys2, linsys1)
lin_t, lin_y, lin_x = ct.forced_response(linsys_series, T, U, X0)
self.assertFalse((np.abs(lin_y - ios_y) < 1e-3).all())
# Parallel interconnection
linsys_parallel = ct.parallel(linsys1, linsys2)
iosys_parallel = ct.parallel(linio1, linio2)
lin_t, lin_y, lin_x = ct.forced_response(linsys_parallel, T, U, X0)
ios_t, ios_y = ios.input_output_response(iosys_parallel, T, U, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Negation
linsys_negate = ct.negate(linsys1)
iosys_negate = ct.negate(linio1)
lin_t, lin_y, lin_x = ct.forced_response(linsys_negate, T, U, X0)
ios_t, ios_y = ios.input_output_response(iosys_negate, T, U, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Feedback interconnection
linsys_feedback = ct.feedback(linsys1, linsys2)
iosys_feedback = ct.feedback(linio1, linio2)
lin_t, lin_y, lin_x = ct.forced_response(linsys_feedback, T, U, X0)
ios_t, ios_y = ios.input_output_response(iosys_feedback, T, U, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
def lag_controller_design(error, percentage_to_reduce, lead_controller, G,
K_lead):
lead_at_0 = control.evalfr(lead_controller, 0)
plant_at_0 = control.evalfr(G, 0)
error_lag_lead_ss = error_ss - percentage_to_reduce
x = Symbol('x')
equation = (1 / (1 + x * control.evalfr(control.series(lead_controller, G),
0) * K_lead)) - error_lag_lead_ss
return solve(equation, x), error_lag_lead_ss
def compute_ise(Kp, Ti, Td):
g = Kp * TransferFunction([Ti * Td, Ti, 1], [Ti, 0])
sys = feedback(series(g, F), 1)
sys_info = step_info(sys)
_, y = step_response(sys, T)
ise = sum((y - 1)**2)
t_r = sys_info['RiseTime']
t_s = sys_info['SettlingTime']
m_p = sys_info['Overshoot']
return ise, t_r, t_s, m_p
def q1_perfFNC(Kp, Ti, Td):
G = Kp * TransferFunction([Ti * Td, Ti, 1], [Ti, 0])
sys = feedback(series(G, F), 1)
sysinf = step_info(sys)
T, y = step_response(sys, T=t)
# return ISE, t_r, t_s, M_p
return sum((
y -
1)**2), sysinf['RiseTime'], sysinf['SettlingTime'], sysinf['Overshoot']
def pid_controller(K_kr, T_kr):
num = [10]
den = [10, 25, 11, 2]
G = ctl.tf(num, den)
K_P = 0.6 * K_kr
K_I = 1.2 * K_kr / T_kr
K_D = 0.072 * K_kr * T_kr
K_PID = ctl.parallel(ctl.tf([K_I], [1, 0]), ctl.tf([K_D, 0], [1]), K_P)
G_0 = ctl.series(K_PID, G)
return ctl.feedback(G_0)
def testMimoSeries(self, tsys):
"""regression: bdalg.series reverses order of arguments"""
g1 = ctrl.ss([], [], [], [[1, 2], [0, 3]])
g2 = ctrl.ss([], [], [], [[1, 0], [2, 3]])
ref = g2 * g1
tst = ctrl.series(g1, g2)
np.testing.assert_array_equal(ref.A, tst.A)
np.testing.assert_array_equal(ref.B, tst.B)
np.testing.assert_array_equal(ref.C, tst.C)
np.testing.assert_array_equal(ref.D, tst.D)
def pid_design(G,
K_guess,
d_tc,
verbose=False,
use_P=True,
use_I=True,
use_D=True):
# type: (control.tf, np.array, float, bool, bool, bool, bool) -> (np.array, control.tf, control.tf)
"""
:param G: transfer function
:param K_guess: gain matrix guess
:param d_tc: time constant for derivative
:param verbose: show debug output
:param use_P: use p gain in design
:param use_I: use i gain in design
:param use_D: use d gain in design
:return: (K, G_comp, Gc_comp)
K: gain matrix
G_comp: open loop compensated plant
Gc_comp: closed loop compensated plant
"""
# compensator transfer function
H = []
if use_P:
H += [control.tf(1, 1)]
if use_I:
H += [control.tf((1), (1, 0))]
if use_D:
H += [control.tf((1, 0), (d_tc, 1))]
H = np.array([H]).T
H_num = [[H[i][j].num[0][0] for i in range(H.shape[0])]
for j in range(H.shape[1])]
H_den = [[H[i][j].den[0][0] for i in range(H.shape[0])]
for j in range(H.shape[1])]
H = control.tf(H_num, H_den)
# print('G', G)
# print('H', H)
ss_open = control.tf2ss(G * H)
if verbose:
print('optimizing controller')
K = lqr_ofb_design(K_guess, ss_open, verbose)
if verbose:
print('done')
# print('K', K)
# print('H', H)
G_comp = control.series(G, H * K)
Gc_comp = control.feedback(G_comp, 1)
return K, G_comp, Gc_comp
def stepresponse(Kp, Ti, Td):
G = Kp * control.tf([Ti * Td, Ti, 1], [Ti, 0])
F = control.tf(1, [1, 6, 11, 6, 0])
system = control.feedback(control.series(G, F), 1)
t = numpy.arange(0, 100, 0.01)
y, T = control.step(system, t)
t_r, t_s, M_p = stepinfo(T, y)
ISE = sum(numpy.power(numpy.subtract(y, 1), 2))
return ISE, t_r, t_s, M_p
def f(X):
Kp, Ki, Kd = X[0], X[1], X[2]
# Controlador
K = Kp + Ki / s + Kd * s / (1 + 0.001 * s)
# Função de transferência de malha fechada r(t) -> [FTMF] -> y(t)
FTMF = co.feedback(co.series(K, H))
# Resposta ao degrau
t1, y1 = co.step_response(FTMF, T=t, X0=0.0)
# plt.plot(t1, y1, label='Resposta ao degrau')
# Erro
erro = 1 - y1
# plt.plot(t1, erro**2, label='Erro quadrático')
# plt.plot(t1, abs(erro), label='Erro absoluto')
# Plot
# plt.xlabel('Tempo(s)')
# plt.ylabel('Amplitude')
# plt.grid()
# plt.legend()
# plt.show()
# u(t)
t2, u1, xout = co.forced_response(K, T=t1, U=erro)
# plt.plot(t2, u1, label='Sinal de controle')
# Plot
# plt.xlabel('Tempo(s)')
# plt.ylabel('Amplitude')
# plt.grid()
# plt.legend()
# x1,x2,y1,y2 = plt.axis()
# plt.axis((x1,5,y1,y2))
# plt.show()
# Integral de erro quadrático
I = sum(erro**2) * dt
# Integral de erro absoluto
# I = sum(abs(erro))*dt
# Custo (Função LQR)
Q = 1
R = 0.01
J = Q * I + R * sum(u1**2) * dt
# print('\n', J)
return J
def test_lineariosys_statespace(self):
"""Make sure that a LinearIOSystem is also a StateSpace object"""
iosys_siso = ct.LinearIOSystem(self.siso_linsys)
self.assertTrue(isinstance(iosys_siso, ct.StateSpace))
# Make sure that state space functions work for LinearIOSystems
np.testing.assert_array_equal(
iosys_siso.pole(), self.siso_linsys.pole())
omega = np.logspace(.1, 10, 100)
mag_io, phase_io, omega_io = iosys_siso.freqresp(omega)
mag_ss, phase_ss, omega_ss = self.siso_linsys.freqresp(omega)
np.testing.assert_array_equal(mag_io, mag_ss)
np.testing.assert_array_equal(phase_io, phase_ss)
np.testing.assert_array_equal(omega_io, omega_ss)
# LinearIOSystem methods should override StateSpace methods
io_mul = iosys_siso * iosys_siso
self.assertTrue(isinstance(io_mul, ct.InputOutputSystem))
# But also retain linear structure
self.assertTrue(isinstance(io_mul, ct.StateSpace))
# And make sure the systems match
ss_series = self.siso_linsys * self.siso_linsys
np.testing.assert_array_equal(io_mul.A, ss_series.A)
np.testing.assert_array_equal(io_mul.B, ss_series.B)
np.testing.assert_array_equal(io_mul.C, ss_series.C)
np.testing.assert_array_equal(io_mul.D, ss_series.D)
# Make sure that series does the same thing
io_series = ct.series(iosys_siso, iosys_siso)
self.assertTrue(isinstance(io_series, ct.InputOutputSystem))
self.assertTrue(isinstance(io_series, ct.StateSpace))
np.testing.assert_array_equal(io_series.A, ss_series.A)
np.testing.assert_array_equal(io_series.B, ss_series.B)
np.testing.assert_array_equal(io_series.C, ss_series.C)
np.testing.assert_array_equal(io_series.D, ss_series.D)
# Test out feedback as well
io_feedback = ct.feedback(iosys_siso, iosys_siso)
self.assertTrue(isinstance(io_series, ct.InputOutputSystem))
# But also retain linear structure
self.assertTrue(isinstance(io_series, ct.StateSpace))
# And make sure the systems match
ss_feedback = ct.feedback(self.siso_linsys, self.siso_linsys)
np.testing.assert_array_equal(io_feedback.A, ss_feedback.A)
np.testing.assert_array_equal(io_feedback.B, ss_feedback.B)
np.testing.assert_array_equal(io_feedback.C, ss_feedback.C)
np.testing.assert_array_equal(io_feedback.D, ss_feedback.D)
def testMimoSeries(self):
"""regression: bdalg.series reverses order of arguments"""
g1 = ctrl.ss([],[],[],[[1,2],[0,3]])
g2 = ctrl.ss([],[],[],[[1,0],[2,3]])
ref = g2*g1
tst = ctrl.series(g1,g2)
# assert_array_equal on mismatched matrices gives
# "repr failed for <matrix>: ..."
def assert_equal(x,y):
np.testing.assert_array_equal(np.asarray(x),
np.asarray(y))
assert_equal(ref.A, tst.A)
assert_equal(ref.B, tst.B)
assert_equal(ref.C, tst.C)
assert_equal(ref.D, tst.D)
def testMimoSeries(self):
"""regression: bdalg.series reverses order of arguments"""
g1 = ctrl.ss([],[],[],[[1,2],[0,3]])
g2 = ctrl.ss([],[],[],[[1,0],[2,3]])
ref = g2*g1
tst = ctrl.series(g1,g2)
# assert_array_equal on mismatched matrices gives
# "repr failed for <matrix>: ..."
def assert_equal(x,y):
np.testing.assert_array_equal(np.asarray(x),
np.asarray(y))
assert_equal(ref.A, tst.A)
assert_equal(ref.B, tst.B)
assert_equal(ref.C, tst.C)
assert_equal(ref.D, tst.D)
def step_response_BF_cor(num1, den1, num2, den2, feedback=1, t=None):
if t == None:
t = np.linspace(0, 10, 101)
sys1 = ctl.tf(num1, den1)
sys2 = ctl.tf(num2, den2)
sys_BO = ctl.series(sys1, sys2)
sys_BF = g = ctl.feedback(sys_BO, feedback)
lti_BF = sys_BF.returnScipySignalLti()[0][0]
t, s = signal.step2(lti_BF, None, t)
return t, s
def func():
import control
sys1 = control.tf([
2,
], [1, 0.1, 1])
sys2 = control.tf([
1,
], [1, 2])
sys = control.series(sys1, sys2)
t = range(100)
t, u = control.step_response(control.tf([
1,
], [10, 1]), t)
t, y, x = control.forced_response(sys, t, u)
h = ["u(t)", "y(t)"]
return [u, y], t, h
def pid_design(G, K_guess, d_tc, verbose=False, use_P=True, use_I=True, use_D=True):
"""
:param G: transfer function
:param K_guess: gain matrix guess
:param d_tc: time constant for derivative
:param use_P: use p gain in design
:param use_I: use i gain in design
:param use_D: use d gain in design
:return: (K, G_comp, Gc_comp)
K: gain matrix
G_comp: open loop compensated plant
Gc_comp: closed loop compensated plant
"""
# compensator transfer function
H = []
if use_P:
H += [control.tf(1, 1)]
if use_I:
H += [control.tf((1), (1, 0))]
if use_D:
H += [control.tf((1, 0), (d_tc, 1))]
H = np.array([H]).T
H_num = [[H[i][j].num[0][0] for i in range(H.shape[0])] for j in range(H.shape[1])]
H_den = [[H[i][j].den[0][0] for i in range(H.shape[0])] for j in range(H.shape[1])]
H = control.tf(H_num, H_den)
# print('G', G)
# print('H', H)
ss_open = control.tf2ss(G*H)
if verbose:
print('optimizing controller')
K = lqr_ofb_design(K_guess, ss_open, verbose)
if verbose:
print('done')
# print('K', K)
# print('H', H)
G_comp = control.series(G, H*K)
Gc_comp = control.feedback(G_comp, 1)
return K, G_comp, Gc_comp
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
def q1_perfFNC(Kp, Ti, Td):
G = Kp * TransferFunction([Ti * Td, Ti, 1], [Ti, 0])
F = TransferFunction(1, [1, 6, 11, 6, 0])
sys = feedback(series(G, F), 1)
sysinf = step_info(sys)
t = []
i = 0
while i < 100:
t.append(i)
i += 0.01
T, y = step_response(sys, T=t)
ISE = sum((y - 1)**2)
t_r = sysinf['RiseTime']
t_s = sysinf['SettlingTime']
M_p = sysinf['Overshoot']
return ISE, t_r, t_s, M_p
def f(X):
sis = tf([2], [4, 1]) #tf do sistema 2/(4s + 1)
sisp = tf([X[0]], [1]) # tf do controlador proporcional Kp
sisi = tf([X[0] * X[1]], [1, 0]) # tf do controlador integral Kp*Ki/s
sisd = tf([X[0] * X[2], 0], [1]) # tf do controlador diferencial Kd*s
sis2 = crt.parallel(sisp, sisi, sisd) # tf do PID
sis3 = crt.series(sis, sis2) # tf do sistema em série com o PID
mf = feedback(sis3, 1) # malha fechada do sistema
time = np.linspace(0, 20, 100) # tempo de simulação
_, y1 = step_response(mf,
time) # resposta do sistema em malha fechada com PID
erro = sum(abs(np.array((y1 - 1) * time))) # integral do erro
return erro
def unknown_step(Kp=None, Ki=None, Kd=None):
"""========================================================================
Function: unknown_step()
generates a step response of the unkown system used in exercise 3 of
sheet 10. The response could either be openloop or with a PID
controller. For the PID controller you have to specify the Kp, Ki and
Kd gains.
Synopsis:
y,t = unknown_step() # openloop
y,t = unknown_step(Kp,Ki,Kd) # with PID controller
Input Parameter:
- Kp: Proportional gain
- Ki: Integral gain
- Kd: Derivative gain
Output Parameter:
- y: The step response of the system
- t: The time grid of the step response
where y is actually t and vice versa
========================================================================"""
t = np.linspace(0, 50, 100)
# the system is unknown, DONT read it!
sys = 5000 * ctrl.tf([1, 12, 32],
[1, 29, 829, 8939, 79418, 187280, 116000])
sys = delay(sys, 2.1)
# openloop if no K is specified
if Kp == None or Ki == None or Kd == None:
y, t = ctrl.step_response(sys, t)
return y, t
# closedloop PID
else:
PID = ctrl.tf([Kd, Kp, Ki], [1e-10, 1, 0])
# the 1e-10 is a bit nasty but otherwise it cannot be turned into a
# state space model, which is needed for the feedback to work proper.
PIDss = ctrl.ss(PID)
sysPID = ctrl.feedback(ctrl.series(PIDss, sys), 1)
y, t = ctrl.step_response(sysPID, t)
return y, t
def step_response_BF_pert(num1, den1, num2, den2, feedback=1, pert=0.25, pert_time=1, t=None):
if t == None:
t = np.linspace(0, 10, 101)
sys1 = ctl.tf(num1, den1)
sys2 = ctl.tf(num2, den2)
sys_BO1 = ctl.series(sys1, sys2)
sys_BF1 = g = ctl.feedback(sys_BO1, feedback)
lti_BF1 = sys_BF1.returnScipySignalLti()[0][0]
sys_BF2 = g = ctl.feedback(sys2, sys1)
lti_BF2 = sys_BF2.returnScipySignalLti()[0][0]
u = pert * (t > pert_time)
t, s1 = signal.step2(lti_BF1, None, t)
t, s2, trash = signal.lsim2(lti_BF2, u, t)
s = s1 + s2
return t, s
def motor_sim():
# Simulation parameters
setpoint = 100
tmax = 20
step = 100
# Vehicule parameters
m = 1000 # (kg) vehicle mass
b = 50 # (N.s/m) damping coefficient
# PI controller
Kp = 300
Ki = 15
Cpi = control.tf([Kp, Ki], [1, 0])
# Transfer function
mot = control.tf([1], [m, b])
gol = control.series(Cpi, mot)
gcl = control.feedback(gol, 1)
# Time
t = np.linspace(0, tmax, step)
# Input
r = np.empty(len(t))
r.fill(setpoint)
# Output
t, y, _ = control.forced_response(gcl, t, r)
# Plot
plt.figure()
plt.title('Cruise Control Simulation')
plt.axis([0, tmax + 1, 0, setpoint + 1])
plt.grid()
plt.xlabel('Time (s)')
plt.ylabel('Speed (km/h)')
# Animation + Sending message
for i in range(step):
y_int = int(round(y[i]))
msg = can.Message(arbitration_id=100, data=[y_int, 0, 0, 0, 0, 0, 0])
node0.send(msg)
plt.scatter(t[i], y[i])
plt.pause(OPTIONS['update_time'])
plt.show()
def form_PI_cl(data):
A = np.array([[1.0]])
B = np.array([[1.0]])
for i in range(N):
C = np.array([[dt*data['Ki_fit'][i]]])
D = np.array([[data['Kp_fit'][i]]])
pi_block = ss(A, B, C, D)
bike_block = ss(data['A_cl'][i], data['B_cl'][i], data['C_cl'][i], 0)
pc = series(pi_block, bike_block)
cl = feedback(pc, 1, sign=-1)
data['yr_cl_evals'][i] = la.eigvals(cl.A)
assert(np.all(abs(data['yr_cl_evals'][i]) < 1.0))
data['A_yr_cl'][i] = cl.A
data['B_yr_cl'][i] = cl.B
data['C_yr_cl'][i] = cl.C
assert(cl.D == 0)
num, den = ss2tf(cl.A, cl.B, cl.C, cl.D)
data['w_psi_r_to_psi_dot'][i], y = freqz(num[0], den)
data['w_psi_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
data['mag_psi_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
data['phase_psi_r_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi
def attitude_rate_design(G, K_guess, d_tc):
# compensator transfer function
H = np.array([[
control.tf(1, 1),
control.tf((1), (1, 0)),
control.tf((1, 0), (d_tc, 1))
]]).T
H_num = [[H[i][j].num[0][0] for i in range(H.shape[0])]
for j in range(H.shape[1])]
H_den = [[H[i][j].den[0][0] for i in range(H.shape[0])]
for j in range(H.shape[1])]
H = control.tf(H_num, H_den)
ss_open = control.tf2ss(G * H)
print('optimizing controller')
K = lqr_ofb_design(K_guess, ss_open)
print('done')
G_comp = control.series(G, H * K)
Gc_comp = G_comp / (1 + G_comp)
return K, G_comp, Gc_comp
def pidplot(num, den, kp, ki, kd):
G = control.tf(num, den)
if ki != 0:
g2 = control.tf([kd, kp, ki], [1, 0])
else:
g2 = control.tf([kd, kp], [1])
k = control.series(G, g2)
s1 = "("
s2 = "("
s = str(G)
s = s.split('\n')
s[1] = s[1].strip()
s[3] = s[3].strip()
s1 = s1 + s[1] + ')'
s2 = s2 + s[3] + ')'
v = control.feedback(k, 1)
t1, y1 = control.step_response(G)
t2, y2 = control.step_response(v)
plt.plot(t1, y1, 'b', linewidth=1, label='G(s)')
plt.grid(True)
plt.title('Time response of G(s)=' + (s1) + '/' + (s2))
plt.xlabel('Time(sec)')
plt.ylabel('Amplitude')
if not os.path.isdir('static'):
os.mkdir('static')
else:
# Remove old plot files
for filename in glob.glob(
os.path.join('static', 'pidcontrol', 'pidc.png')):
os.remove(filename)
pidc = os.path.join('static', 'pidcontrol', 'pidc.png')
plt.savefig(pidc)
plt.clf()
plt.cla()
plt.close()
return pidc
def form_PI_cl(data):
A = np.array([[1.0]])
B = np.array([[1.0]])
for i in range(N):
C = np.array([[dt * data['Ki_fit'][i]]])
D = np.array([[data['Kp_fit'][i]]])
pi_block = ss(A, B, C, D)
bike_block = ss(data['A_cl'][i], data['B_cl'][i], data['C_cl'][i], 0)
pc = series(pi_block, bike_block)
cl = feedback(pc, 1, sign=-1)
data['yr_cl_evals'][i] = la.eigvals(cl.A)
assert (np.all(abs(data['yr_cl_evals'][i]) < 1.0))
data['A_yr_cl'][i] = cl.A
data['B_yr_cl'][i] = cl.B
data['C_yr_cl'][i] = cl.C
assert (cl.D == 0)
num, den = ss2tf(cl.A, cl.B, cl.C, cl.D)
data['w_psi_r_to_psi_dot'][i], y = freqz(num[0], den)
data['w_psi_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
data['mag_psi_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
data['phase_psi_r_to_psi_dot'][i] = np.unwrap(
np.angle(y)) * 180.0 / np.pi
import control as co
import matplotlib.pyplot as plt
import numpy as np
s = co.tf('s')
a = 1 / (s - 3)
b = (s + 2) / (s - 2)
c = (s + 3) / (s + 4)
e = 1 / (s * ((s**2) + 1))
acp = co.parallel(c, a)
bap = co.parallel(b, a)
acbas = co.series(acp, bap)
acbaef = co.feedback(acbas, e, +1)
print(acbaef)
g_poles = co.pole(acbaef)
print("G poles")
for elem in g_poles:
print(f'{elem: .2f}')
g_zeros = co.zero(acbaef)
print("G zeros")
for elem in g_zeros:
print(f'{elem: .2f}')
g1_map = co.pzmap(acbaef, plot=True)
t = np.linspace(0, 25, 100)
def compute_gains(Q, R, W, V, dt):
"""Given LQR Q and R matrices, and Kalman W and V matrices, and sample
time, compute optimal feedback gain and optimal filter gains."""
data = np.empty((N,), dtype=controller_t)
# Loop over all speeds for which we have system dynamics
for i in range(N):
data['theta_R_dot'][i] = theta_R_dot[i]
data['dt'][i] = dt
# Convert the bike dynamics to discrete time using a zero order hold
data['A'][i], data['B'][i], _, _, _ = cont2discrete(
(A_w[i], B_w[i, :], eye(4), zeros((4, 1))), dt)
data['plant_evals_d'][i] = la.eigvals(data['A'][i])
data['plant_evals_c'][i] = np.log(data['plant_evals_d'][i]) / dt
# Bicycle measurement matrices
# - steer angle
# - roll rate
data['C_m'][i] = C_w[i, :2, :]
# - yaw rate
data['C_z'][i] = C_w[i, 2, :]
A = data['A'][i]
B = data['B'][i, :, 2].reshape((4, 1))
C_m = data['C_m'][i]
C_z = data['C_z'][i]
# Controllability from steer torque
data['ctrb_plant'][i] = ctrb(A, B)
u, s, v = la.svd(data['ctrb_plant'][i])
assert(np.all(s > 1e-13))
# Solve discrete algebraic Ricatti equation associated with LQI problem
P_c = dare(A, B, R, Q)
# Optimal feedback gain using solution of Ricatti equation
K_c = -la.solve(R + dot(B.T, dot(P_c, B)),
dot(B.T, dot(P_c, A)))
data['K_c'][i] = K_c
data['A_c'][i] = A + dot(B, K_c)
data['B_c'][i] = B
data['controller_evals'][i] = la.eigvals(data['A_c'][i])
data['controller_evals_c'][i] = np.log(data['controller_evals'][i]) / dt
assert(np.all(abs(data['controller_evals'][i]) < 1.0))
# Observability from steer angle and roll rate measurement
# Note that (A, C_m * A) must be observable in the "current estimator"
# formulation
data['obsv_plant'][i] = obsv(A, dot(C_m, A))
u, s, v = la.svd(data['obsv_plant'][i])
assert(np.all(s > 1e-13))
# Solve Riccati equation
P_e = dare(A.T, C_m.T, V, W)
# Compute Kalman gain
K_e = dot(P_e, dot(C_m.T, la.inv(dot(C_m, dot(P_e, C_m.T)) + V)))
data['K_e'][i] = K_e
data['A_e'][i] = dot(eye(4) - dot(K_e, C_m), A)
data['B_e'][i] = np.hstack((dot(eye(4) - dot(K_e, C_m), B), K_e))
data['estimator_evals'][i] = la.eigvals(data['A_e'][i])
data['estimator_evals_c'][i] = np.log(data['estimator_evals'][i]) / dt
# Verify that Kalman estimator eigenvalues are stable
assert(np.all(abs(data['estimator_evals'][i]) < 1.0))
# Closed loop state space equations
A_cl = np.zeros((8, 8))
A_cl[:4, :4] = A
A_cl[:4, 4:] = dot(B, K_c)
A_cl[4:, :4] = dot(K_e, dot(C_m, A))
A_cl[4:, 4:] = A - A_cl[4:, :4] + A_cl[:4, 4:]
data['A_cl'][i] = A_cl
data['closed_loop_evals'][i] = la.eigvals(A_cl)
assert(np.all(abs(data['closed_loop_evals'][i]) < 1.0))
B_cl = np.zeros((8, 1))
B_cl[:4, 0] = B.reshape((4,))
B_cl[4:, 0] = dot(eye(4) - dot(K_e, C_m), B).reshape((4,))
data['B_cl'][i] = B_cl
C_cl = np.hstack((C_z, np.zeros((1, 4))))
data['C_cl'][i] = C_cl
# Transfer functions from r to yaw rate
num, den = ss2tf(A_cl, B_cl, C_cl, 0)
data['w_r_to_psi_dot'][i], y = freqz(num[0], den)
data['w_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
data['mag_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
data['phase_r_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi
# Open loop transfer function from e to yaw rate (PI loop not closed,
# but LQR/LQG loop closed.
inner_cl = ss(A_cl, B_cl, C_cl, 0)
pi_block = ss([[1]], [[1]], [[data['Ki_fit'][i]*dt]], [[data['Kp_fit'][i]]])
e_to_psi_dot = series(pi_block, inner_cl)
num, den = ss2tf(e_to_psi_dot.A, e_to_psi_dot.B, e_to_psi_dot.C, e_to_psi_dot.D)
data['w_e_to_psi_dot'][i], y = freqz(num[0], den)
data['w_e_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
data['mag_e_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
data['phase_e_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi
return data
from control.matlab import ss, c2d
from matlab_ext.plotter import *
from matlab_ext.r_ext import *
if __name__ == "__main__":
cls()
Kt = 1.0
Jt = 1.0 # [?]
u1 = tf([Kt], [1])
u2 = tf([1 / Jt], [1])
# fixme: как добавить сигнал шума?
u12 = series(u1, u2)
u3 = tf([1], [1, 0])
u4 = copy.deepcopy(u3)
u34 = series(u3, u4)
# похоже нельзя соединить аналоговую и дискретную
u34_d0 = c2d(u3, Ts=0.5)
u34_d = c2d(u3, Ts=0.5)
print series(u34_d0, u34_d)
u14 = series(u12, u34)
print u14
ts, xs = step_response(u14)
#deriv = ctrl.tf ([Kd, Kp], [1,pd]) #C = ctrl.parallel (integ, deriv) # Modelo del Controlador Kp = 4; Kd = 2; Ki = .1 integ = ctrl.tf ([Ki],[1,0]) deriv = ctrl.tf ([Kd, 0], [0.1,1]) prop = Kp # Interconección del controlador C = ctrl.parallel (deriv + prop, integ) #derivPuro = ctrl.tf ([Kd, Kp], [0,1]) # Serie del Controlador y la Planta l = ctrl.series (C,P) # Realimentación Unitaria sys = ctrl.feedback(l,1) # Respuesta al escalón unitario y,t = ctrl.matlab.step(sys, T = np.arange(0, Ttotal, DT) ) # Gráfica del escalón unitario #mpl.plot(t,y) #mpl.show() #print y.shape, t.shape #mag, phase, omega = bode(C)