def test_squeeze(self, fcn, nstate, nout, ninp, omega, squeeze, shape,
omega_type):
"""Test correct behavior of frequencey response squeeze parameter."""
# Create the system to be tested
if fcn == ct.frd:
sys = fcn(ct.rss(nstate, nout, ninp), [1e-2, 1e-1, 1, 1e1, 1e2])
elif fcn == ct.tf and (nout > 1 or ninp > 1) and not slycot_check():
pytest.skip("Conversion of MIMO systems to transfer functions "
"requires slycot.")
else:
sys = fcn(ct.rss(nstate, nout, ninp))
if omega_type == "numpy":
omega = np.asarray(omega)
isscalar = omega.ndim == 0
# keep the ndarray type even for scalars
s = np.asarray(omega * 1j)
else:
isscalar = not hasattr(omega, '__len__')
if isscalar:
s = omega * 1J
else:
s = [w * 1J for w in omega]
# Call the transfer function directly and make sure shape is correct
assert sys(s, squeeze=squeeze).shape == shape
# Make sure that evalfr also works as expected
assert ct.evalfr(sys, s, squeeze=squeeze).shape == shape
# Check frequency response
mag, phase, _ = sys.frequency_response(omega, squeeze=squeeze)
if isscalar and squeeze is not True:
# sys.frequency_response() expects a list as an argument
# Add the shape of the input to the expected shape
assert mag.shape == shape + (1, )
assert phase.shape == shape + (1, )
else:
assert mag.shape == shape
assert phase.shape == shape
# Make sure the default shape lines up with squeeze=None case
if squeeze is None:
assert sys(s).shape == shape
# Changing config.default to False should return 3D frequency response
ct.config.set_defaults('control', squeeze_frequency_response=False)
mag, phase, _ = sys.frequency_response(omega)
if isscalar:
assert mag.shape == (sys.noutputs, sys.ninputs, 1)
assert phase.shape == (sys.noutputs, sys.ninputs, 1)
assert sys(s).shape == (sys.noutputs, sys.ninputs)
assert ct.evalfr(sys, s).shape == (sys.noutputs, sys.ninputs)
else:
assert mag.shape == (sys.noutputs, sys.ninputs, len(omega))
assert phase.shape == (sys.noutputs, sys.ninputs, len(omega))
assert sys(s).shape == \
(sys.noutputs, sys.ninputs, len(omega))
assert ct.evalfr(sys, s).shape == \
(sys.noutputs, sys.ninputs, len(omega))
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 fpeak(siso):
"""Estimates the frequency where the transfer function achieves it's
peak magnitude.
Parameters
----------
siso: (tf object) siso transfer function
Returns
-------
theta: (real number) The number theta such that |G(exp(i*theta))|
is maximized (discrete time)
or
jomega: (imaginary number) The number jomega such that |G(jomega)|
is maximized (continuous time).
"""
dt = siso.dt
#CONTINUOUS TIME
if dt is None:
jomega = np.zeros(100,complex)
jomega.imag = np.linspace(0,2*np.pi,100)
gain = np.apply_along_axis(lambda x : ctrl.evalfr(siso,x),0,jomega)
gain = (gain*gain.conjugate())**.5
return jomega[np.argmax(gain)]
#DISCRETE TIME
else:
theta = np.linspace(0,2*np.pi,100)
gain = np.apply_along_axis(lambda x : ctrl.evalfr(siso,np.exp(x*1j)),0,theta)
gain = (gain*gain.conjugate())**.5
return theta[np.argmax(gain)]
def testLinkPer(N=300):
theta = np.array([1]*100,complex)
theta.imag = np.linspace(0,2*np.pi,100)
theta = np.exp(theta)
gainP = [0]*100
gainG = [0]*100
Success = [0]*6
Y1 = []
Y2 = []
X = []
for n in range(6): X = X + [n]*N
for n in range(6):
count = 0
y1 = [0]*N
y2 = [0]*N
#Generate random transfer functions of size N
for i in range(N):
a = .3*np.random.rand()
b = .3*np.random.rand()
c = .3*np.random.rand()
G = tf.tf([1.,c],[1.,-1.*complex(a,b)])
for k in range(n):
a = .5*np.random.rand()
b = .5*np.random.rand()
c = .5*np.random.rand()
G = G*tf.tf([1.,c],[1.,-1.*complex(a,b)])
#Perturb each transfer function and compute the infinity norm of the
#transfer function and it's perturbation
Gp = G*Per.attack(G)
for j in range(100):
gainG[j] = la.norm(tf.evalfr(G,theta))
gainP[j] = la.norm(tf.evalfr(Gp,theta))
try:
y1[i] = max(gainG)
except ValueError:
y1[i] = -10
try:
y2[i] = max(gainP)
except ValueError:
y2[i] = -10
if np.isclose(y2[i],1):
count += 1
Y1 = Y1 + y1
Y2 = Y2 + y2
Success[n] = count
plt.scatter(X,Y1)
plt.scatter(X,Y2)
plt.show()
return Success,X,Y1,Y2
def sisoAttack(siso):
"""Function for creating a destabilizing pertubation for a given stable
discrete time siso transfer function.
Parameters
----------
siso: (tf object) Discrete time siso transfer function
Returns
-------
D: (tf object) Minimum perturbation to destabilize G
"""
dt = siso.dt
#Find the frequency peak and take a specific SVD at that frequency
theta = fpeak(siso)
s0 = ctrl.evalfr(siso,np.exp(theta*1j))
sigma,phi = abs(s0),np.angle(s0)
#The numbers, u = np.exp(phi*1j), v=1 complete the SVD but are not needed
#CONTINUOUS TIME
if dt is None:
sign = 1
#Check the angle in order to ensure a solution
if phi > 0:
phi = phi - np.pi
sign = -1
#Calculate all pass filter parameter
jomega = theta
uH = np.exp(-phi*1j)
a = jomega*(1 - uH)/(1 + uH)
utf = ctrl.tf([1.,-a],[1., a])
D = utf/sigma
#DISCRETE TIME
else:
#Find the frequency peak and take a specific SVD at that frequency
theta = fpeak(siso)
s0 = ctrl.evalfr(siso,np.exp(theta*1j))
sigma,phi = abs(s0),np.angle(s0)
#The numbers, u = np.exp(phi*1j), v=1 complete the SVD but are not needed
#Solve for all pass filter parameter
a = np.exp(-1j*theta) - np.exp(-.5j*(phi+theta))
#Choose appropriate filter
if ((a*a.conjugate())**.5).real < 1:
utf = ctrl.tf([1.,-1.*a.conjugate()],[-1.*a,1.],1.)
else:
utf = np.exp(2j*phi)*ctrl.tf([-1.*a,1.],[1.,-1.*a.conjugate()],1.)
#Create a unitary transfer function and incoorperate it into D
D = conj(utf)/sigma
return D
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 test_sample_system_prewarp(self, tsys, plantname):
"""bilinear approximation with prewarping test"""
wwarp = 50
Ts = 0.025
# test state space version
plant = getattr(tsys, plantname)
plant_d_warped = plant.sample(Ts, 'bilinear', prewarp_frequency=wwarp)
plant_fr = evalfr(plant, wwarp * 1j)
dt = plant_d_warped.dt
plant_d_fr = evalfr(plant_d_warped, np.exp(wwarp * 1.j * dt))
np.testing.assert_array_almost_equal(plant_fr, plant_d_fr)
def test_squeeze(self, fcn, nstate, nout, ninp, omega, squeeze, shape):
# Create the system to be tested
if fcn == ct.frd:
sys = fcn(ct.rss(nstate, nout, ninp), [1e-2, 1e-1, 1, 1e1, 1e2])
elif fcn == ct.tf and (nout > 1 or ninp > 1) and not slycot_check():
pytest.skip("Conversion of MIMO systems to transfer functions "
"requires slycot.")
else:
sys = fcn(ct.rss(nstate, nout, ninp))
# Convert the frequency list to an array for easy of use
isscalar = not hasattr(omega, '__len__')
omega = np.array(omega)
# Call the transfer function directly and make sure shape is correct
assert sys(omega * 1j, squeeze=squeeze).shape == shape
# Make sure that evalfr also works as expected
assert ct.evalfr(sys, omega * 1j, squeeze=squeeze).shape == shape
# Check frequency response
mag, phase, _ = sys.frequency_response(omega, squeeze=squeeze)
if isscalar and squeeze is not True:
# sys.frequency_response() expects a list as an argument
# Add the shape of the input to the expected shape
assert mag.shape == shape + (1, )
assert phase.shape == shape + (1, )
else:
assert mag.shape == shape
assert phase.shape == shape
# Make sure the default shape lines up with squeeze=None case
if squeeze is None:
assert sys(omega * 1j).shape == shape
# Changing config.default to False should return 3D frequency response
ct.config.set_defaults('control', squeeze_frequency_response=False)
mag, phase, _ = sys.frequency_response(omega)
if isscalar:
assert mag.shape == (sys.noutputs, sys.ninputs, 1)
assert phase.shape == (sys.noutputs, sys.ninputs, 1)
assert sys(omega * 1j).shape == (sys.noutputs, sys.ninputs)
assert ct.evalfr(sys,
omega * 1j).shape == (sys.noutputs, sys.ninputs)
else:
assert mag.shape == (sys.noutputs, sys.ninputs, len(omega))
assert phase.shape == (sys.noutputs, sys.ninputs, len(omega))
assert sys(omega * 1j).shape == \
(sys.noutputs, sys.ninputs, len(omega))
assert ct.evalfr(sys, omega * 1j).shape == \
(sys.noutputs, sys.ninputs, len(omega))
def ss_error(g, err_step=None, err_ramp=None, err_para=None):
s = ct.TransferFunction([1, 0], [1])
kx = None
ess = None
if err_step is not None:
ess = ct.evalfr(ct.minreal(g), 0j)
kx = 1 / err_step
elif err_ramp is not None:
ess = ct.evalfr(ct.minreal(s*g), 0j)
kx = 1 / err_ramp
elif err_para is not None:
ess = ct.evalfr(ct.minreal(s**2*g), 0j)
kx = 1 / err_para
return kx, ess
def pid_by_frequency(g, po, ts, err_step=None, err_ramp=None, err_para=None):
s = ct.TransferFunction([1, 0], [1])
ki, ess = ss_error(g/s, err_step, err_ramp, err_para)
if abs(ess) == np.inf or abs(ess) == np.nan:
ess = None
if ess is None or ki is None:
assert "Inconsistent system"
ki = ki / np.real(ess)
print("ki=", ki)
log_po = np.log(100 / po)
psi = log_po / np.sqrt(np.pi ** 2 + log_po ** 2)
wn = 4 / psi / ts
print("wn=", wn, "psi=", psi)
pm = 100 * psi
wcp = wn
p_cut = ct.evalfr(g, wcp * 1j)
p_cut_mag = np.abs(p_cut)
p_cut_angle = np.angle(p_cut)
controller_mag = 1 / p_cut_mag
controller_angle = -np.pi + pm * np.pi / 180 - p_cut_angle
kp = controller_mag * np.cos(controller_angle)
kd = (controller_mag * np.sin(controller_angle) + ki / wcp) / wcp
print(f"pm= {pm}, ki= {ki}, kp= {kp}, kd= {kd}")
controller = kp + kd * s + ki / s
return controller
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(evalfr(sys_tf, 1J), frd_tf.eval(1))
# Should get an error if we evaluate at an unknown frequency
with pytest.raises(ValueError):
frd_tf.eval(2)
def get_steady_state_error(system):
controler_and_plant_evaluated_at_0 = control.evalfr(system, 0)
if (np.isinf(controler_and_plant_evaluated_at_0)):
return 0
else:
return abs(
1 /
(1 +
controler_and_plant_evaluated_at_0 * value_of_K_lead_controller))
def norm(siso):
""" Estimates the norm of a siso transfer function
Parameters
----------
siso: (tf object) siso transfer function
Returns
-------
norm: the infinity norm of siso (frequency peak)
"""
dt = siso.dt
peak = fpeak(siso)
if dt is None:
norm = abs(ctrl.evalfr(siso, 1j * peak))
else:
norm = abs(ctrl.evalfr(siso, np.exp(1j * peak)))
return norm
def get_matrix_at_freq(x: float):
return np.array([[
1 / (1 + evalfr(plant * controller, 1j * x)),
evalfr(plant, 1j * x) / (1 + evalfr(plant * controller, 1j * x))
],
[
evalfr(controller, 1j * x) /
(1 + evalfr(plant * controller, 1j * x)),
evalfr(plant * controller, 1j * x) /
(1 + evalfr(plant * controller, 1j * x))
]])
def attackLink(G):
""" Function for creating a destabilizing pertubation for a given stable
discrete time siso transfer function.
Parameters
----------
G: (tf object) Discrete time siso transfer function
Returns
-------
D: (tf object) Perturbation (Not minimal) to destabilize G
"""
dt = G.dt
neg = ctrl.evalfr(G, -1.)
pos = ctrl.evalfr(G, 1.)
if abs(neg) > abs(pos):
D = -1 * ctrl.tf(1, [1, 0], dt) / neg
else:
D = ctrl.tf(1, [1, 0], dt) / pos
return D
def f(x):
tf_1 = P_1
tf_2 = P_2
# Object to maximize:
max_obj = abs((evalfr(tf_1, 1j * x) - evalfr(tf_2, 1j * x)) / np.sqrt(
(1 + evalfr(tf_1, 1j * x) * evalfr(tf_1, -1j * x)) *
(1 + evalfr(tf_2, 1j * x) * evalfr(tf_2, -1j * x))))
min_obj = -max_obj
return min_obj
def test_call_mimo(self):
"""Evaluate the frequency response of a MIMO system at one frequency."""
num = [[[1., 2.], [0., 3.], [2., -1.]],
[[1.], [4., 0.], [1., -4., 3.]]]
den = [[[-3., 2., 4.], [1., 0., 0.], [2., -1.]],
[[3., 0., .0], [2., -1., -1.], [1.]]]
sys = TransferFunction(num, den)
resp = [[0.147058823529412 + 0.0882352941176471j, -0.75, 1.],
[-0.083333333333333, -0.188235294117647 - 0.847058823529412j,
-1. - 8.j]]
np.testing.assert_array_almost_equal(evalfr(sys, 2j), resp)
# Test call version as well
np.testing.assert_array_almost_equal(sys(2.j), resp)
def test_call_siso(self, dt, omega, resp):
"""Evaluate the frequency response of a SISO system at one frequency."""
sys = TransferFunction([1., 3., 5], [1., 6., 2., -1])
if dt:
sys = sample_system(sys, dt)
s = np.exp(omega * 1j * dt)
else:
s = omega * 1j
# Correct versions of the call
np.testing.assert_allclose(evalfr(sys, s), resp, atol=1e-3)
np.testing.assert_allclose(sys(s), resp, atol=1e-3)
# Deprecated version of the call (should generate exception)
with pytest.raises(AttributeError):
np.testing.assert_allclose(sys.evalfr(omega), resp, atol=1e-3)
def randomPureReal(N=4, M=3):
#Generate a random transfer function of degree N
num = np.poly(2 * np.random.rand(N) - 1)
den = np.poly(2 * np.random.rand(M) - 1)
G = ctrl.tf(num, den, 1)
realFreq = pureReal(G)
sizes = [abs(ctrl.evalfr(G, z)) for z in realFreq]
realFreq = realFreq[np.argsort(sizes)]
sizes = 10 * (np.arange(len(realFreq)) + 1)
#Plot the unit circle and all the values where it is pure real
x = np.linspace(0, 2 * np.pi)
plt.plot(np.cos(x), np.sin(x))
plt.scatter(realFreq.real, realFreq.imag, s=sizes, c='r')
plt.axis('scaled')
plt.show()
def lag_analytic_mode(g,
wcg,
pm_desired=None,
psi=None,
err_step=None,
err_ramp=None,
err_para=None):
s = ct.TransferFunction([1, 0], [1])
if psi is not None:
pm_desired = 100 * psi * np.pi / 180
if pm_desired > 2 * np.pi:
print("remember, I need phase in radians")
kc, ess = ss_error(g, err_step, err_ramp, err_para)
if abs(ess) == np.inf or abs(ess) == np.nan:
ess = None
if ess is None or kc is None:
assert "Inconsistent system"
kc = kc / np.real(ess)
print(f"kc= {kc}")
i_point = ct.evalfr(g, 1j * wcg)
p_mag = np.abs(i_point)
p_angle = np.angle(i_point)
k_mag = 1 / p_mag
k_angle = -np.pi + pm_desired - p_angle
alpha = k_mag * (kc * np.cos(k_angle) -
k_mag) / (kc * (kc - k_mag * np.cos(k_angle)))
tau = (k_mag * np.cos(k_angle) - kc) / (k_mag * wcg * np.sin(k_angle))
print(f"alpha= {alpha}, tau= {tau}")
controller = kc * (alpha * tau * s + 1) / (tau * s + 1)
return controller
from utils import feedback w = np.logspace(-1, 2, 1000) s = control.tf([1, 0], 1) G = 4 / ((s - 1) * (0.02 * s + 1)**2) Kc = 1.25 tau1 = 1.5 K = Kc * (1 + 1 / (tau1 * s)) L = K * G S = feedback(1, L) T = feedback(L, 1) mag, phase, omega = control.bode(L, w) magS, phaseS, omega = control.bode(S, w) magT, phaseT, omega = control.bode(T, w) plt.legend(["L", "S", "T"], bbox_to_anchor=(0, 1.01, 1, 0), loc=3, ncol=3) Ms = max(magS) Mt = max(magT) gm, pm, wg, wp = control.margin(mag, phase, omega) Lu_180 = 1 / np.abs(control.evalfr(L, wg)) P = np.angle(control.evalfr(L, wp)) + np.pi print "Lower GM:", Lu_180 print "PM:", np.round(P * 180 / np.pi, 1), "deg or", np.round(P, 2), "rad" print "Ms:", Ms print "Mt:", Mt plt.show()
w = np.logspace(-1, 2, 1000)
s = control.tf([1, 0], 1)
G = 4 /((s - 1)*(0.02*s + 1)**2)
Kc = 1.25
tau1 = 1.5
K = Kc*(1+1/(tau1*s))
L = K*G
S = feedback(1, L)
T = feedback(L, 1)
mag, phase, omega = control.bode(L, w)
magS, phaseS, omega = control.bode(S, w)
magT, phaseT, omega = control.bode(T, w)
plt.legend(["L", "S", "T"],
bbox_to_anchor=(0, 1.01, 1, 0), loc=3, ncol=3)
Ms = max(magS)
Mt = max(magT)
gm, pm, wg, wp = control.margin(mag, phase, omega)
Lu_180 = 1/np.abs(control.evalfr(L, wg))
P = np.angle(control.evalfr(L, wp)) + np.pi
print "Lower GM:", Lu_180
print "PM:", np.round(P*180/np.pi, 1), "deg or", np.round(P, 2), "rad"
print "Ms:", Ms
print "Mt:", Mt
plt.show()
def f(x):
p = abs(evalfr(1 + plant_2_c * plant_1, 1j * x))
return p
def find_angle(testing_point, G):
degrees = math.degrees(np.angle(control.evalfr(G, testing_point)))
radians = math.radians(degrees)
return radians, degrees
def get_value_of_K(testing_point, G):
plant_value = control.evalfr(G, testing_point)
value_of_K = 1 / abs(plant_value)
return value_of_K
def get_value_of_K(controller, testing_point, G):
controler_and_plant = control.series(G, controller)
lead_controller_value = control.evalfr(controler_and_plant, testing_point)
value_of_K_lead_controller = 1 / abs(lead_controller_value)
return value_of_K_lead_controller
import control
import control.matlab as cm
# 5/s+5
teller = np.array([5.0])
noemer = np.array([1.0,5.0])
H = control.tf(teller, noemer)
W = [0,10,100]
Hjw = []
absoluut = []
angles = []
degrees = []
for w in range (0,3,1):
Hw = control.evalfr(H,1j*W[w])
# Vul een list met waarde
Hjw.append(Hw)
absoluut.append(np.abs(Hw))
angles.append(np.angle(Hw))
degrees.append(np.angle(Hw,deg=True))
#tabel van maken
print (H)
print ("\n")
print (Hjw)
print ("\n")
print (absoluut)
print ("\n")
print (angles)
print ("\n")
