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 instantiate (self, specs = None):
if specs is not None:
self.specs = specs;
self.Kc = insert (1, self.specs, 'Kc');
self.Ti = insert (np.inf, self.specs, 'Ti');
self.Td = insert (0, self.specs, 'Td');
self.b = insert (1.0, self.specs, 'b');
self.c = insert (1.0, self.specs, 'c');
self.N = insert (9.0, self.specs, 'N');
num_p = self.Kc * self.b;
den_p = 1;
Cp = ctrl.tf (num_p, den_p);
if (self.Ti != np.inf):
num_i = self.Kc;
den_i = [self.Ti, 0];
Ci = ctrl.tf (num_i, den_i);
else:
Ci = 0;
if (self.Td != 0):
num_d = [self.Kc * self.Td * self.c, 0];
den_d = [(self.Td / self.N), 0];
Cd = ctrl.tf (num_d, den_d);
else:
Cd = 0;
self.tf = Cp + Ci + Cd;
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 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 attitude_sysid(y_acc, u_mix, verbose=False):
"""
roll/pitch system id assuming delay/gain model
:param y_acc: (roll or pitch acceleration)
:param u_mix: (roll or pitch acceleration command)
:return: (G_ol, delay, k)
G_ol: open loop plant
delay: time delay (sec)
k: gain
"""
if verbose:
print('solving for plant model ', end='')
k, delay = delay_and_gain_sysid(y_acc, u_mix, verbose)
if verbose:
print('done')
# open loop, rate output, mix input plant
tf_acc = k*control.tf(*control.pade(delay, 1)) # order 1 approx
# can neglect motor, pole too fast to matter compared to delay, not used in sysid either
tf_integrator = control.tf((1), (1, 0))
G_ol = tf_acc * tf_integrator
return G_ol, delay, k
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 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 test_bode_margin(self):
num = [1000]
den = [1, 25, 100, 0]
sys = ctrl.tf(num, den)
ctrl.bode_plot(sys, margins=True,dB=False,deg = True)
fig = plt.gcf()
allaxes = fig.get_axes()
mag_to_infinity = (np.array([6.07828691, 6.07828691]),
np.array([1.00000000e+00, 1.00000000e-08]))
assert_array_almost_equal(mag_to_infinity, allaxes[0].lines[2].get_data())
gm_to_infinty = (np.array([10., 10.]), np.array([4.00000000e-01, 1.00000000e-08]))
assert_array_almost_equal(gm_to_infinty, allaxes[0].lines[3].get_data())
one_to_gm = (np.array([10., 10.]), np.array([1., 0.4]))
assert_array_almost_equal(one_to_gm, allaxes[0].lines[4].get_data())
pm_to_infinity = (np.array([6.07828691, 6.07828691]),
np.array([100000., -157.46405841]))
assert_array_almost_equal(pm_to_infinity, allaxes[1].lines[2].get_data())
pm_to_phase = (np.array([6.07828691, 6.07828691]), np.array([-157.46405841, -180.]))
assert_array_almost_equal(pm_to_phase, allaxes[1].lines[3].get_data())
phase_to_infinity = (np.array([10., 10.]), np.array([1.00000000e-08, -1.80000000e+02]))
assert_array_almost_equal(phase_to_infinity, allaxes[1].lines[4].get_data())
def test_arguments(self):
# Additional unit tests added on 25 May 2019 to increase coverage
# Unknown canonical forms should generate exception
sys = tf([1], [1, 2, 1])
np.testing.assert_raises(
ControlNotImplemented, canonical_form, sys, 'unknown')
def test_observable_form(self):
"""Test the observable canonical form"""
# Create a system in the observable canonical form
coeffs = [1.0, 2.0, 3.0, 4.0, 1.0]
A_true = np.polynomial.polynomial.polycompanion(coeffs)
A_true = np.fliplr(np.flipud(A_true))
B_true = np.matrix("1.0 1.0 1.0 1.0").T
C_true = np.matrix("1.0 0.0 0.0 0.0")
D_true = 42.0
# Perform a coordinate transform with a random invertible matrix
T_true = np.matrix([[-0.27144004, -0.39933167, 0.75634684, 0.44135471],
[-0.74855725, -0.39136285, -0.18142339, -0.50356997],
[-0.40688007, 0.81416369, 0.38002113, -0.16483334],
[-0.44769516, 0.15654653, -0.50060858, 0.72419146]])
A = np.linalg.solve(T_true, A_true)*T_true
B = np.linalg.solve(T_true, B_true)
C = C_true*T_true
D = D_true
# Create a state space system and convert it to the observable canonical form
sys_check, T_check = canonical_form(ss(A, B, C, D), "observable")
# Check against the true values
np.testing.assert_array_almost_equal(sys_check.A, A_true)
np.testing.assert_array_almost_equal(sys_check.B, B_true)
np.testing.assert_array_almost_equal(sys_check.C, C_true)
np.testing.assert_array_almost_equal(sys_check.D, D_true)
np.testing.assert_array_almost_equal(T_check, T_true)
# Observable form only supports SISO
sys = tf([[ [1], [1] ]], [[ [1, 2, 1], [1, 2, 1] ]])
np.testing.assert_raises(ControlNotImplemented, observable_form, sys)
def test_tf_input_with_int_element_works(self):
num = 1
den = np.convolve([1.0, 2, 1], [1, 1, 1])
sys = ctl.tf(num, den)
self.assertAlmostEqual(1.0, ctl.dcgain(sys))
def test_tf_num_with_numpy_int_element(self):
num = np.convolve([1], [1, 1])
den = np.convolve([1, 2, 1], [1, 1, 1])
sys = ctl.tf(num, den)
self.assertAlmostEqual(1.0, ctl.dcgain(sys))
def test_eval(self):
sys_tf = ct.tf([1], [1, 2, 1])
frd_tf = FRD(sys_tf, np.logspace(-1, 1, 3))
np.testing.assert_almost_equal(sys_tf.evalfr(1), frd_tf.eval(1))
# Should get an error if we evaluate at an unknown frequency
self.assertRaises(ValueError, frd_tf.eval, 2)
def plot_zero_pole_diagram(num,den,title):
try:
sys = control.tf(num,den)
control.matlab.pzmap(sys,True,title)
except:
print('Error al Introducir los Datos, Vuelva a Intentarlo')
def siso_almost_equal(self,g,h):
"""siso_almost_equal(g,h) -> None
Raises AssertionError if g and h, two SISO LTI objects, are not almost equal"""
from control import tf, minreal
gmh = tf(minreal(g-h,verbose=False))
if not (gmh.num[0][0]<self.TOL).all():
maxnum = max(abs(gmh.num[0][0]))
raise AssertionError('systems not approx equal; max num. coeff is {}\nsys 1:\n{}\nsys 2:\n{}'.format(maxnum,g,h))
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 weighting(wb, m, a):
"""weighting(wb,m,a) -> wf
wb - design frequency (where |wf| is approximately 1)
m - high frequency gain of 1/wf; should be > 1
a - low frequency gain of 1/wf; should be < 1
wf - SISO LTI object
"""
s = tf([1, 0], [1])
return (s / m + wb) / (s + wb * a)
def testTf2ssStaticSiso(self):
"""Regression: tf2ss for SISO static gain"""
import control
gsiso = control.tf2ss(control.tf(23, 46))
self.assertEqual(0, gsiso.states)
self.assertEqual(1, gsiso.inputs)
self.assertEqual(1, gsiso.outputs)
# in all cases ratios are exactly representable, so assert_array_equal is fine
np.testing.assert_array_equal([[0.5]], gsiso.D)
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 control_design_ulog(raw_data, do_plot=False, rolling_mean_window=100, verbose=False):
"""
Design a PID controller from log file.
"""
data, dt = setup_data(raw_data)
d_tc = 1.0/125 # nyquist frequency of derivative in PID, (250 Hz/2)
K_guess_att = np.matrix([[1.0]]).T
roll_acc = data.ATT_RollRate.diff()/dt
K_rollrate, G_cl_rollrate = attitude_control_design(
'roll rate', roll_acc, data.ATTC_Roll,
rolling_mean_window=rolling_mean_window,
do_plot=do_plot, verbose=verbose, d_tc=d_tc)
tf_integrator = control.tf((1), (1, 0))
K_roll, G_roll, G_cl_roll = pid_design(
G_cl_rollrate*tf_integrator, K_guess_att, d_tc,
verbose=verbose, use_I=False, use_D=False)
if do_plot:
plt.figure()
plot_loops('roll', G_roll, G_cl_roll)
pitch_acc = data.ATT_PitchRate.diff()/dt
K_pitchrate, G_cl_pitchrate = attitude_control_design(
'pitch rate', pitch_acc, data.ATTC_Pitch,
rolling_mean_window=rolling_mean_window,
do_plot=do_plot, d_tc=d_tc, verbose=verbose)
K_pitch, G_pitch, G_cl_pitch = pid_design(
G_cl_pitchrate*tf_integrator, K_guess_att, d_tc,
verbose=verbose, use_I=False, use_D=False)
if do_plot:
plt.figure()
plot_loops('pitch', G_pitch, G_cl_pitch)
if verbose:
print('G_roll', G_roll)
print('G_cl_roll', G_cl_roll)
print('G_cl_rollrate', G_cl_rollrate)
print('G_pitch', G_pitch)
print('G_cl_pitch', G_cl_pitch)
print('G_cl_pitchrate', G_cl_pitchrate)
return OrderedDict([
('MC_ROLL_P', round(K_roll[0, 0], 3)),
('MC_ROLLRATE_P', round(K_rollrate[0, 0], 3)),
('MC_ROLLRATE_I', round(K_rollrate[1, 0], 3)),
('MC_ROLLRATE_D', round(K_rollrate[2, 0], 3)),
('MC_PITCH_P', round(K_pitch[0, 0], 3)),
('MC_PITCHRATE_P', round(K_pitchrate[0, 0], 3)),
('MC_PITCHRATE_I', round(K_pitchrate[1, 0], 3)),
('MC_PITCHRATE_D', round(K_pitchrate[2, 0], 3)),
]), locals()
def draw_step_response_feedback(K,sys):
# Defining feedback system
C = control.tf(K,1)
# Using feedback according to http://nl.mathworks.com/help/control/examples/using-feedback-to-close-feedback-loops.html
controled_sys = control.feedback(C*sys,1)
poles = control.pole(controled_sys)
y,t = control.step(controled_sys)
plt.plot(t,y)
plt.xlabel('t')
plt.ylabel('y(t)')
def plant():
"""plant() -> g
g - LTI object; 2x2 plant with a RHP zero, at s=0.5.
"""
den = [0.2, 1.2, 1]
gtf = tf([[[1], [1]],
[[2, 1], [2]]],
[[den, den],
[den, den]])
return ss(gtf)
def testTf2ssStaticMimo(self):
"""Regression: tf2ss for MIMO static gain"""
import control
# 2x3 TFM
gmimo = control.tf2ss(control.tf([[ [23], [3], [5] ], [ [-1], [0.125], [101.3] ]],
[[ [46], [0.1], [80] ], [ [2], [-0.1], [1] ]]))
self.assertEqual(0, gmimo.states)
self.assertEqual(3, gmimo.inputs)
self.assertEqual(2, gmimo.outputs)
d = np.matrix([[0.5, 30, 0.0625], [-0.5, -1.25, 101.3]])
np.testing.assert_array_equal(d, gmimo.D)
def ramp_response_BF(num, den, feedback=1, t=None):
if t == None:
t = np.linspace(0, 10, 101)
u = t
# calcul de la BF
sys = ctl.tf(num, den)
sys_BF = g = ctl.feedback(sys, feedback)
lti_BF = sys_BF.returnScipySignalLti()[0][0]
t, s, x = signal.lsim2(lti_BF, u, t)
return t, s
def test_tf2ss_robustness(self):
"""Unit test to make sure that tf2ss is working correctly.
Source: https://github.com/python-control/python-control/issues/240
"""
import control
num = [ [[0], [1]], [[1], [0]] ]
den1 = [ [[1], [1,1]], [[1,4], [1]] ]
sys1tf = control.tf(num, den1)
sys1ss = control.tf2ss(sys1tf)
# slight perturbation
den2 = [ [[1], [1e-10, 1, 1]], [[1,4], [1]] ]
sys2tf = control.tf(num, den2)
sys2ss = control.tf2ss(sys2tf)
# Make sure that the poles match for StateSpace and TransferFunction
np.testing.assert_array_almost_equal(np.sort(sys1tf.pole()),
np.sort(sys1ss.pole()))
np.testing.assert_array_almost_equal(np.sort(sys2tf.pole()),
np.sort(sys2ss.pole()))
def weight_function(option, w_B, M=2, A=0):
"""
This function should return a transfer function object of a weight function
as described in Skogestad pg. 58 (section 2.7.2)
Options:
2.72 roll off of 1 for |S|.
2.73 forces a steeper slope on |S|
5.37 is good for tight control at high frequencies for S.
5.45 is good for T where the roll off of |T| is at least 1 at high frequencies,
|T| is less than M at low frequencies and the cross over frequency is w_B.
Parameters: option => the weight function (enter the equation ref in skogestad)
w_B => minimum bandwidth (not optional)
M => maximum peak of transfer function (optional at M = 2)
A => steady state offset of less than A < 1 (optional at A = 0)
"""
if option == 2.72:
wp = cn.tf([1, M * w_B], [M, M * A * w_B])
elif option == 2.73:
wp = cn.tf([1, 2 * (M ** 0.5) * w_B, M * (w_B ** 2)], [M, w_B * M * 2 * (A ** 0.5), A * M * (w_B**2)])
elif option == 5.37:
wp = cn.tf([1], [M]) + cn.tf([1, 0], [0, w_B])
elif option == 5.45:
wp = cn.tf([1, 0], [w_B]) + cn.tf([1], [M])
return wp
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 testSiso(self):
"mixsyn with SISO system"
from control import tf, augw, hinfsyn, mixsyn
from control import ss
# Skogestad+Postlethwaite, Multivariable Feedback Control, 1st Ed., Example 2.11
s = tf([1, 0], 1)
# plant
g = 200/(10*s+1)/(0.05*s+1)**2
# sensitivity weighting
M = 1.5
wb = 10
A = 1e-4
w1 = (s/M+wb)/(s+wb*A)
# KS weighting
w2 = tf(1, 1)
p = augw(g, w1, w2)
kref, clref, gam, rcond = hinfsyn(p, 1, 1)
ktest, cltest, info = mixsyn(g, w1, w2)
# check similar to S+P's example
np.testing.assert_allclose(gam, 1.37, atol = 1e-2)
# mixsyn is a convenience wrapper around augw and hinfsyn, so
# results will be exactly the same. Given than, use the lazy
# but fragile testing option.
np.testing.assert_allclose(ktest.A, kref.A)
np.testing.assert_allclose(ktest.B, kref.B)
np.testing.assert_allclose(ktest.C, kref.C)
np.testing.assert_allclose(ktest.D, kref.D)
np.testing.assert_allclose(cltest.A, clref.A)
np.testing.assert_allclose(cltest.B, clref.B)
np.testing.assert_allclose(cltest.C, clref.C)
np.testing.assert_allclose(cltest.D, clref.D)
np.testing.assert_allclose(gam, info[0])
np.testing.assert_allclose(rcond, info[1])
def testTf2SsDuplicatePoles(self):
"""Tests for "too few poles for MIMO tf #111" """
import control
try:
import slycot
num = [ [ [1], [0] ],
[ [0], [1] ] ]
den = [ [ [1,0], [1] ],
[ [1], [1,0] ] ]
g = control.tf(num, den)
s = control.ss(g)
np.testing.assert_array_equal(g.pole(), s.pole())
except ImportError:
print("Slycot not present, skipping")
def instantiate (self, specs = None):
if specs is not None:
self.specs = specs;
self.Kc = insert (1, self.specs, 'Kc');
self.Ti = insert (np.inf, self.specs, 'Ti');
self.Td = insert (0, self.specs, 'Td');
self.b = insert (1.0, self.specs, 'b');
self.c = insert (1.0, self.specs, 'c');
self.N = insert (np.inf, self.specs, 'N');
num_p = self.Kc * self.b;
den_p = 1;
Cpff = ctrl.tf (num_p, den_p);
Cpc = ctrl.tf (self.Kc, 1);
if (self.Ti != np.inf):
num_i = self.Kc;
den_i = [self.Ti, 0];
Ci = ctrl.tf (num_i, den_i);
else:
Ci = 0;
if (self.Td != 0):
num_d = [self.Kc * self.Td * self.c, 0];
den_d = [(self.Td / self.N), 1];
Cdff = ctrl.tf (num_d, 1);
Cdc = ctrl.tf ([self.Kc * self.Td], 1);
else:
Cdff = 0;
Cdc = 0;
self.Gff = Cpff + Ci + Cdff;
self.Gc = Cpc + Ci + Cdc;
def add_control_lpf(C, p):
LPF = tf(p, [1, p])
return C * LPF
def test_similarity(self):
"""Test similarty transform"""
# Single input, single output systems
siso_ini = tf2ss(tf([1, 1], [1, 1, 1]))
for form in 'reachable', 'observable':
# Convert the system to one of the canonical forms
siso_can, T_can = canonical_form(siso_ini, form)
# Use a similarity transformation to transform it back
siso_sim = similarity_transform(siso_can, np.linalg.inv(T_can))
# Make sure everything goes back to the original form
np.testing.assert_array_almost_equal(siso_sim.A, siso_ini.A)
np.testing.assert_array_almost_equal(siso_sim.B, siso_ini.B)
np.testing.assert_array_almost_equal(siso_sim.C, siso_ini.C)
np.testing.assert_array_almost_equal(siso_sim.D, siso_ini.D)
# Multi-input, multi-output systems
mimo_ini = ss(
[[-1, 1, 0, 0], [0, -2, 1, 0], [0, 0, -3, 1], [0, 0, 0, -4]],
[[1, 0], [0, 0], [0, 1], [1, 1]],
[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]],
np.zeros((3, 2)))
# Simple transformation: row/col flips + scaling
mimo_txf = np.array(
[[0, 1, 0, 0], [2, 0, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
# Transform the system and transform it back
mimo_sim = similarity_transform(mimo_ini, mimo_txf)
mimo_new = similarity_transform(mimo_sim, np.linalg.inv(mimo_txf))
np.testing.assert_array_almost_equal(mimo_new.A, mimo_ini.A)
np.testing.assert_array_almost_equal(mimo_new.B, mimo_ini.B)
np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C)
np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D)
# Make sure rescaling by identify does nothing
mimo_new = similarity_transform(mimo_ini, np.eye(4))
np.testing.assert_array_almost_equal(mimo_new.A, mimo_ini.A)
np.testing.assert_array_almost_equal(mimo_new.B, mimo_ini.B)
np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C)
np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D)
# Time rescaling
mimo_tim = similarity_transform(mimo_ini, np.eye(4), timescale=0.3)
mimo_new = similarity_transform(mimo_tim, np.eye(4), timescale=1/0.3)
np.testing.assert_array_almost_equal(mimo_new.A, mimo_ini.A)
np.testing.assert_array_almost_equal(mimo_new.B, mimo_ini.B)
np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C)
np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D)
# Time + transformation, in one step
mimo_sim = similarity_transform(mimo_ini, mimo_txf, timescale=0.3)
mimo_new = similarity_transform(mimo_sim, np.linalg.inv(mimo_txf),
timescale=1/0.3)
np.testing.assert_array_almost_equal(mimo_new.A, mimo_ini.A)
np.testing.assert_array_almost_equal(mimo_new.B, mimo_ini.B)
np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C)
np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D)
# Time + transformation, in two steps
mimo_sim = similarity_transform(mimo_ini, mimo_txf, timescale=0.3)
mimo_tim = similarity_transform(mimo_sim, np.eye(4), timescale=1/0.3)
mimo_new = similarity_transform(mimo_tim, np.linalg.inv(mimo_txf))
np.testing.assert_array_almost_equal(mimo_new.A, mimo_ini.A)
np.testing.assert_array_almost_equal(mimo_new.B, mimo_ini.B)
np.testing.assert_array_almost_equal(mimo_new.C, mimo_ini.C)
np.testing.assert_array_almost_equal(mimo_new.D, mimo_ini.D)
# Inverted Pendulum Parameter File
import sys
sys.path.append('..') # add parent directory
import VTOLParam as P
sys.path.append('../hw7') # add parent directory
import VTOLParamHW7 as P7
import numpy as np
# from scipy import signal
from control import TransferFunction as tf
import matplotlib.pyplot as plt
import control as cnt
# Compute plant transfer functions
th_e = 0
Plant_long = tf([1.0/(P.mc + 2*P.mr)],
[1, 0.0, 0.0])
Plant_lat_inner = tf([(-P.g)],[1, (P.mu/(P.mc+2*P.mr)), 0])
Plant_lat_outer = tf([(1/(P.Jc + 2*P.mr*P.d**2))], [1, 0, 0])
# Compute transfer function of controller
C_pid_long = tf([(P7.kd_h+P7.kp_h*P.sigma), (P7.kp_h+P7.ki_h*P.sigma), P7.ki_h],
[P.sigma, 1, 0])
C_pid_lat_inner = tf([(P7.kd_th+P7.kp_th*P.sigma), P7.kp_th, 0],
[P.sigma, 1, 0])
C_pid_lat_outer = tf([(P7.kd_z+P7.kp_z*P.sigma), (P7.kp_z+P7.ki_z*P.sigma), P7.ki_z],
[P.sigma, 1, 0])
# display bode plots of transfer functions
cnt.bode(Plant_long, dB=False, margins=False)
cnt.bode(Plant_long*C_pid_long, dB=False, margins=False)
import control as cn import matplotlib.pyplot as plt import numpy as np import control_add_on as cadd import siso_controllability as scont """ Scaling: x* refers to the unscaled perturbation variable q = q*/1 T_0 = T_0*/10 T = T*/10 """ G = cn.tf([0.8],[60, 1])*cn.tf([1],[12, 1]) #this is the only tf which changes due to scaling => equates to *0.1 Gd = cn.tf([20*0.6, 0.6],[60, 1])*cn.tf([1],[12,1]) # remember the dead time in measurement which is 3 seconds freqs = np.arange(0.001, 1, 0.001) plt.figure(1) scont.rule_three_four(G, Gd, R = 2, perfect = True, freq = freqs) plt.show() """ Performance: The plant will perform well to input (R) and disturbance tracking as it is self regulating i.e. |Gd| < 1 for all frequencies. Controllability: It is self regulating so... this is awkward... """
def add_control_proportional(C, kp):
proportional = tf([kp], [1])
return C * proportional
# import matplotlib
import control
import tensorflow as tf
from RM import ReplayMemory
# matplotlib.use("TkAgg")
# import matplotlib.pyplot as plt
steps = 1000
episodes = 500
y_set = np.random.uniform(0, 1)
eps = 0.01 # Tolerance του reward function
c = 100 # reward value
# Το περιβάλλον του προβλήματος
g = control.tf([0.05, 0], [-0.6, 1])
sys = control.tf2ss(g)
number_of_states = 2
number_of_actions = 1
def reward(state):
if abs(state - y_set) < eps:
return c
else:
return -np.power(abs(state - y_set), 2)
def output(system, T, U, init_cond):
return control.forced_response(system, T, U, init_cond)[1][49]
#!/usr/bin/env python
from matplotlib.pyplot import plot, legend, savefig, gcf
from numpy import logspace, sqrt, log
from control import bode, tf
k = 3
G = tf([k], [1])
om = logspace(-2, 1, 500)
bode(G, dB=True, omega=om)
f = gcf()
r, mag, fas = f.get_children()
mag.set_yticks([20, 8.75 * log(k), 0, -20])
fas.set_yticks([90, 0, -90])
mag.set_xticklabels([])
mag.set_yticklabels(['20.00', r'$k_{dB}$', '0.00', '-20.00'])
fas.set_xticklabels([])
#a.set_yticklabels([])
# Se guarda la figura en la misma carpeta
savefig("bodeordencero.pdf",
bbox_inches='tight',
pad_inches=0,
transparent="True")
def set_plant(self, **kwargs):
self.num = kwargs['num']
self.den = kwargs['den']
self.tf_plant = control.tf(self.num, self.den)
@author: St Elmo Wilken ''' import control as cn import matplotlib.pyplot as plt import numpy as np import siso_controllability as scont # Scaling: x* refers to the unscaled perturbation variable # q = q*/1 # T_0 = T_0*/10 # T = T*/10 # this is the only tf which changes due to scaling => equates to *0.1 G = cn.tf([0.8], [60, 1]) * cn.tf([1], [12, 1]) Gd = cn.tf([20 * 0.6, 0.6], [60, 1]) * cn.tf([1], [12, 1]) # remember the dead time in measurement which is 3 seconds freqs = np.arange(0.001, 1, 0.001) plt.figure(1) scont.rule_three_four(G, Gd, R=2, perfect=True, freq=freqs) plt.show() # Performance: # The plant will perform well to input (R) and disturbance tracking as it # is self regulating i.e. |Gd| < 1 for all frequencies. # # Controllability: # It is self regulating so... this is awkward...
plantWn11 = 3 * hz2rps
plantD11 = 0.2
plantK12 = 0.5
plantWn12 = 5 * hz2rps
plantD12 = 0.3
plantK21 = 0.25
plantWn21 = 4 * hz2rps
plantD21 = 0.1
plantK22 = 1.0
plantWn22 = 6 * hz2rps
plantD22 = 0.4
sysPlant = control.tf(
[[[0, 0, plantK11 * plantWn11**2], [0, 0, plantK21 * plantWn21**2]],
[[0, 0, plantK12 * plantWn12**2], [0, 0, plantK22 * plantWn22**2]]],
[[[1, 2.0 * plantD11 * plantWn11, plantWn11**2],
[1, 2.0 * plantD21 * plantWn21, plantWn21**2]],
[[1, 2.0 * plantD12 * plantWn12, plantWn12**2],
[1, 2.0 * plantD22 * plantWn22, plantWn22**2]]])
sysK = control.ss([], [], [], [[0.75, 0.25], [0.25, 0.5]])
elif False: # SP Example #3.8
freqLin_hz = np.linspace(1e-2 * rps2hz, 1e2 * rps2hz, 200)
freqRate_hz = 50
plantK11 = 1.0
plantWn11 = np.sqrt(5)
plantD11 = 6 / (2 * plantWn11)
plantK12 = 1.0
plantWn12 = 1.0 * plantWn11
import control
import matplotlib.pyplot as plt
import numpy as np
#if using termux
import subprocess
import shlex
#end if
#coefficents of open loop transfer function
den = [4, 5, 1, 0]
num1 = [1.25]
num2 = [2]
num3 = [1]
#Creating a transfer function G = num/den
G1 = control.tf(num1, den)
G2 = control.tf(num2, den)
G3 = control.tf(num3, den)
#plotting the nyquist plot for three different transfer functions for a variable K
control.nyquist(G1, label='K=1.25')
control.nyquist(G2, label='K=2')
control.nyquist(G3, label='K=1')
plt.xlim(-4, 0)
plt.ylim(-2, 2)
plt.legend()
plt.annotate("-1+0j", (-1, 0))
plt.xlabel('Re(s)')
plt.ylabel('Im(s)')
#if using termux
for filename in filenames:
df = pd.read_csv(filename, index_col=None, header=0)
df = np.array(df)
df = np.concatenate(df)
dfs.append(df)
return dfs
dfs = read_profiles()
tinitial = 0
tfinal = 59
N = len(dfs[0])
print(N)
times = np.linspace(tinitial, tfinal, N)
s = tf([1, 0], [0, 1])
Gp = 1 / (s**2 + s + 1)
Gd = (s + 1) / (s**2 + s + 1)
def calc_average_perf(s, Gp, Gd, Kc, tauI, tauD, tauC, times):
Perf = np.linspace(0, 1, 100)
Gc = Kc * (1 + 1 / (tauI * s) + tauD * s / (tauC * s + 1))
sys_D = Gd / (1 + Gp * Gc)
average_Perf = []
for j in range(0, 100):
Ti = dfs[j]
_, Y, _ = control.forced_response(sys_D, times, Ti)
sys_U = Gc
_, U, _ = control.forced_response(sys_U, times, -Y)
Perf[j] = sum(abs(Y)) + 0.2 * sum(abs(U))
# Single link arm Parameter File
import sys
sys.path.append('..') # add parent directory
import massParam as P
import numpy as np
import control as cnt
from control import TransferFunction as tf
import matplotlib.pyplot as plt
# from mpldatacursor import datacursor
# Compute plant transfer functions
th_e = 0
Plant = tf([1.0 / P.m], [1, P.b / P.m, P.k / P.m])
# Bode plot of the plant
plt.figure(3), cnt.bode_plot(Plant, dB=False, margins=False)
# if you want specific values at specific frequencies, you can do the following:
mag, phase, omega = cnt.bode(Plant,
plot=False,
dB=False,
omega=[0.3, 10.0, 100.0])
# Closes plot windows when the user presses a button.
plt.pause(0.0001) # not sure why this is needed for both figures to display
print('Close window to end program')
plt.show()
def test_modal_form_MIMO(self):
"""Test error because modal form only supports SISO"""
sys = tf([[[1], [1]]], [[[1, 2, 1], [1, 2, 1]]])
with pytest.raises(ControlNotImplemented):
modal_form(sys)
def add_control_lag(C, z, M):
Lag = tf([1, z], [1, z/M])
return C * Lag
import control as co
import matplotlib.pyplot as plt
import numpy as np
s = co.tf('s')
g = (s**2 + 5 * s + 4) / (s**2 - 5 * s - 24)
"""a) οχι ευσταθές"""
print(co.pole(g))
co.root_locus(g, ylim=(-0.25, 0.25))
"""b) απο root_locus μια καλή
τιμή για ευστάθεια είναι k=8"""
g_gained = co.feedback(8 * g, sign=-1)
plt.figure(2)
#co.root_locus(g_gained, ylim=(-0.025, 0.025))
"""c) """
print(g_gained)
co.pzmap(g_gained)
plt.show()
def add_control_integral(C, ki):
integrator = tf([1, ki], [1, 0])
return C * integrator
def test_modal_form(self):
"""Test the modal canonical form"""
# Create a system in the modal canonical form
A_true = np.diag([4.0, 3.0, 2.0,
1.0]) # order from the largest to the smallest
B_true = np.matrix("1.1 2.2 3.3 4.4").T
C_true = np.matrix("1.3 1.4 1.5 1.6")
D_true = 42.0
# Perform a coordinate transform with a random invertible matrix
T_true = np.matrix(
[[-0.27144004, -0.39933167, 0.75634684, 0.44135471],
[-0.74855725, -0.39136285, -0.18142339, -0.50356997],
[-0.40688007, 0.81416369, 0.38002113, -0.16483334],
[-0.44769516, 0.15654653, -0.50060858, 0.72419146]])
A = np.linalg.solve(T_true, A_true) * T_true
B = np.linalg.solve(T_true, B_true)
C = C_true * T_true
D = D_true
# Create a state space system and convert it to the modal canonical form
sys_check, T_check = canonical_form(ss(A, B, C, D), "modal")
# Check against the true values
# TODO: Test in respect to ambiguous transformation (system characteristics?)
np.testing.assert_array_almost_equal(sys_check.A, A_true)
#np.testing.assert_array_almost_equal(sys_check.B, B_true)
#np.testing.assert_array_almost_equal(sys_check.C, C_true)
np.testing.assert_array_almost_equal(sys_check.D, D_true)
#np.testing.assert_array_almost_equal(T_check, T_true)
# Check conversion when there are complex eigenvalues
A_true = np.array([[-1, 1, 0, 0], [-1, -1, 0, 0], [0, 0, -2, 0],
[0, 0, 0, -3]])
B_true = np.array([[0], [1], [0], [1]])
C_true = np.array([[1, 0, 0, 1]])
D_true = np.array([[0]])
A = np.linalg.solve(T_true, A_true) * T_true
B = np.linalg.solve(T_true, B_true)
C = C_true * T_true
D = D_true
# Create state space system and convert to modal canonical form
sys_check, T_check = canonical_form(ss(A, B, C, D), 'modal')
# Check A and D matrix, which are uniquely defined
np.testing.assert_array_almost_equal(sys_check.A, A_true)
np.testing.assert_array_almost_equal(sys_check.D, D_true)
# B matrix should be all ones (or zero if not controllable)
# TODO: need to update modal_form() to implement this
if np.allclose(T_check, T_true):
np.testing.assert_array_almost_equal(sys_check.B, B_true)
np.testing.assert_array_almost_equal(sys_check.C, C_true)
# Make sure Hankel coefficients are OK
from numpy.linalg import matrix_power
for i in range(A.shape[0]):
np.testing.assert_almost_equal(
np.dot(np.dot(C_true, matrix_power(A_true, i)), B_true),
np.dot(np.dot(C, matrix_power(A, i)), B))
# Reorder rows to get complete coverage (real eigenvalue cxrtvfirst)
A_true = np.array([[-1, 0, 0, 0], [0, -2, 1, 0], [0, -1, -2, 0],
[0, 0, 0, -3]])
B_true = np.array([[0], [0], [1], [1]])
C_true = np.array([[0, 1, 0, 1]])
D_true = np.array([[0]])
A = np.linalg.solve(T_true, A_true) * T_true
B = np.linalg.solve(T_true, B_true)
C = C_true * T_true
D = D_true
# Create state space system and convert to modal canonical form
sys_check, T_check = canonical_form(ss(A, B, C, D), 'modal')
# Check A and D matrix, which are uniquely defined
np.testing.assert_array_almost_equal(sys_check.A, A_true)
np.testing.assert_array_almost_equal(sys_check.D, D_true)
# B matrix should be all ones (or zero if not controllable)
# TODO: need to update modal_form() to implement this
if np.allclose(T_check, T_true):
np.testing.assert_array_almost_equal(sys_check.B, B_true)
np.testing.assert_array_almost_equal(sys_check.C, C_true)
# Make sure Hankel coefficients are OK
from numpy.linalg import matrix_power
for i in range(A.shape[0]):
np.testing.assert_almost_equal(
np.dot(np.dot(C_true, matrix_power(A_true, i)), B_true),
np.dot(np.dot(C, matrix_power(A, i)), B))
# Modal form only supports SISO
sys = tf([[[1], [1]]], [[[1, 2, 1], [1, 2, 1]]])
np.testing.assert_raises(ControlNotImplemented, modal_form, sys)
import control as co
import numpy as np
import matplotlib.pyplot as plt
import sympy as sp
num = [-25.95]
den = [1, 7.25, 9.77]
sys = co.tf(num, den)
co.rlocus(sys)
plt.title('K->+inf')
plt.figure(2)
sys = co.tf(num, den)
co.rlocus(-sys)
plt.title('K->-inf')
plt.grid()
plt.show()
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)
#Paramentros da Planta
K = 2
T = 4
#Função Transferência da Planta
G = tf(K, [T, 1])
#Criando vetor de tempo para ser o mesmo em todas as análises
tdiv = 201
time = np.linspace(0, 6, tdiv)
#Fator indesejado, Sinal de controle >max
maxcontrolador = 3
Kpi = 0
Kii = 0
Kdi = 0
faixa = 5 * 100
# Faixa de valores para as variáveis do GA
varbound = np.array([[-Kpi, faixa - Kpi], [-Kii, faixa - Kii],
[-Kdi, faixa - Kdi]])
# p: lpf cutoff frequency
def add_control_lpf(C, p):
LPF = tf(p, [1, p])
return C * LPF
# phase lead: increase PM (stability)
# w_L: location of maximum frequency bump
# M: separation between zero and pole
def add_control_lead(C, w_L, M):
gain = (1.0+np.sqrt(M))/(1.0+1.0/np.sqrt(M))
Lead = tf([gain * 1.0, gain * w_L / np.sqrt(M)],
[1.0, w_L * np.sqrt(M)])
return C * Lead
# Compute plant transfer functions
Plant = cnt.tf([1.0 / (P.m * P.ell**2)], [1, P.b/(P.m*P.ell**2), P.k1/(P.m*P.ell**2)])
C_pid = cnt.tf([(P.kd+P.kp*P.sigma), (P.kp+P.ki*P.sigma), P.ki], [P.sigma, 1, 0])
PLOT = True
# PLOT = False
# calculate bode plot and gain and phase margin
mag, phase, omega = cnt.bode(Plant, dB=True, omega=np.logspace(-3, 5), Plot=False)
gm, pm, Wcg, Wcp = cnt.margin(Plant*C_pid)
print(" pm: ", pm, " Wcp: ", Wcp, "gm: ", gm, " Wcg: ", Wcg)
if PLOT:
plt.figure(3), plt.clf()
plt.subplot(211), plt.grid(True)
plantMagPlot, = plt.semilogx(omega, 20*np.log10(mag), label='Plant')
plt.subplot(212), plt.grid(True)
def simulate_pendulum_MPC(sim_options):
seed_val = get_parameter(sim_options, 'seed_val')
if seed_val is not None:
np.random.seed(seed_val)
Ac = get_parameter(sim_options, 'Ac')
Bc = get_parameter(sim_options, 'Bc')
Cc = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.]])
Dc = np.zeros((2, 1))
[nx, nu] = Bc.shape # number of states and number or inputs
ny = np.shape(Cc)[0]
Ts_slower_loop = get_parameter(sim_options, 'Ts_slower_loop')
ratio_Ts = int(Ts_slower_loop // Ts_PID)
# Brutal forward euler discretization
Ad = np.eye(nx) + Ac * Ts_slower_loop
Bd = Bc * Ts_slower_loop
Cd = Cc
Dd = Dc
# Standard deviation of the measurement noise on position and angle
std_npos = get_parameter(sim_options, 'std_npos')
std_nphi = get_parameter(sim_options, 'std_nphi')
# Force disturbance
std_dF = get_parameter(sim_options, 'std_dF')
# Disturbance power spectrum
w_F = get_parameter(sim_options,
'w_F') # bandwidth of the force disturbance
tau_F = 1 / w_F
Hu = control.TransferFunction([1], [1 / w_F, 1])
Hu = Hu * Hu
Hud = control.matlab.c2d(Hu, Ts_PID)
N_sim_imp = tau_F / Ts_PID * 20
t_imp = np.arange(N_sim_imp) * Ts_PID
t, y = control.impulse_response(Hud, t_imp)
y = y[0]
std_tmp = np.sqrt(np.sum(y**2)) # np.sqrt(trapz(y**2,t))
Hu = Hu / (std_tmp) * std_dF
N_skip = int(20 * tau_F //
Ts_PID) # skip initial samples to get a regime sample of d
t_sim_d = get_parameter(sim_options, 'len_sim') # simulation length (s)
N_sim_d = int(t_sim_d // Ts_PID)
N_sim_d = N_sim_d + N_skip + 1
e = np.random.randn(N_sim_d)
te = np.arange(N_sim_d) * Ts_PID
_, d, _ = control.forced_response(Hu, te, e)
d = d.ravel()
angle_ref = d[N_skip:]
# Initialize simulation system
t0 = 0
phi0 = 0.0 * 2 * np.pi / 360
x0 = np.array([0, 0, phi0, 0]) # initial state
system_dyn = ode(f_ODE_wrapped).set_integrator('vode',
method='bdf') # dopri5
# system_dyn = ode(f_ODE_wrapped).set_integrator('dopri5')
system_dyn.set_initial_value(x0, t0)
system_dyn.set_f_params(0.0)
# Default controller parameters -
P = -100.0
I = -1
D = -10
N = 100.0
kP = control.tf(P, 1, Ts_PID)
kI = I * Ts_PID * control.tf([0, 1], [1, -1], Ts_PID)
kD = D * control.tf([N, -N], [1.0, Ts_PID * N - 1], Ts_PID)
PID_tf = kP + kD + kI
PID_ss = control.ss(PID_tf)
k_PID = LinearStateSpaceSystem(A=PID_ss.A,
B=PID_ss.B,
C=PID_ss.C,
D=PID_ss.D)
# Simulate in closed loop
len_sim = get_parameter(sim_options, 'len_sim') # simulation length (s)
nsim = int(
len_sim // Ts_slower_loop
) #int(np.ceil(len_sim / Ts_slower_loop)) # simulation length(timesteps) # watch out! +1 added, is it correct?
t_vec = np.zeros((nsim, 1))
t_calc_vec = np.zeros(
(nsim, 1)) # computational time to get MPC solution (+ estimator)
status_vec = np.zeros((nsim, 1))
x_vec = np.zeros((nsim, nx))
x_ref_vec = np.zeros((nsim, nx))
y_vec = np.zeros((nsim, ny))
y_meas_vec = np.zeros((nsim, ny))
u_vec = np.zeros((nsim, nu))
nsim_fast = int(len_sim // Ts_PID)
t_vec_fast = np.zeros((nsim_fast, 1))
x_vec_fast = np.zeros(
(nsim_fast, nx)) # finer integration grid for performance evaluation
ref_angle_vec_fast = np.zeros((nsim_fast, 1))
y_meas_vec_fast = np.zeros((nsim_fast, ny))
x_ref_vec_fast = np.zeros(
(nsim_fast, nx)) # finer integration grid for performance evaluatio
u_vec_fast = np.zeros(
(nsim_fast, nu)) # finer integration grid for performance evaluatio
Fd_vec_fast = np.zeros((nsim_fast, nu)) #
t_int_vec_fast = np.zeros((nsim_fast, 1))
emergency_vec_fast = np.zeros((nsim_fast, 1)) #
t_step = t0
x_step = x0
u_PID = None
for idx_fast in range(nsim_fast):
## Determine step type: fast simulation only or MPC step
idx_inner_controller = idx_fast // ratio_Ts
run_inner_controller = (idx_fast % ratio_Ts) == 0
y_step = Cd.dot(x_step) # y[i] from the system
ymeas_step = np.copy(y_step)
ymeas_step[0] += std_npos * np.random.randn()
ymeas_step[1] += std_nphi * np.random.randn()
y_meas_vec_fast[idx_fast, :] = ymeas_step
# Output for step i
# Ts_slower_loop outputs
if run_inner_controller: # it is also a step of the simulation at rate Ts_slower_loop
if idx_inner_controller < nsim:
t_vec[idx_inner_controller, :] = t_step
y_vec[idx_inner_controller, :] = y_step
y_meas_vec[idx_inner_controller, :] = ymeas_step
u_vec[idx_inner_controller, :] = u_PID
# PID angle CONTROLLER
ref_angle = angle_ref[idx_fast]
error_angle = ref_angle - ymeas_step[1]
u_PID = k_PID.output(error_angle)
u_PID[u_PID > 10.0] = 10.0
u_PID[u_PID < -10.0] = -10.0
u_TOT = u_PID
# Ts_fast outputs
t_vec_fast[idx_fast, :] = t_step
x_vec_fast[idx_fast, :] = x_step #system_dyn.y
u_vec_fast[idx_fast, :] = u_TOT
Fd_vec_fast[idx_fast, :] = 0.0
ref_angle_vec_fast[idx_fast, :] = ref_angle
## Update to step i+1
k_PID.update(error_angle)
# Controller simulation step at rate Ts_slower_loop
if run_inner_controller:
pass
# System simulation step at rate Ts_fast
time_integrate_start = time.perf_counter()
system_dyn.set_f_params(u_TOT)
system_dyn.integrate(t_step + Ts_PID)
x_step = system_dyn.y
#x_step = x_step + f_ODE_jit(t_step, x_step, u_TOT)*Ts_fast
#x_step = x_step + f_ODE(0.0, x_step, u_TOT) * Ts_fast
t_int_vec_fast[
idx_fast, :] = time.perf_counter() - time_integrate_start
# Time update
t_step += Ts_PID
simout = {
't': t_vec,
'x': x_vec,
'u': u_vec,
'y': y_vec,
'y_meas': y_meas_vec,
'x_ref': x_ref_vec,
'status': status_vec,
'Fd_fast': Fd_vec_fast,
't_fast': t_vec_fast,
'x_fast': x_vec_fast,
'x_ref_fast': x_ref_vec_fast,
'u_fast': u_vec_fast,
'y_meas_fast': y_meas_vec_fast,
'emergency_fast': emergency_vec_fast,
'K': k_PID,
'nsim': nsim,
'Ts_slower_loop': Ts_slower_loop,
't_calc': t_calc_vec,
'ref_angle_fast': ref_angle_vec_fast,
't_int_fast': t_int_vec_fast
}
return simout
Created on Mon Feb 22 21:22:49 2016
@author: Charles
"""
from __future__ import division
import numpy as np
from control import tf, ss
from matplotlib import pyplot as plot
import scipy
# Exact Process Model
K = 2
tau = 5
numer = [K]
denom = [tau,1]
sys1 = tf(numer,denom)
#Transformation to State-Space
sys = ss(sys1)
A, B, C, D = np.asarray(sys.A), np.asarray(sys.B), np.asarray(sys.C), \
np.asarray(sys.D)
Nstates = A.shape[0]
z = np.zeros((Nstates, 1))
t_start = 0
t_end = 100
dt = 0.01
tspan = np.arange(t_start, t_end, dt)
npoints = len(tspan)
def add_control_lead(C, w_L, M):
gain = (1.0+np.sqrt(M))/(1.0+1.0/np.sqrt(M))
Lead = tf([gain * 1.0, gain * w_L / np.sqrt(M)],
[1.0, w_L * np.sqrt(M)])
return C * Lead
def test_dcgain(self):
sys = tf(84, [1, 2])
np.testing.assert_allclose(sys.dcgain(), 42)
np.testing.assert_allclose(dcgain(sys), 42)
def test_observable_form_MIMO(self):
"""Test error as Observable form only supports SISO"""
sys = tf([[[1], [1] ]], [[[1, 2, 1], [1, 2, 1]]])
with pytest.raises(ControlNotImplemented):
observable_form(sys)
def system(K_P):
num = [10]
den = [10, 25, 11, 2]
G = ctl.tf(num, den)
G_0 = K_P * G
return ctl.feedback(G_0)
if e_mode == "two_added_sines":
e = 1 * np.sin(t + np.pi / 4) + 2 * np.sin(0.25 * t + 0)
# ################# initialize controllers
T_s = 0.5 # T_sample
K_p = 2
K_I = 0.5
K_D = 2
K_N = 0.5
# P-controller
# apparently the control toolbox cant simulate this simple system due to some strange bug
# PI-controller
pi_controller = ctrl.tf([K_p + K_I * T_s / 2, K_I * T_s / 2 - K_p], [1, -1],
T_s)
# PID-controller
a1 = K_p + K_I * T_s / 2
a0 = K_I * T_s / 2 - K_p
num = [
a1 + K_D * K_N, -a1 + K_N * T_s * a1 + a0 - 2 * K_D * K_N,
-a0 + K_N * T_s * a0 + K_D * K_N
]
den = [1, K_N * T_s - 2, -K_N * T_s + 1]
pid_controller = ctrl.tf(num, den, T_s)
print("PI-controller: " + str(pi_controller))
print("PID-controller: " + str(pid_controller))
# ################# simulate systems
""" Spyder Editor This is a temporary script file. """ import control import carla from scipy import signal import matplotlib.pyplot as plt import numpy as np from collections import deque num = [1.0] den = [1., 1., 0.5] sys1 = control.tf(num, den, 0.002) t = np.linspace(0, 1, 500, endpoint=False) #sig = np.sin(2 * np.pi * t) pwm = signal.square(2 * np.pi * 5 * t) num_low_pass = [1.0] den_low_pass = [1.0, 10.0] low_pass_c = control.tf(num_low_pass, den_low_pass) low_pass_d = control.sample_system(low_pass_c, 0.002) sys = low_pass_d sys = control.tf2ss(sys) T, yout, xout = control.forced_response(sys, U=pwm, X0=0)
import numpy as np
import matplotlib.pyplot as plt
import control
from scipy import signal
#if using termux
import subprocess
import shlex
#end if
k = 96
num = [k]
den = [1, 12, 44, 48, 0]
sys = control.tf(num, den)
gm, pm, wg, wp = control.margin(sys)
s = signal.lti(num, den)
w, mag, phase = signal.bode(s)
gain_y = np.full((len(w)), -4.81)
phase_y = np.full((len(w)), -180)
idx = np.argwhere(np.diff(np.sign(mag - gain_y))).flatten()
print("The Freqency of intersection:", w[idx])
plt.figure()
plt.semilogx(w, mag)
plt.plot(w, gain_y)
plt.plot(2.06, -4.81, 'ro')
plt.ylabel("Gain")
plt.xlabel("Frequency")
plt.grid()
