def testMinrealBrute(self):
for n, m, p in permutations(range(1,6), 3):
s = matlab.rss(n, p, m)
sr = s.minreal()
if s.states > sr.states:
self.nreductions += 1
else:
np.testing.assert_array_almost_equal(
np.sort(eigvals(s.A)), np.sort(eigvals(sr.A)))
for i in range(m):
for j in range(p):
ht1 = matlab.tf(
matlab.ss(s.A, s.B[:,i], s.C[j,:], s.D[j,i]))
ht2 = matlab.tf(
matlab.ss(sr.A, sr.B[:,i], sr.C[j,:], sr.D[j,i]))
try:
self.assert_numden_almost_equal(
ht1.num[0][0], ht2.num[0][0],
ht1.den[0][0], ht2.den[0][0])
except Exception as e:
# for larger systems, the tf minreal's
# the original rss, but not the balanced one
if n < 6:
raise e
self.assertEqual(self.nreductions, 2)
def testCombi01(self):
"""Test from a "real" case, combines tf, ss, connect and margin.
This is a type 2 system, with phase starting at -180. The
margin command should remove the solution for w = nearly zero.
"""
# Example is a concocted two-body satellite with flexible link
Jb = 400
Jp = 1000
k = 10
b = 5
# can now define an "s" variable, to make TF's
s = tf([1, 0], [1])
hb1 = 1 / (Jb * s)
hb2 = 1 / s
hp1 = 1 / (Jp * s)
hp2 = 1 / s
# convert to ss and append
sat0 = append(ss(hb1), ss(hb2), k, b, ss(hp1), ss(hp2))
# connection of the elements with connect call
Q = [
[1, -3, -4], # link moment (spring, damper), feedback to body
[2, 1, 0], # link integrator to body velocity
[3, 2, -6], # spring input, th_b - th_p
[4, 1, -5], # damper input
[5, 3, 4], # link moment, acting on payload
[6, 5, 0]
]
inputs = [1]
outputs = [1, 2, 5, 6]
sat1 = connect(sat0, Q, inputs, outputs)
# matched notch filter
wno = 0.19
z1 = 0.05
z2 = 0.7
Hno = (1 + 2 * z1 / wno * s + s**2 / wno**2) / (1 + 2 * z2 / wno * s +
s**2 / wno**2)
# the controller, Kp = 1 for now
Kp = 1.64
tau_PD = 50.
Hc = (1 + tau_PD * s) * Kp
# start with the basic satellite model sat1, and get the
# payload attitude response
Hp = tf(np.array([0, 0, 0, 1]) * sat1)
# total open loop
Hol = Hc * Hno * Hp
gm, pm, wg, wp = margin(Hol)
# print("%f %f %f %f" % (gm, pm, wg, wp))
np.testing.assert_allclose(gm, 3.32065569155)
np.testing.assert_allclose(pm, 46.9740430224)
np.testing.assert_allclose(wg, 0.176469728448)
np.testing.assert_allclose(wp, 0.0616288455466)
def process_data(num11, den11, num21, den21):
w11 = ctrl.tf(num11, den11)
w21 = ctrl.tf(num21, den21)
print('результат w11={} w21={}'.format(w11, w21))
TimeLine = []
for i in range(1, 3000):
TimeLine.append(i / 1000)
plt.figure(0, figsize=[7, 6])
[y11, x11] = ctrl.step(w11, TimeLine)
[y21, x21] = ctrl.step(w21, TimeLine)
plt.plot(x11, y11, "r", label='Исходная')
plt.plot(x21, y21, "b", label='Увеличенная k и уменшенная Т')
plt.title('Переходная функция звена')
plt.ylabel('Амплитуда')
plt.xlabel('Время(с)')
plt.grid(True)
plt.show()
[y11, x11] = ctrl.impulse(w11, TimeLine)
[y21, x21] = ctrl.impulse(w21, TimeLine)
plt.plot(x11, y11, "r", label='Исходная')
plt.plot(x21, y21, "b", label='Увеличенная k и уменшенная Т')
plt.title('Импульсная функция звена')
plt.ylabel('Амплитуда')
plt.xlabel('Время(с)')
plt.grid(True)
plt.show()
ctrl.mag, ctrl.phase, ctrl.omega = ctrl.bode(w11, w21, dB=False)
plt.plot()
plt.show()
return w11, w21
def testMinrealBrute(self):
for n, m, p in permutations(range(1, 6), 3):
s = matlab.rss(n, p, m)
sr = s.minreal()
if s.states > sr.states:
self.nreductions += 1
else:
np.testing.assert_array_almost_equal(np.sort(eigvals(s.A)),
np.sort(eigvals(sr.A)))
for i in range(m):
for j in range(p):
ht1 = matlab.tf(
matlab.ss(s.A, s.B[:, i], s.C[j, :], s.D[j, i]))
ht2 = matlab.tf(
matlab.ss(sr.A, sr.B[:, i], sr.C[j, :], sr.D[j,
i]))
try:
self.assert_numden_almost_equal(
ht1.num[0][0], ht2.num[0][0], ht1.den[0][0],
ht2.den[0][0])
except Exception as e:
# for larger systems, the tf minreal's
# the original rss, but not the balanced one
if n < 6:
raise e
self.assertEqual(self.nreductions, 2)
def main():
# 制御対象を定義(今回はモータを想定)
J = 0.6 # 慣性
D = 0.2 # 粘性
Kt = 0.15 # トルク定数
P = crl.tf([Kt], [J, D, 0]) # モータ角度の電流制御
# 参照モデルの定義
M = crl.tf([50], [1, 50])
# シミュレーション条件を定義
Ts = 0.001 # サンプリング時間
# 制御器構造の定義
beta = crl.ss("0", "1", "0; 1", "1; 0") # PI制御器
print(crl.tf(beta))
beta = crl.c2d(beta, Ts)
# 制御対象と参照モデルのゲイン線図
plt.figure(1)
crl.bode([P, M])
plt.legend(["Plant", "Ref. model"])
# prbsを生成する
n = 12 # m系列信号の次数
Tp = 5 # 何周期印加するか？
Ap = 0.5 # 振幅 [A]
u0 = 2 * prbs(n)
u0 = Ap * (u0 - np.average(u0))
u0 = np.tile(u0, Tp)
t = np.linspace(0, Ts * u0.shape[0], u0.shape[0])
plt.figure(2)
plt.plot(t, u0)
plt.xlabel("Time [s]")
plt.ylabel("Current [A]")
# 実験データの取得
print("=== Start Experiment ===")
Pd = crl.c2d(P, Ts, "tustin") # 双一次変換で離散時間モデルを作成
t = np.arange(start=0, stop=Tp * Ts * (2**n - 1), step=Ts)
(y0, t0, xout) = crl.lsim(Pd, u0[:], t[:]) # 応答を取得
plt.figure(3)
plt.plot(y0)
# VRFTを実行する
print("=== Start VRFT ===")
W = crl.tf([50], [1, 50]) # 周波数重み
L = crl.minreal((1 - M) * M * W) # プレフィルタ
Ld = crl.c2d(L, Ts, "tustin")
(ul, tl, xlout) = crl.lsim(Ld, u0, t)
(phi_l, tl, xlout) = crl.lsim(beta, y0[:])
print(phi_l)
rho = np.linalg.solve(phi_l, ul)
print(rho)
plt.show()
def test_mimo():
"""Test MIMO frequency response calls"""
A = np.array([[1, 1], [0, 1]])
B = np.array([[1, 0], [0, 1]])
C = np.array([[1, 0]])
D = np.array([[0, 0]])
omega = np.linspace(10e-2, 10e2, 1000)
sysMIMO = ss(A, B, C, D)
sysMIMO.freqresp(omega)
tf(sysMIMO)
def select_order_GEN(id_method, y, u, tsample=1., na_ord=[0, 5], nb_ord=[1, 5], nc_ord=[0, 5], nd_ord=[0, 5], nf_ord=[0, 5], delays=[0, 5], method='AIC', max_iterations = 200):
# order ranges
na_Min = min(na_ord)
na_MAX = max(na_ord) + 1
nb_Min = min(nb_ord)
nb_MAX = max(nb_ord) + 1
nc_Min = min(nc_ord)
nc_MAX = max(nc_ord) + 1
nd_Min = min(nd_ord)
nd_MAX = max(nd_ord) + 1
nf_Min = min(nf_ord)
nf_MAX = max(nf_ord) + 1
theta_Min = min(delays)
theta_Max = max(delays) + 1
# check orders
sum_ords = np.sum(na_Min + na_MAX + nb_Min + nb_MAX + nc_Min + nc_MAX + nd_Min + nd_MAX + nf_Min + nf_MAX + theta_Min + theta_Max)
if ((np.issubdtype(sum_ords, np.signedinteger) or np.issubdtype(sum_ords, np.unsignedinteger))
and na_Min >= 0 and nb_Min > 0 and nc_Min >= 0 and nd_Min >= 0 and nf_Min >= 0 and theta_Min >= 0) is False:
sys.exit("Error! na, nc, nd, nf, theta must be positive integers, nb must be strictly positive integer")
# return 0.,0.,0.,0.,0.,0.,0.,np.inf
elif y.size != u.size:
sys.exit("Error! y and u must have tha same length")
# return 0.,0.,0.,0.,0.,0.,0.,np.inf
else:
ystd, y = rescale(y)
Ustd, u = rescale(u)
IC_old = np.inf
for i_a in range(na_Min, na_MAX):
for i_b in range(nb_Min, nb_MAX):
for i_c in range(nc_Min, nc_MAX):
for i_d in range(nd_Min, nd_MAX):
for i_f in range(nf_Min, nf_MAX):
for i_t in range(theta_Min, theta_Max):
useless1, useless2, useless3, useless4, Vn, y_id = GEN_RLS_id(id_method, y, u, i_a, i_b, i_c, i_d, i_f, i_t, max_iterations)
IC = information_criterion(i_a + i_b + i_c + i_d + i_f, y.size - max(i_a, i_b + i_t, i_c, i_d, i_f), Vn * 2, method)
# --> nota: non mi torna cosa scritto su ARMAX
if IC < IC_old:
na_min, nb_min, nc_min, nd_min, nf_min, theta_min = i_a, i_b, i_c, i_d, i_f, i_t
IC_old = IC
print("suggested orders are: Na=", na_min, "; Nb=", nb_min, "; Nc=", nc_min, "; Nd=", nd_min, "; Nf=", nf_min, "; Delay: ", theta_min)
# rerun identification
NUM, DEN, NUMH, DENH, Vn, y_id = GEN_RLS_id(id_method, y, u, na_min, nb_min, nc_min, nd_min, nf_min, theta_min, max_iterations)
Y_id = np.atleast_2d(y_id) * ystd
# rescale NUM coeff
NUM[theta_min:nb_min + theta_min] = NUM[theta_min:nb_min + theta_min] * ystd / Ustd
# FdT
g_identif = cnt.tf(NUM, DEN, tsample)
h_identif = cnt.tf(NUMH, DENH, tsample)
return na_min, nb_min, nc_min, nd_min, nf_min, theta_min, g_identif, h_identif, NUM, DEN, Vn, Y_id
def select_order_ARX(y,
u,
tsample=1.,
na_ord=[0, 5],
nb_ord=[1, 5],
delays=[0, 5],
method='AIC'):
# order ranges
na_Min = min(na_ord)
na_MAX = max(na_ord) + 1
nb_Min = min(nb_ord)
nb_MAX = max(nb_ord) + 1
theta_Min = min(delays)
theta_Max = max(delays) + 1
# check orders
sum_ords = np.sum(na_Min + na_MAX + nb_Min + nb_MAX + theta_Min +
theta_Max)
if ((np.issubdtype(sum_ords, np.signedinteger)
or np.issubdtype(sum_ords, np.unsignedinteger)) and na_Min >= 0
and nb_Min > 0 and theta_Min >= 0) == False:
sys.exit(
"Error! na, theta must be positive integers, nb must be strictly positive integer"
)
# return 0.,0.,0.,0.,0.,0.,0.,np.inf
elif y.size != u.size:
sys.exit("Error! y and u must have tha same length")
# return 0.,0.,0.,0.,0.,0.,0.,np.inf
else:
ystd, y = rescale(y)
Ustd, u = rescale(u)
IC_old = np.inf
for i in range(na_Min, na_MAX):
for j in range(nb_Min, nb_MAX):
for k in range(theta_Min, theta_Max):
useless1, useless2, useless3, Vn, y_id = ARX_id(
y, u, i, j, k)
IC = information_criterion(i + j, y.size - max(i, j + k),
Vn * 2, method)
if IC < IC_old:
na_min, nb_min, theta_min = i, j, k
IC_old = IC
print("suggested orders are: Na=", na_min, "; Nb=", nb_min, "Delay: ",
theta_min)
# rerun identification
NUM, DEN, NUMH, Vn, y_id = ARX_id(y, u, na_min, nb_min, theta_min)
Y_id = np.atleast_2d(y_id) * ystd
NUM[theta_min:nb_min +
theta_min] = NUM[theta_min:nb_min + theta_min] * ystd / Ustd
# FdT
g_identif = cnt.tf(NUM, DEN, tsample)
h_identif = cnt.tf(NUMH, DEN, tsample)
return na_min, nb_min, theta_min, g_identif, h_identif, NUM, DEN, Vn, Y_id
def test_tf_string_args(self):
"""Make sure s and z are defined properly"""
s = tf('s')
G = (s + 1)/(s**2 + 2*s + 1)
np.testing.assert_array_almost_equal(G.num, [[[1, 1]]])
np.testing.assert_array_almost_equal(G.den, [[[1, 2, 1]]])
assert isctime(G, strict=True)
z = tf('z')
G = (z + 1)/(z**2 + 2*z + 1)
np.testing.assert_array_almost_equal(G.num, [[[1, 1]]])
np.testing.assert_array_almost_equal(G.den, [[[1, 2, 1]]])
assert isdtime(G, strict=True)
def ARX_MIMO_id(y, u, na, nb, theta, tsample=1.):
na = np.array(na)
nb = np.array(nb)
theta = np.array(theta)
[ydim, ylength] = y.shape
[udim, ulength] = u.shape
[th1, th2] = theta.shape
# check dimensions
sum_ords = np.sum(nb) + np.sum(na) + np.sum(theta)
if na.size != ydim:
sys.exit(
"Error! na must be a vector, whose length must be equal to y dimension"
)
# return 0.,0.,0.,0.,np.inf
elif nb[:, 0].size != ydim:
sys.exit(
"Error! nb must be a matrix, whose dimensions must be equal to yxu"
)
# return 0.,0.,0.,0.,np.inf
elif th1 != ydim:
sys.exit("Error! theta matrix must have yxu dimensions")
# return 0.,0.,0.,0.,np.inf
elif ((np.issubdtype(sum_ords, np.signedinteger)
or np.issubdtype(sum_ords, np.unsignedinteger)) and np.min(nb) >= 0
and np.min(na) >= 0 and np.min(theta) >= 0) == False:
sys.exit(
"Error! na, nb, theta must contain only positive integer elements")
# return 0.,0.,0.,0.,np.inf
else:
# preallocation
Vn_tot = 0.
NUMERATOR = []
DENOMINATOR = []
DENOMINATOR_H = []
NUMERATOR_H = []
Y_id = np.zeros((ydim, ylength))
# identification in MISO approach
for i in range(ydim):
DEN, NUM, NUMH, Vn, y_id = ARX_MISO_id(y[i, :], u, na[i], nb[i, :],
theta[i, :])
# append values to vectors
DENOMINATOR.append(DEN.tolist())
NUMERATOR.append(NUM.tolist())
NUMERATOR_H.append(NUMH.tolist())
DENOMINATOR_H.append([DEN.tolist()[0]])
Vn_tot = Vn_tot + Vn
Y_id[i, :] = y_id
# FdT
G = cnt.tf(NUMERATOR, DENOMINATOR, tsample)
H = cnt.tf(NUMERATOR_H, DENOMINATOR_H, tsample)
return DENOMINATOR, NUMERATOR, G, H, Vn_tot, Y_id
def __init__(self, Ts):
# steady state wind defined in the inertial frame
self._steady_state = np.array([[0., 0., 0.]]).T
# self.steady_state = np.array([[3., 1., 0.]]).T
# Dryden gust model parameters (pg 56 UAV book)
# HACK: Setting Va to a constant value is a hack. We set a nominal airspeed for the gust model.
# Could pass current Va into the gust function and recalculate A and B matrices.
Va = 17
# self._A = np.array([[1-Ts*c, -Ts*d],[Ts, 1]])
# self._B = np.array([[Ts],[0]])
# self._C = np.array([[a, b]])
suv = 1.06
sw = 0.7
luv = 200
lw = 50
hu_n = np.array([suv * np.sqrt(2 * Va / luv)])
hu_d = np.array([1, Va / luv])
hv_n = suv * np.sqrt(3 * Va / luv) * np.array(
[1, Va / (np.sqrt(3) * luv)])
hv_d = np.array([1, 2 * Va / luv, (Va / luv)**2])
hw_n = sw * np.sqrt(3 * Va / lw) * np.array(
[1, Va / (np.sqrt(3) * lw)])
hw_d = np.array([1, 2 * Va / lw, (Va / lw)**2])
self.hu = ctl.ss(ctl.tf(hu_n, hu_d, Ts))
self.hv = ctl.ss(ctl.tf(hv_n, hv_d, Ts))
self.hw = ctl.ss(ctl.tf(hw_n, hw_d, Ts))
self.huA = np.asarray(self.hu.A)
self.huB = np.asarray(self.hu.B)
self.huC = np.asarray(self.hu.C)
self.hvA = np.asarray(self.hv.A)
self.hvB = np.asarray(self.hv.B)
self.hvC = np.asarray(self.hv.C)
self.hwA = np.asarray(self.hw.A)
self.hwB = np.asarray(self.hw.B)
self.hwC = np.asarray(self.hw.C)
self._gust_state_u = np.array([0.])
self._gust_state_v = np.array([[0.], [0.]])
self._gust_state_w = np.array([[0.], [0.]])
self._Ts = Ts
def t10(): # fixme: add PlotStyle # http://sTckoverflow.com/questions/11481644/how-do-i-assign-multiple-labels-at-once-in-matplotlib syss = [tf([10], [1, 10]), tf([100], convolve([1, 10], [1, 10])), tf([1000], convolve([1, 10], convolve([1, 10], [1, 10])))] labels = ['T', 'F', 'M'] w = logspace(-1, 3, 1e3) # better then linspace for l, sys in zip(labels, syss): bode(sys, w, label=l) legend() show()
def margin_zpk(sys, z, p, k):
# open-loop system transfer function
try:
num, den = model(sys)
except:
# for error detection
print("Err: system in not defined")
return
Gs = control.tf(num, den)
# define compensator transfer function
# convert zero and pole to numpy arrays
z = np.array([z])
p = np.array([p])
num, den = matlab.zpk2tf(z, p, k)
Ds = matlab.tf(num, den)
# Compensated open-loop transfer function
DsGs = Ds*Gs
# phase & gain margins
gm, pm, wg, wp = control.margin(DsGs)
# round phase & gain margin variables
ndigits = 2
gm = round(gm, ndigits)
pm = round(pm, ndigits)
wg = round(wg, ndigits)
wp = round(wp, ndigits)
return gm, pm, wg, wp
def margin_pid(sys, Kp, Ki, Kd):
# open-loop system transfer function
try:
num, den = model(sys)
except:
# for error detection
print("Err: system in not defined")
return
Gs = control.tf(num, den)
# PID Controller
s = matlab.tf('s')
Ds = Kp + Ki/s + Kd*s
# Compensated open-loop transfer function
DsGs = Ds*Gs
# phase & gain margins
gm, pm, wg, wp = control.margin(DsGs)
# round phase & gain margin variables
ndigits = 2
gm = round(gm, ndigits)
pm = round(pm, ndigits)
wg = round(wg, ndigits)
wp = round(wp, ndigits)
return gm, pm, wg, wp
def bode_pid(sys, Kp, Ki, Kd):
# open-loop system transfer function
try:
num, den = model(sys)
except:
# for error detection
print("Err: system in not defined")
return
Gs = control.tf(num, den)
# PID Controller
s = matlab.tf('s')
Ds = Kp + Ki/s + Kd*s
# Compensated open-loop transfer function
DsGs = Ds*Gs
# bode plot arrays
omega = np.logspace(-2, 2, 2000)
mag, phase, omega = control.bode(DsGs, omega=omega)
mag = 20 * np.log10(mag) # mag in db
phase = np.degrees(phase) # phase in degrees
# convert numpy arrays to lists
omega = list(omega)
phase = list(phase)
mag = list(mag)
# round lists to 6 decimal floating digits
ndigits = 6
omega = [round(num, ndigits) for num in omega]
phase = [round(num, ndigits) for num in phase]
mag = [round(num, ndigits) for num in mag]
return omega, mag, phase
def stepResponse(Ts):
num = Poly(Ts.as_numer_denom()[0],s).all_coeffs()
den = Poly(Ts.as_numer_denom()[1],s).all_coeffs()
tf = matlab.tf(map(float,num),map(float,den))
y,t = matlab.step(tf)
plt.plot(t,y)
plt.title("Step Response")
plt.grid()
plt.xlabel("time (s)")
plt.ylabel("y(t)")
info = "OS:%f%s"%(round((y.max()/y[-1]-1)*100,2),'%')
try:
i10 = next(i for i in range(0,len(y)-1) if y[i]>=y[-1]*.10)
Tr = round(t[next(i for i in range(i10,len(y)-1) if y[i]>=y[-1]*.90)]-t[i10],2)
except StopIteration:
Tr = "unknown"
try:
Ts = round(t[next(len(y)-i for i in range(2,len(y)-1) if abs(y[-i]/y[-1])>1.02)]-t[0],2)
except StopIteration:
Ts = "unknown"
info += "\nTr: %s"%(Tr)
info +="\nTs: %s"%(Ts)
print info
plt.legend([info],loc=4)
plt.show()
def bode_as(num, den, freq_range=None):
'''Creates the asymptotic log-magnitude Bode plot for type zero transfer
functions.
Example : for the system G(s) = (4s + 1) / (3s + 2), use
bode_as([4, 1], [3, 2])
for the system G(s) = [(s + 1)(s + 20)] / [(s + 2)(s + 8)], use
bode_as(poly((-1, -20)), poly((-2, -8)))
'''
G = tf(num, den)
# If the user provides the range of frequencies, take it
# Else calculate the most suitable range
if freq_range:
freq_min, freq_max = freq_range
else:
freq_min, freq_max = plot_range(num, den)
omega = logspace(log10(freq_min), log10(freq_max), 1000)
# The actual Bode plot calculation
mag, phase, freq = bode(G, omega, dB=True, Plot=False)
# Asymptotic log-magnitude Bode plot calculation
asymptotic_plot_data = asymptote(num, den)
omega_as, mag_as = asymptotic_plot_data[:, 0], asymptotic_plot_data[:, 1]
plot_config(mag, omega, mag_as, omega_as)
def main():
print "Root Locus Extras"
print "Methods to add functionality to control.matlab.rlocus"
print "requires control.matlab package"
print "suggested usage: import homework8.root_locus_extras as rlx"
_s = sympy.Symbol("s")
_S = 1 / ((_s+1)*(_s+2)*(_s+10))
_numerS = map(float, sympy.Poly(_S.as_numer_denom()[0], _s).all_coeffs())
_denomS = map(float, sympy.Poly(_S.as_numer_denom()[1], _s).all_coeffs())
_tf = matlab.tf(_numerS, _denomS)
_gain = 164.5
_damping_ratio = 0.174
_gain = gainFromDampingRatio(_tf, _damping_ratio)
_poles = polesFromTransferFunction(_tf)
_overshoot = overshootFromDampingRatio(_tf, _damping_ratio)
_damping_ratio = dampingRatioFromGain(_tf, _gain)
print "example: "
print "system = ", _tf
print "Damping Ratio provided: ", _damping_ratio
print "Calculated values:"
print "Gain: ", _gain
print "Poles: ", _poles
print "Overshoot: ", _overshoot
print "Damping Ratio: ", dampingRatioFromGain(_tf, _gain)
print "Overshoot: ", overshootFromGain(_tf, _gain)
print "Frequency:", frequencyFromGain(_tf, _gain)
def bode_diagram():
a=[]
b=[]
print("分子の最高次は？")
bunshi = int(input("入力："))
print("分子の高次の項の係数を入れてください")
for i in range(bunshi+1):
print("s^",(bunshi-i),"の係数を入力してください",sep='')
a.append(float(input("入力：")))
print('\n')
print("分母の最高次は？")
bunbo = int(input("入力："))
print("分母の高次の項の係数を入れてください")
for i in range(bunbo+1):
print("s^",(bunbo-i),"の係数を入力してください",sep='')
b.append(float(input("入力：")))
print('\n')
Ga=matlab.tf(a,b)
print(Ga)
fig=plt.figure()
matlab.bode(Ga)
plt.show()
fig.savefig("ボード線図.jpg")
fig = plt.figure()
matlab.nyquist(Ga)
plt.show()
fig.savefig("ナエキスト線図")
def main():
print "Root Locus Extras"
print "Methods to add functionality to control.matlab.rlocus"
print "requires control.matlab package"
print "suggested usage: import homework8.root_locus_extras as rlx"
_s = sympy.Symbol("s")
_S = 1 / ((_s + 1) * (_s + 2) * (_s + 10))
_numerS = map(float, sympy.Poly(_S.as_numer_denom()[0], _s).all_coeffs())
_denomS = map(float, sympy.Poly(_S.as_numer_denom()[1], _s).all_coeffs())
_tf = matlab.tf(_numerS, _denomS)
_gain = 164.5
_damping_ratio = 0.174
_gain = gainFromDampingRatio(_tf, _damping_ratio)
_poles = polesFromTransferFunction(_tf)
_overshoot = overshootFromDampingRatio(_tf, _damping_ratio)
_damping_ratio = dampingRatioFromGain(_tf, _gain)
print "example: "
print "system = ", _tf
print "Damping Ratio provided: ", _damping_ratio
print "Calculated values:"
print "Gain: ", _gain
print "Poles: ", _poles
print "Overshoot: ", _overshoot
print "Damping Ratio: ", dampingRatioFromGain(_tf, _gain)
print "Overshoot: ", overshootFromGain(_tf, _gain)
print "Frequency:", frequencyFromGain(_tf, _gain)
def __init__(self, ts_control):
# instantiate lateral controllers
self.roll_from_aileron = pdControlWithRate(kp=AP.roll_kp,
kd=AP.roll_kd,
limit=np.radians(45))
self.course_from_roll = piControl(kp=AP.course_kp,
ki=AP.course_ki,
Ts=ts_control,
limit=np.radians(30))
self.yaw_damper = matlab.tf([0.5, 0.], [
1.0,
], ts_control)
#
# num=np.array([[AP.yaw_damper_kp, 0]]),
# den=np.array([[1, 1/AP.yaw_damper_tau_r]]),
# Ts=ts_control)
# instantiate lateral controllers
self.pitch_from_elevator = pdControlWithRate(kp=AP.pitch_kp,
kd=AP.pitch_kd,
limit=np.radians(45))
self.altitude_from_pitch = piControl(kp=AP.altitude_kp,
ki=AP.altitude_ki,
Ts=ts_control,
limit=np.radians(30))
self.airspeed_from_throttle = piControl(kp=AP.airspeed_throttle_kp,
ki=AP.airspeed_throttle_ki,
Ts=ts_control,
limit=1.0)
self.commanded_state = msgState()
def pid(Kp, Ki, Kd):
# PID Controller
s = matlab.tf('s')
Ds = Kp + Ki/s + Kd*s
# Draw the open-loop frequency resp. for the comp. sys
Gs = control.tf(36, [1, 3.6, 0])
DsGs = Ds*Gs
gm, pm, wg, wp = control.margin(DsGs)
print(f"Gain margin = {gm} dB")
print(f"Phase margin = {round(pm, 2)} degrees")
print(f"Frequency for Gain Margin = {wg} radians/sec")
print(f"Frequecny for Phase Margin = {wp} radians/sec")
omega_comp = np.logspace(-2,2,2000)
mag_comp, phase_comp, omega_comp = control.bode(DsGs, omega=omega_comp)
mag_comp = 20 * np.log10(mag_comp)
phase_comp = np.degrees(phase_comp)
omega_comp = omega_comp.T
phase_comp = phase_comp.T
mag_comp = mag_comp.T
omega_comp = list(omega_comp)
phase_comp = list(phase_comp)
mag_comp = list(mag_comp)
return omega_comp, mag_comp, phase_comp, gm, pm, wp, wg
def zpk(z, p, k):
Gs = control.tf(36, [1, 3.6, 0])
# Define Compensator transfer function
num, den = matlab.zpk2tf(z, p, k)
Ds = matlab.tf(num, den)
print(Ds)
# Draw the open-loop frequency resp. for the comp. sys
DsGs = Ds*Gs
gm, pm, wg, wp = control.margin(DsGs)
print(f"Gain margin = {gm} dB")
print(f"Phase margin = {round(pm, 2)} degrees")
print(f"Frequency for Gain Margin = {wg} radians/sec")
print(f"Frequecny for Phase Margin = {wp} radians/sec")
omega_comp = np.logspace(-2,2,2000)
mag_comp, phase_comp, omega_comp = control.bode(DsGs, omega=omega_comp)
mag_comp = 20 * np.log10(mag_comp)
phase_comp = np.degrees(phase_comp)
omega_comp = omega_comp.T
phase_comp = phase_comp.T
mag_comp = mag_comp.T
omega_comp = list(omega_comp)
phase_comp = list(phase_comp)
mag_comp = list(mag_comp)
return omega_comp, mag_comp, phase_comp, gm, pm, wp, wg
def test_discrete(self):
# Test discrete time frequency response
# SISO state space systems with either fixed or unspecified sampling times
sys = rss(3, 1, 1)
siso_ss1d = StateSpace(sys.A, sys.B, sys.C, sys.D, 0.1)
siso_ss2d = StateSpace(sys.A, sys.B, sys.C, sys.D, True)
# MIMO state space systems with either fixed or unspecified sampling times
A = [[-3., 4., 2.], [-1., -3., 0.], [2., 5., 3.]]
B = [[1., 4.], [-3., -3.], [-2., 1.]]
C = [[4., 2., -3.], [1., 4., 3.]]
D = [[-2., 4.], [0., 1.]]
mimo_ss1d = StateSpace(A, B, C, D, 0.1)
mimo_ss2d = StateSpace(A, B, C, D, True)
# SISO transfer functions
siso_tf1d = TransferFunction([1, 1], [1, 2, 1], 0.1)
siso_tf2d = TransferFunction([1, 1], [1, 2, 1], True)
# Go through each system and call the code, checking return types
for sys in (siso_ss1d, siso_ss2d, mimo_ss1d, mimo_ss2d, siso_tf1d,
siso_tf2d):
# Set frequency range to just below Nyquist freq (for Bode)
omega_ok = np.linspace(10e-4, 0.99, 100) * np.pi / sys.dt
# Test frequency response
ret = sys.freqresp(omega_ok)
# Check for warning if frequency is out of range
import warnings
warnings.simplefilter('always', UserWarning) # don't supress
with warnings.catch_warnings(record=True) as w:
# Set up warnings filter to only show warnings in control module
warnings.filterwarnings("ignore")
warnings.filterwarnings("always", module="control")
# Look for a warning about sampling above Nyquist frequency
omega_bad = np.linspace(10e-4, 1.1, 10) * np.pi / sys.dt
ret = sys.freqresp(omega_bad)
print("len(w) =", len(w))
self.assertEqual(len(w), 1)
self.assertIn("above", str(w[-1].message))
self.assertIn("Nyquist", str(w[-1].message))
# Test bode plots (currently only implemented for SISO)
if (sys.inputs == 1 and sys.outputs == 1):
# Generic call (frequency range calculated automatically)
ret_ss = bode(sys)
# Convert to transfer function and test bode again
systf = tf(sys)
ret_tf = bode(systf)
# Make sure we can pass a frequency range
bode(sys, omega_ok)
else:
# Calling bode should generate a not implemented error
self.assertRaises(NotImplementedError, bode, (sys, ))
def __init__(self, dt):
num = np.array([0.01, 0.01, 0.02])
den = np.array([0.001, 1, 0])
sys_tf = ctrl.tf(num, den)
self.A, self.B, self.C, self.D = ctrl.ssdata(
ctrl.c2d(sys_tf, dt, 'foh'))
self.X = np.zeros((self.A.shape[0]))
def trans_func(a):
w = ctr.tf(numer[a], denumer[a])
w1 = ctr.tf(numer2[a], denumer2[a])
print(w)
y1, t1 = step(w, t)
y2, t2 = step(w1, t)
ln[0], ln[1] = plt.plot(t, y1, "r", t, y2, "b")
plt.legend(ln, ['Standart pams', 'x2 pams'])
plt.title('Step Response')
plt.ylabel('Amplitude h(t)')
plt.xlabel('Time(sec)')
plt.grid(True)
plt.show()
return
def test_discrete(dsystem_type):
"""Test discrete time frequency response"""
dsys = dsystem_type
# Set frequency range to just below Nyquist freq (for Bode)
omega_ok = np.linspace(10e-4, 0.99, 100) * np.pi / dsys.dt
# Test frequency response
dsys.frequency_response(omega_ok)
# Check for warning if frequency is out of range
with pytest.warns(UserWarning, match="above.*Nyquist"):
# Look for a warning about sampling above Nyquist frequency
omega_bad = np.linspace(10e-4, 1.1, 10) * np.pi / dsys.dt
dsys.frequency_response(omega_bad)
# Test bode plots (currently only implemented for SISO)
if (dsys.ninputs == 1 and dsys.noutputs == 1):
# Generic call (frequency range calculated automatically)
bode(dsys)
# Convert to transfer function and test bode again
systf = tf(dsys)
bode(systf)
# Make sure we can pass a frequency range
bode(dsys, omega_ok)
else:
# Calling bode should generate a not implemented error
with pytest.raises(NotImplementedError):
bode((dsys, ))
def test_discrete(self):
# Test discrete time frequency response
# SISO state space systems with either fixed or unspecified sampling times
sys = rss(3, 1, 1)
siso_ss1d = StateSpace(sys.A, sys.B, sys.C, sys.D, 0.1)
siso_ss2d = StateSpace(sys.A, sys.B, sys.C, sys.D, True)
# MIMO state space systems with either fixed or unspecified sampling times
A = [[-3., 4., 2.], [-1., -3., 0.], [2., 5., 3.]]
B = [[1., 4.], [-3., -3.], [-2., 1.]]
C = [[4., 2., -3.], [1., 4., 3.]]
D = [[-2., 4.], [0., 1.]]
mimo_ss1d = StateSpace(A, B, C, D, 0.1)
mimo_ss2d = StateSpace(A, B, C, D, True)
# SISO transfer functions
siso_tf1d = TransferFunction([1, 1], [1, 2, 1], 0.1)
siso_tf2d = TransferFunction([1, 1], [1, 2, 1], True)
# Go through each system and call the code, checking return types
for sys in (siso_ss1d, siso_ss2d, mimo_ss1d, mimo_ss2d,
siso_tf1d, siso_tf2d):
# Set frequency range to just below Nyquist freq (for Bode)
omega_ok = np.linspace(10e-4,0.99,100) * np.pi/sys.dt
# Test frequency response
ret = sys.freqresp(omega_ok)
# Check for warning if frequency is out of range
import warnings
warnings.simplefilter('always', UserWarning) # don't supress
with warnings.catch_warnings(record=True) as w:
# Set up warnings filter to only show warnings in control module
warnings.filterwarnings("ignore")
warnings.filterwarnings("always", module="control")
# Look for a warning about sampling above Nyquist frequency
omega_bad = np.linspace(10e-4,1.1,10) * np.pi/sys.dt
ret = sys.freqresp(omega_bad)
print("len(w) =", len(w))
self.assertEqual(len(w), 1)
self.assertIn("above", str(w[-1].message))
self.assertIn("Nyquist", str(w[-1].message))
# Test bode plots (currently only implemented for SISO)
if (sys.inputs == 1 and sys.outputs == 1):
# Generic call (frequency range calculated automatically)
ret_ss = bode(sys)
# Convert to transfer function and test bode again
systf = tf(sys);
ret_tf = bode(systf)
# Make sure we can pass a frequency range
bode(sys, omega_ok)
else:
# Calling bode should generate a not implemented error
self.assertRaises(NotImplementedError, bode, (sys,))
def test_nyquist_basic(ss_siso):
"""Test nyquist plot call (Very basic)"""
# TODO: proper test
tf_siso = tf(ss_siso)
nyquist_plot(ss_siso)
nyquist_plot(tf_siso)
count, contour = nyquist_plot(tf_siso,
plot=False,
return_contour=True,
omega_num=20)
assert len(contour) == 20
with pytest.warns(UserWarning, match="encirclements was a non-integer"):
count, contour = nyquist_plot(tf_siso,
plot=False,
omega_limits=(1, 100),
return_contour=True)
assert_allclose(contour[0], 1j)
assert_allclose(contour[-1], 100j)
count, contour = nyquist_plot(tf_siso,
plot=False,
omega=np.logspace(-1, 1, 10),
return_contour=True)
assert len(contour) == 10
def update_den(val):
global den
den=den+val
print(den)
tf = matlab.tf(map(float,num),map(float,den))
matlab.bode(tf)
plt.show()
def update_num(val):
global num
num=num+val
print(num)
tf = matlab.tf(map(float,num),map(float,den))
matlab.bode(tf)
plt.show()
def get_TF(zeros, poles, constant):
num_fact = []
den_fact = []
i = 0
while i < len(zeros):
num_fact.insert(i, [1, -1 * zeros[i]])
i = i + 1
i = 0
while i < len(poles):
den_fact.insert(i, [1, -1 * poles[i]])
i = i + 1
i = 0
num = [1]
den = [1]
while i < (len(zeros)):
num = np.polymul(num, num_fact[i])
i = i + 1
i = 0
while i < len(poles):
den = np.polymul(den, den_fact[i])
i = i + 1
H_s = constant * mt.tf(num, den)
# print(H_s)
d, n = [], []
for i in range(0, len(num)):
n.append(num[i].real * constant)
for i in range(0, len(den)):
d.append(den[i].real)
return H_s, n, d
def ClosedLoop_TF(AOL_TF, Beta_TF, C_s):
n1 = AOL_TF.num[0][0]
d1 = AOL_TF.den[0][0]
n2 = Beta_TF.num[0][0]
d2 = Beta_TF.den[0][0]
n1 = conv_arr2list(n1)
d1 = conv_arr2list(d1)
n2 = conv_arr2list(n2)
d2 = conv_arr2list(d2)
num = np.polymul(conv_list2arr(n1), conv_list2arr(d2))
den1 = np.polymul(conv_list2arr(d1), conv_list2arr(d2))
den2 = np.polymul(conv_list2arr(n1), conv_list2arr(n2))
den1 = conv_arr2list(den1)
den2 = conv_arr2list(den2)
if len(den1) > len(den2):
i = len(den2)
while i < len(den1):
den2 = append2first(den2, 0)
i = i + 1
else:
i = len(den1)
while i < len(den2):
den1 = append2first(den1, 0)
i = i + 1
den = conv_list2arr(add_elm_by_elm(den2, den1))
CL_TF = C_s * mt.tf(num, den)
num_cl = CL_TF.num
den_cl = CL_TF.den
return CL_TF, num_cl[0][0], den_cl[0][0]
def pid_lti_feedback(set_point, *, Kp, Ki, Kd, Nfilt, plant):
pid = pidtf(Kp, Ki, Kd, Nfilt)
plant = tf([1], [1, 10, 20])
sys = feedback(pid * plant)
return set_point | lti(sys=sys, init_x=0)
def testMinreal(self, verbose=False):
"""Test a minreal model reduction"""
# A = [-2, 0.5, 0; 0.5, -0.3, 0; 0, 0, -0.1]
A = [[-2, 0.5, 0], [0.5, -0.3, 0], [0, 0, -0.1]]
# B = [0.3, -1.3; 0.1, 0; 1, 0]
B = [[0.3, -1.3], [0.1, 0.], [1.0, 0.0]]
# C = [0, 0.1, 0; -0.3, -0.2, 0]
C = [[0., 0.1, 0.0], [-0.3, -0.2, 0.0]]
# D = [0 -0.8; -0.3 0]
D = [[0., -0.8], [-0.3, 0.]]
# sys = ss(A, B, C, D)
sys = ss(A, B, C, D)
sysr = minreal(sys, verbose=verbose)
assert sysr.nstates == 2
assert sysr.ninputs == sys.ninputs
assert sysr.noutputs == sys.noutputs
np.testing.assert_array_almost_equal(
eigvals(sysr.A), [-2.136154, -0.1638459])
s = tf([1, 0], [1])
h = (s+1)*(s+2.00000000001)/(s+2)/(s**2+s+1)
hm = minreal(h, verbose=verbose)
hr = (s+1)/(s**2+s+1)
np.testing.assert_array_almost_equal(hm.num[0][0], hr.num[0][0])
np.testing.assert_array_almost_equal(hm.den[0][0], hr.den[0][0])
def test_mimo(self):
# MIMO
B = np.matrix('1,0;0,1')
D = np.matrix('0,0')
sysMIMO = ss(self.A,B,self.C,D)
frqMIMO = sysMIMO.freqresp(self.omega)
tfMIMO = tf(sysMIMO)
def t18(): # fixme: make lplane() T = ([20, 30, 40], [2, 1, 1, 0, 3]) sys = tf(*T) mag, phase, omega = bode(sys) # clear() # plot(phase, mag) # grid() show()
def t11(): # fixme: add labels to everything def plot_sys(sys): ys, ts = step(sys) plot(ts, ys) syss = [tf([100], [1, 4, 100]), tf([10], [1, 10]), tf([100], [1, 2, 100]), tf([100], [1, 1, 100])] for sys in syss: plot_sys(sys) legend() figure() for sys in syss: bode(sys) legend() show()
def test_siso(self):
B = np.matrix('0;1')
D = 0
sys = StateSpace(self.A,B,self.C,D)
# test frequency response
frq=sys.freqresp(self.omega)
# test bode plot
bode(sys)
# Convert to transfer function and test bode
systf = tf(sys)
bode(systf)
def testFreqResp(self):
"""Compare the bode reponses of the SS systems and TF systems to the original SS
They generally are different realizations but have same freq resp.
Currently this test may only be applied to SISO systems.
"""
for states in range(1,self.maxStates):
for testNum in range(self.numTests):
for inputs in range(1,1):
for outputs in range(1,1):
ssOriginal = matlab.rss(states, inputs, outputs)
tfOriginal_Actrb, tfOriginal_Bctrb, tfOriginal_Cctrb, tfOrigingal_nctrb, tfOriginal_index,\
tfOriginal_dcoeff, tfOriginal_ucoeff = tb04ad(states,inputs,outputs,\
ssOriginal.A,ssOriginal.B,ssOriginal.C,ssOriginal.D,tol1=0.0)
ssTransformed_nr, ssTransformed_A, ssTransformed_B, ssTransformed_C, ssTransformed_D\
= td04ad('R',inputs,outputs,tfOriginal_index,tfOriginal_dcoeff,tfOriginal_ucoeff,tol=0.0)
tfTransformed_Actrb, tfTransformed_Bctrb, tfTransformed_Cctrb, tfTransformed_nctrb,\
tfTransformed_index, tfTransformed_dcoeff, tfTransformed_ucoeff = tb04ad(\
ssTransformed_nr,inputs,outputs,ssTransformed_A, ssTransformed_B, ssTransformed_C,\
ssTransformed_D,tol1=0.0)
numTransformed = np.array(tfTransformed_ucoeff)
denTransformed = np.array(tfTransformed_dcoeff)
numOriginal = np.array(tfOriginal_ucoeff)
denOriginal = np.array(tfOriginal_dcoeff)
ssTransformed = matlab.ss(ssTransformed_A,ssTransformed_B,ssTransformed_C,ssTransformed_D)
for inputNum in range(inputs):
for outputNum in range(outputs):
[ssOriginalMag,ssOriginalPhase,freq] = matlab.bode(ssOriginal,Plot=False)
[tfOriginalMag,tfOriginalPhase,freq] = matlab.bode(matlab.tf(numOriginal[outputNum][inputNum],denOriginal[outputNum]),Plot=False)
[ssTransformedMag,ssTransformedPhase,freq] = matlab.bode(ssTransformed,freq,Plot=False)
[tfTransformedMag,tfTransformedPhase,freq] = matlab.bode(matlab.tf(numTransformed[outputNum][inputNum],denTransformed[outputNum]),freq,Plot=False)
#print 'numOrig=',numOriginal[outputNum][inputNum]
#print 'denOrig=',denOriginal[outputNum]
#print 'numTrans=',numTransformed[outputNum][inputNum]
#print 'denTrans=',denTransformed[outputNum]
np.testing.assert_array_almost_equal(ssOriginalMag,tfOriginalMag,decimal=3)
np.testing.assert_array_almost_equal(ssOriginalPhase,tfOriginalPhase,decimal=3)
np.testing.assert_array_almost_equal(ssOriginalMag,ssTransformedMag,decimal=3)
np.testing.assert_array_almost_equal(ssOriginalPhase,ssTransformedPhase,decimal=3)
np.testing.assert_array_almost_equal(tfOriginalMag,tfTransformedMag,decimal=3)
np.testing.assert_array_almost_equal(tfOriginalPhase,tfTransformedPhase,decimal=2)
def set_parameters(self,x,y):
self.a=x
self.b=y
from sympy.parsing.sympy_parser import parse_expr
#parse_expr(self.a)
#parse_expr(self.b)
G1= parse_expr(self.a)/parse_expr(self.b)
#parse_expr(G1)
num = Poly(G1.as_numer_denom()[0],s).all_coeffs()
den = Poly(G1.as_numer_denom()[1],s).all_coeffs()
num_coe_lenght = len(num)
den_coe_lenght = len(den)
tf = matlab.tf(map(float,num),map(float,den))
matlab.bode(tf)
plt.show()
def main():
print "This is an example of homework8 root locus extras"
s = sympy.Symbol("s")
S = 1 / ((s+1)*(s+2)*(s+10))
numerS = map(float, sympy.Poly(S.as_numer_denom()[0], s).all_coeffs())
denomS = map(float, sympy.Poly(S.as_numer_denom()[1], s).all_coeffs())
tf = matlab.tf(numerS, denomS)
gain = 164.5
damping_ratio = 0.174
print "provided test data :"
print "system =", tf, "\ngain =", gain,"\ndamping ratio =",damping_ratio
print "Gain ", rlx.gainFromDampingRatio(tf, damping_ratio)
print "Poles: ", rlx.polesFromTransferFunction(tf)
print "Overshoot: ", rlx.overshootFromDampingRatio(tf, damping_ratio)
print "Damping Ratio: ", rlx.dampingRatioFromGain(tf, gain)
print "Overshoot: ", rlx.overshootFromGain(tf, gain)
print "Frequency: ", rlx.frequencyFromGain(tf, gain)
def testConvert(self):
"""Test state space to transfer function conversion."""
verbose = self.debug
from control.statesp import _mimo2siso
#print __doc__
# Machine precision for floats.
eps = np.finfo(float).eps
for states in range(1, self.maxStates):
for inputs in range(1, self.maxIO):
for outputs in range(1, self.maxIO):
# start with a random SS system and transform to TF then
# back to SS, check that the matrices are the same.
ssOriginal = matlab.rss(states, outputs, inputs)
if (verbose):
self.printSys(ssOriginal, 1)
# Make sure the system is not degenerate
Cmat = control.ctrb(ssOriginal.A, ssOriginal.B)
if (np.linalg.matrix_rank(Cmat) != states):
if (verbose):
print(" skipping (not reachable)")
continue
Omat = control.obsv(ssOriginal.A, ssOriginal.C)
if (np.linalg.matrix_rank(Omat) != states):
if (verbose):
print(" skipping (not observable)")
continue
tfOriginal = matlab.tf(ssOriginal)
if (verbose):
self.printSys(tfOriginal, 2)
ssTransformed = matlab.ss(tfOriginal)
if (verbose):
self.printSys(ssTransformed, 3)
tfTransformed = matlab.tf(ssTransformed)
if (verbose):
self.printSys(tfTransformed, 4)
# Check to see if the state space systems have same dim
if (ssOriginal.states != ssTransformed.states):
print("WARNING: state space dimension mismatch: " + \
"%d versus %d" % \
(ssOriginal.states, ssTransformed.states))
# Now make sure the frequency responses match
# Since bode() only handles SISO, go through each I/O pair
# For phase, take sine and cosine to avoid +/- 360 offset
for inputNum in range(inputs):
for outputNum in range(outputs):
if (verbose):
print("Checking input %d, output %d" \
% (inputNum, outputNum))
ssorig_mag, ssorig_phase, ssorig_omega = \
control.bode(_mimo2siso(ssOriginal, \
inputNum, outputNum), \
deg=False, Plot=False)
ssorig_real = ssorig_mag * np.cos(ssorig_phase)
ssorig_imag = ssorig_mag * np.sin(ssorig_phase)
#
# Make sure TF has same frequency response
#
num = tfOriginal.num[outputNum][inputNum]
den = tfOriginal.den[outputNum][inputNum]
tforig = control.tf(num, den)
tforig_mag, tforig_phase, tforig_omega = \
control.bode(tforig, ssorig_omega, \
deg=False, Plot=False)
tforig_real = tforig_mag * np.cos(tforig_phase)
tforig_imag = tforig_mag * np.sin(tforig_phase)
np.testing.assert_array_almost_equal( \
ssorig_real, tforig_real)
np.testing.assert_array_almost_equal( \
ssorig_imag, tforig_imag)
#
# Make sure xform'd SS has same frequency response
#
ssxfrm_mag, ssxfrm_phase, ssxfrm_omega = \
control.bode(_mimo2siso(ssTransformed, \
inputNum, outputNum), \
ssorig_omega, \
deg=False, Plot=False)
ssxfrm_real = ssxfrm_mag * np.cos(ssxfrm_phase)
ssxfrm_imag = ssxfrm_mag * np.sin(ssxfrm_phase)
np.testing.assert_array_almost_equal( \
ssorig_real, ssxfrm_real)
np.testing.assert_array_almost_equal( \
ssorig_imag, ssxfrm_imag)
#
# Make sure xform'd TF has same frequency response
#
num = tfTransformed.num[outputNum][inputNum]
den = tfTransformed.den[outputNum][inputNum]
tfxfrm = control.tf(num, den)
tfxfrm_mag, tfxfrm_phase, tfxfrm_omega = \
control.bode(tfxfrm, ssorig_omega, \
deg=False, Plot=False)
tfxfrm_real = tfxfrm_mag * np.cos(tfxfrm_phase)
tfxfrm_imag = tfxfrm_mag * np.sin(tfxfrm_phase)
np.testing.assert_array_almost_equal( \
ssorig_real, tfxfrm_real)
np.testing.assert_array_almost_equal( \
ssorig_imag, tfxfrm_imag)
def transferFunction(self, N=15):
""" PID transfer function """
r = self.kd + self.ki * mtl.tf([1], [1, 0]) + self.kd * mtl.tf([1, 0], [self.kd / self.kp * N, 1])
return r
for i in range(0, len(data[0])) :
data_point = data[0][i][j]
if data_point.imag > 0 :
print "Damping Ratio : ", sin(atan(abs(-1*data_point.real / data_point.imag)))
def overshootFromGain(transferFunction, gain) :
data = matlab.rlocus(tf, array([gain]))
dampingRatio = 0
for j in range(0, len(data[0][0])):
for i in range(0, len(data[0])) :
data_point = data[0][i][j]
if data_point.imag > 0 :
dampingRatio = sin(atan(abs(-1*data_point.real / data_point.imag)))
exponent = -1*dampingRatio*( pi/sqrt(1 - dampingRatio**2))
overshoot = 100*exp(exponent)
print "Overshoot : ", overshoot
s = Symbol("s")
S = 1 / ((s+1)*(s+2)*(s+10))
numerS = map(float, Poly(S.as_numer_denom()[0], s).all_coeffs())
denomS = map(float, Poly(S.as_numer_denom()[1], s).all_coeffs())
tf = matlab.tf(numerS, denomS)
valuesFromDampingRatio(tf, 0.174)
polesFromTransferFunction(tf)
overshootFromDampingRatio(tf, 0.174)
dampingRatioFromGain(tf, 164.5)
overshootFromGain(tf, 164.5)
from pid_libs import Analysis, Controller, Plotter
import control.matlab as ctrl
import numpy as np
def step (T, t0):
u = []
for t in T:
if (t < t0):
u.append (0);
else:
u.append (1);
return u;
casos = [(0, 0), (0.5, 0), (1, 0), (0, 0.5), (0, 1), (0.5, 0.5), (0.5, 1), (1, 0.5), (1, 1), (1.5, 1.5)]
G = ctrl.tf (1, [1, 3, 3, 1]);
t = np.arange (0, 100, 0.01);
for i in range (10):
(b, c) = casos [i];
spec = {'Kc' : 0.9259 / (0.2705*0.8045), 'Ti' : 4 * 0.8045, 'b' : b, 'c' : c} # minimo IAE p77, l4
C = Controller(spec).Gc;
Cff = Controller(spec).Gff;
YR = Cff*G / (1 + C*G);
YL = G / (1 + C*G);
YN = 1 / (1 + C*G);
import control.matlab as ctrl
import numpy as np
import matplotlib.pyplot as plt
# Casos abordados para ponderacao de setpoint:
casos = [(0, 0), (0.5, 0), (1, 0), (0, 0.5), (0, 1), (0.5, 0.5), (0.5, 1), (1, 0.5), (1, 1), (1.5, 1.5)]
# Processo em malha aberta:
G = ctrl.tf (1, [1, 3, 3, 1]);
# Ultimate cycle ZN equivalent (Hay, 1998 (p. 188))
Kc = 0.4/ (0.2705*0.8045);
Ti = 3.2 * 0.8045;
Td = 0.5 * 0.8045;
# Controlador:
C = ctrl.tf ([Td*Ti, Kc*Ti, 1], [Ti, 0]);
# Dados de simulacao:
tF = 200;
t = np.linspace (0, tF, 1e4);
# Teste dos casos:
for i in range (10):
(b, c) = casos [i];
print "b = ", b, ", c = ", c
# Controlador feedforward:
Cff = ctrl.tf ([c*Td*Ti, Kc*b*Ti, 1], [Ti, 0]);
# coding: utf-8 import numpy as np from control import matlab from matplotlib import pylab as plt # 制御対象 num = [1] den = [1, 1.02, 0.02] P = matlab.tf(num, den) # 制御器 num = [50, 1] den = [50, 0] C = matlab.tf(num, den) matlab.gangof4(P, C) plt.show()
#from scipy.signal import step from stmcb import stmcb # MATLAB: # syst_fake=tf([1],[1 2 3]) # from matlab to python, commas must be used to separate elements # >> syst_fake # syst_fake = # # 1 # ------------- # s^2 + 2 s + 3 # # Continuous-time transfer function. syst_fake = tf([1],[1, 2, 3]) print syst_fake # Python: # 1 # ------------- # s^2 + 2 s + 3 # MATLAB: # zero order hold by default # syst_fake_dis=c2d(syst_fake,0.01) # syst_fake_dis = # # 4.967e-05 z + 4.934e-05 # ----------------------- # z^2 - 1.98 z + 0.9802
plt.legend()
plt.show()
################################################################################
# End PID test
################################################################################
################################################################################
# Transfer function test
################################################################################
# Open loop control system
# u -> R -> G -> y
# Process transfer function G(s)
G = mtl.tf([1,1],[2,1,1])
# PID Controller R(s)
R = pid.transferFunction()
# Open loop system transfer function
sys = R * G
print('System transfer function',sys)
# Bode plot
mtl.bode(sys)
################################################################################
# End transfer function test
################################################################################
def syms2tf(f, s): n, d = fraction(f.cancel()) n = map(float, Poly(n, s).all_coeffs()) d = map(float, Poly(d, s).all_coeffs()) return tf( n, d )
def __zp2tf(): z = 10*numpy.array([-1, -3, -8]) p = [-4, -35, -100, -200] return tf( *zpk2tf(z, p, 1) )
@author: Charles
"""
from __future__ import division
import numpy as np
from control.matlab import tf
from matplotlib import pyplot as plot
from numpy.linalg import lstsq
from control.statesp import _convertToStateSpace
# Exact Process Model
K = 10
numer = [1]
denom = [5,1]
sys1 = tf(numer,denom)
#denominator = tf([[5,1+K]])
#Transformation to State-Space
sys = _convertToStateSpace(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 = 30
dt = 0.05
tspan = np.arange(t_start, t_end, dt)
