def attitude_sysid(y_acc, u_mix, verbose=False):
# type: (np.array, np.array, bool) -> (control.tf, float, float)
"""
roll/pitch system id assuming delay/gain model
:param y_acc: (roll or pitch acceleration)
:param u_mix: (roll or pitch acceleration command)
:param verbose: show debug output
: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 test_describing_function_plot():
# Simple linear system with at most 1 intersection
H_simple = ct.tf([1], [1, 2, 2, 1])
omega = np.logspace(-1, 2, 100)
# Saturation nonlinearity
F_saturation = ct.descfcn.saturation_nonlinearity(1)
amp = np.linspace(1, 4, 10)
# No intersection
xsects = ct.describing_function_plot(H_simple, F_saturation, amp, omega)
assert xsects == []
# One intersection
H_larger = H_simple * 8
xsects = ct.describing_function_plot(H_larger, F_saturation, amp, omega)
for a, w in xsects:
np.testing.assert_almost_equal(H_larger(1j * w),
-1 /
ct.describing_function(F_saturation, a),
decimal=5)
# Multiple intersections
H_multiple = H_simple * ct.tf(*ct.pade(5, 4)) * 4
omega = np.logspace(-1, 3, 50)
F_backlash = ct.descfcn.friction_backlash_nonlinearity(1)
amp = np.linspace(0.6, 5, 50)
xsects = ct.describing_function_plot(H_multiple, F_backlash, amp, omega)
for a, w in xsects:
np.testing.assert_almost_equal(-1 /
ct.describing_function(F_backlash, a),
H_multiple(1j * w),
decimal=5)
def test_nyquist_fbs_examples():
s = ct.tf('s')
"""Run through various examples from FBS2e to compare plots"""
plt.figure()
plt.title("Figure 10.4: L(s) = 1.4 e^{-s}/(s+1)^2")
sys = ct.tf([1.4], [1, 2, 1]) * ct.tf(*ct.pade(1, 4))
count = ct.nyquist_plot(sys)
assert _Z(sys) == count + _P(sys)
plt.figure()
plt.title("Figure 10.4: L(s) = 1/(s + a)^2 with a = 0.6")
sys = 1 / (s + 0.6)**3
count = ct.nyquist_plot(sys)
assert _Z(sys) == count + _P(sys)
plt.figure()
plt.title("Figure 10.6: L(s) = 1/(s (s+1)^2) - pole at the origin")
sys = 1 / (s * (s + 1)**2)
count = ct.nyquist_plot(sys)
assert _Z(sys) == count + _P(sys)
plt.figure()
plt.title("Figure 10.10: L(s) = 3 (s+6)^2 / (s (s+1)^2)")
sys = 3 * (s + 6)**2 / (s * (s + 1)**2)
count = ct.nyquist_plot(sys)
assert _Z(sys) == count + _P(sys)
plt.figure()
plt.title("Figure 10.10: L(s) = 3 (s+6)^2 / (s (s+1)^2) [zoom]")
count = ct.nyquist_plot(sys, omega_limits=[1.5, 1e3])
# Frequency limits for zoom give incorrect encirclement count
# assert _Z(sys) == count + _P(sys)
assert count == -1
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 pt1(smooth, time):
smoothed_df = pd.concat([pd.DataFrame(smooth, columns = ['smoothed']), \
pd.DataFrame(time, columns = ['time'])], axis = 1)
steady_state = smooth[-1]
#last element of the smoothed data is passed as steady state value
standard_output = steady_state * (1 - np.exp(-1))
#case when t = time_constant in the eqn.
#############################################################################
# standard_output = steady_state * (1 - e ^ (−t / time_constant)) #
#############################################################################
delay = smoothed_df.time[smoothed_df.smoothed < 0.02].values[-1]
#returns the time at which the step change occurs i.e a transition from 0 to some value.
#Values lesser than 0.02 were treatred as zero.
time_constant = smoothed_df.time[smoothed_df.index == abs(smoothed_df.smoothed - standard_output)\
.sort_values().index[0]].values[0]
#derived from the equation of standard_output
tf_pt1 = con.matlab.tf(steady_state, [time_constant, 1])
numerator, denominator = con.pade(delay, 1)
delay_tf_pt1 = con.matlab.tf(numerator, denominator)
yout_pt1, t_pt1 = con.matlab.step(tf_pt1 * delay_tf_pt1)
#first order transfer function is given by
###############################################################################
# steady_state * (e ^ - (delay * s)) #
# transfer_function(s) = ------------------------------------ #
# (time_constant * s + 1 ) #
###############################################################################
return tf_pt1 * delay_tf_pt1, yout_pt1, t_pt1, delay, time_constant, steady_state
def pt2(smooth, time):
def fourpoint(z):
f1_zeta = 0.451465 + (0.066696 * z) + (0.013639 * z**2)
f3_zeta = 0.800879 + (0.194550 * z) + (0.101784 * z**2)
f6_zeta = 1.202664 + (0.288331 * z) + (0.530572 * z**2)
f9_zeta = 1.941112 - (1.237235 * z) + (3.182373 * z**2)
return f1_zeta, f3_zeta, f6_zeta, f9_zeta
def method():
#the second method from the article is adopted, as the response/data handled strictly follows this method.
zeta = np.sqrt(
(np.log(overshoot)**2) / ((np.pi**2) + (np.log(overshoot)**2)))
f1_zeta, f3_zeta, f6_zeta, f9_zeta = fourpoint(zeta)
time_constant = (t9 - t1) / (f9_zeta - f1_zeta)
#delay = t1 - time_constant*f1_zeta #based on article.
delay = smoothed_df.time[smoothed_df.smoothed < 0.02].values[-1]
return zeta, time_constant, delay
smoothed_df = pd.concat([pd.DataFrame(smooth, columns = ['smoothed']), \
pd.DataFrame(time, columns = ['time'])], axis = 1)
steady_state = smooth[-1]
#ssn = steady state at n/10th instant of time
ss1 = steady_state * 0.1
ss3 = steady_state * 0.3
ss6 = steady_state * 0.6
ss9 = steady_state * 0.9
#tn = time at n/10th instant
t1 = smoothed_df.time[smoothed_df.index == abs(
smoothed_df.smoothed - ss1).sort_values().index[0]].values[0]
t3 = smoothed_df.time[smoothed_df.index == abs(
smoothed_df.smoothed - ss3).sort_values().index[0]].values[0]
t6 = smoothed_df.time[smoothed_df.index == abs(
smoothed_df.smoothed - ss6).sort_values().index[0]].values[0]
t9 = smoothed_df.time[smoothed_df.index == abs(
smoothed_df.smoothed - ss9).sort_values().index[0]].values[0]
peak = smoothed_df.smoothed.max()
#returns the highest output in the response
overshoot = (peak - steady_state) / steady_state
#represented as Mp in article
zeta, time_constant, delay = method()
tf_pt2 = con.matlab.tf(steady_state,
[time_constant**2, 2 * zeta * time_constant, 1])
n_2, d_2 = con.pade(delay, 1)
delay_tf_pt2 = con.matlab.tf(n_2, d_2)
yout_pt2, t_pt2 = con.matlab.step(tf_pt2 * delay_tf_pt2)
#second order transfer function is given by
########################################################################################################
# steady_state * (e ^ - (delay * s)) #
# transfer_function(s) = ---------------------------------------------------------------------- #
# ((time_constant ^ 2) * (s ^ 2)) + (2 * zeta * time_constant * s) + 1 ) #
########################################################################################################
return tf_pt2 * delay_tf_pt2, yout_pt2, t_pt2, delay, time_constant, steady_state, zeta
def ideal_pt1(ss_array, tc_array, d_array):
ideal_ss = np.average(ss_array)
ideal_tc = np.average(tc_array)
ideal_d = np.average(d_array)
ideal_tf_pt1 = con.matlab.tf(ideal_ss, [ideal_tc, 1])
numerator, denominator = con.pade(ideal_d, 1)
ideal_d_tf_pt1 = con.matlab.tf(numerator,denominator)
ideal_yout_pt1, ideal_t_pt1 = con.matlab.step(ideal_tf_pt1 * ideal_d_tf_pt1)
return ideal_tf_pt1 * ideal_d_tf_pt1, ideal_yout_pt1, ideal_t_pt1
def ideal_pt2(ss_array, tc_array, d_array, z_array):
ideal_ss = np.average(ss_array)
ideal_tc = np.average(tc_array)
ideal_d = np.average(d_array)
ideal_z = np.average(z_array)
ideal_tf_pt2 = con.matlab.tf(ideal_ss, [ideal_tc ** 2, 2 * ideal_z * ideal_tc, 1])
numerator, denominator = con.pade(ideal_d, 1)
ideal_d_tf_pt2 = con.matlab.tf(numerator,denominator)
ideal_yout_pt2, ideal_t_pt2 = con.matlab.step(ideal_tf_pt2 * ideal_d_tf_pt2)
return ideal_tf_pt2 * ideal_d_tf_pt2, ideal_yout_pt2, ideal_t_pt2
def ActuatorModel(bw, delay=(0, 1)):
# Inputs: ['cmd']
# Outputs: ['pos']
sysNom = control.tf2ss(control.tf(1, [1 / bw, 1]))
delayPade = control.pade(delay[0], n=delay[1])
sysDelay = control.tf2ss(control.tf(delayPade[0], delayPade[1]))
sys = sysDelay * sysNom
return sys
def attitude_sysid(y_acc, u_mix):
print('solving for plant model ', end='')
k, delay = delay_and_gain_sysid(y_acc, u_mix)
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 WoodBerry(model_id):
g11 = control.tf([12.8], [16.7, 1]) * control.tf(*control.pade(1, 1))
g12 = control.tf([-18.9], [21.0, 1]) * control.tf(*control.pade(3, 1))
g21 = control.tf([6.6], [10.9, 1]) * control.tf(*control.pade(7, 1))
g22 = control.tf([-19.4], [14.4, 1]) * control.tf(*control.pade(3, 1))
g1f = control.tf([3.8], [14.9, 1]) * control.tf(*control.pade(8, 1))
g2f = control.tf([4.9], [13.2, 1]) * control.tf(*control.pade(3, 1))
row_1_num = [x[0][0] for x in (g11.num, g12.num, g1f.num)]
row_2_num = [x[0][0] for x in (g21.num, g22.num, g2f.num)]
row_1_den = [x[0][0] for x in (g11.den, g12.den, g1f.den)]
row_2_den = [x[0][0] for x in (g21.den, g22.den, g2f.den)]
sys = control.tf([row_1_num, row_2_num], [row_1_den, row_2_den])
R = Tag(0, 'Reflux', 'Reflux flow rate', TagType.INPUT)
S = Tag(1, 'Steam', 'Steam flow rate', TagType.INPUT)
F = Tag(2, 'Feed', 'Feed flow rate', TagType.INPUT)
x_D = Tag(3, 'x_D', 'Distillate purity', TagType.OUTPUT)
x_B = Tag(4, 'x_B', 'Bottoms purity', TagType.OUTPUT)
return Model('Wood-Berry Distillation Model', sys,
OrderedDict([('R', R), ('S', S), ('F', F)]),
OrderedDict([('x_D', x_D), ('x_B', x_B)]), model_id)
def SensorModel(bw, delay=(0, 1)):
# Inputs: ['meas', 'dist']
# Outputs: ['sens']
sysNom = control.tf2ss(control.tf(1, [1 / bw, 1]))
delayPade = control.pade(delay[0], n=delay[1])
sysDelay = control.tf2ss(control.tf(delayPade[0], delayPade[1]))
sysDist = control.tf2ss(control.tf(1, 1))
sys = control.append(sysDelay * sysNom, sysDist)
sys.C = sys.C[0, :] + sys.C[1, :]
sys.D = sys.D[0, :] + sys.D[1, :]
sys.outputs = 1
return sys
def attitude_loop_design(m, name, dcut_hz, fbcut_hz, est_delay):
tf_est = control.tf(*control.pade(est_delay, 1))
G_rate_ol = model_to_acc_tf(m)
K_rate, G_rate_ol, G_rate_comp_cl = ut.lqrofb.pid_design(
G=G_rate_ol, K_guess=[0.2, 0.2, 0], dcut_hz=dcut_hz, fbcut_hz=fbcut_hz)
tf_integrator = control.tf([1], [1, 0])
G_ol = G_rate_comp_cl * tf_integrator * tf_est
K, G_ol, G_comp_cl = ut.lqrofb.pid_design(G=G_ol,
K_guess=[1],
dcut_hz=dcut_hz,
fbcut_hz=None,
use_I=False,
use_D=False)
return {
'MC_{:s}RATE_P'.format(name): K_rate[0, 0],
'MC_{:s}RATE_I'.format(name): K_rate[1, 0],
'MC_{:s}RATE_D'.format(name): K_rate[2, 0],
'MC_{:s}_P'.format(name): K[0, 0],
}
def test_fixed_view(self):
# respect xlim, ylim set by user
g = (tf([1], [1 / 1, 2 * 0.01 / 1, 1]) *
tf([1], [1 / 100**2, 2 * 0.001 / 100, 1]) * tf(*pade(0.01, 5)))
# normally a broad axis
nichols(g)
assert (plt.xlim()[0] == -1440)
assert (plt.ylim()[0] <= -240)
nichols(g, grid=False)
# zoom in
plt.axis([-360, 0, -40, 50])
# nichols_grid doesn't expand limits
nichols_grid()
assert (plt.xlim()[0] == -360)
assert (plt.ylim()[1] >= -40)
def ideal_pt1(ss_array, tc_array, d_array):
ideal_ss = np.average(ss_array)
ideal_tc = np.average(tc_array)
ideal_d = np.average(d_array)
ideal_tf_pt1 = con.matlab.tf(ideal_ss, [ideal_tc, 1])
numerator, denominator = con.pade(ideal_d, 1)
ideal_d_tf_pt1 = con.matlab.tf(numerator, denominator)
transfer_function = ideal_tf_pt1 * ideal_d_tf_pt1
#############################################################################################################################
# manual fitting of ideal pt1 model
# Based on the assumption that the plot of denominator of the transfer function has only one intercept in x-axis
# We have ax ^ 2 + bx + c = 0
# Therefore we change b such that b ^ 2 - 4 * a * c = 0
# For more fitting, we round down the result
transfer_function.den[0][0][1] = math.floor(
math.sqrt(4 * transfer_function.den[0][0][0] *
transfer_function.den[0][0][2]))
# To obtain the normal result, comment the above line
#############################################################################################################################
ideal_yout_pt1, ideal_t_pt1 = con.matlab.step(transfer_function)
return transfer_function, ideal_yout_pt1, ideal_t_pt1
#Code by P.Aashrith #April 28, 2020 #Released under GNU GPL import control import numpy as np import matplotlib.pyplot as plt #if using termux import subprocess import shlex #end if num = 4 den = [1,3,2,0] [n1,d1]=control.pade(0.1744,10) #Creating delay of 0.1744 [n2,d2]=control.pade(0.5,10) #Creating delay of 0.5 [n3,d3]=control.pade(0.0005,10) #Creating delay of 0.0005 #Creating a transfer function G = num/den G = control.tf(num,den) G1=control.tf(n1,d1) G2=control.tf(n2,d2) G3=control.tf(n3,d3) #assigning delay to system G G4=G*G1 G5=G*G2 G6=G*G3 #plotting nyquist plot with variable tau
sns.despine()
plt.savefig('ctl_id_2z.pdf')
H_ring.plot_step(figsize=(4, 3))
plt.ylim(ymax=1.1)
sns.despine()
plt.grid('on')
plt.savefig('ctl_id_2z_step.pdf')
pid = ms.PID(0.6, 1.0 * fs, 0.3 / fs)
pid = ms.PID(0, .8 * fs, 0. / fs)
delay_calc = 2e-3 # 2e-3
delay_adc = 0.5e-3
delay_dac = 0.5e-3
delay = delay_calc + delay_adc + delay_dac
H_delay = ms.TF(*control.pade(delay))
pid = ms.PID(0.9, 0.5 * fs, 0.15 / fs)
# H_delay = ms.TF([1],[1])
# H = pid*H_delay*H_lp
Hpid = pid * H_delay * H_lp
Gpid = H_ring / (1 + Hpid * H_ring)
Gpid.plot_hw(w=np.linspace(0.0001, 75) * 2 * np.pi,
bode=False,
xscale='linear',
yscale='db',
figsize=(4, 3))
sns.despine()
plt.savefig('ctl_freqresp_pid.pdf')
# https://etda.libraries.psu.edu/catalog/13071yxl5197
Alat = np.array([[-0.1180, 32.033, 0, 0], [0, -6.411, 1, 0],
[0, -20.639, 0, 1], [0, -1.235, 0, 0]])
Blat = np.array([[0], [0.0039974], [0.016275], [0.0040072]])
Clat = np.array([0.3048, 0, 0, 0])
Dlat = 0
sys_lat = control.ss(Alat, Blat, Clat, Dlat)
print(sys_lat)
# Test if system is controllable
ContMatrix = control.ctrb(Alat, Blat)
print("Rank of Controllability Matrix: %s" % np.linalg.matrix_rank(ContMatrix))
# Test if system is observable
ObsMatrix = control.obsv(Alat, Clat)
print("Rank of Observability Matrix: %s" % np.linalg.matrix_rank(ObsMatrix))
sys_tf_lat = control.ss2tf(sys_lat)
lat_delay = 0.156
delay_NUM, delay_DEN = control.pade(lat_delay, 4)
sys_tf_lat_delay = control.tf(delay_NUM, delay_DEN)
sys_tf_lat = sys_tf_lat * sys_tf_lat_delay
T, yout = control.step_response(sys_tf_lat * 17.3333)
plt.plot(T, yout)
plt.show()
# Fully controllable, fully observable. So we can employ state feedback
# = control.acker(Alat, Blat, [-10])
#print(K)
import numpy as np import control from matplotlib import pyplot as plt # definir las fdt sysgp = control.tf(10, [1, 1]) [numd, dend] = control.pade(0.01, 3) sysd = control.tf(numd, dend) sysgpd = sysgp * sysd #sysga=control.tf(1,[1/10,1]) sysga = control.tf(1, [1 / 10, 1]) sysgt = control.tf(1, 1) Kp = 0.1 Ti = 10 #Ki=1/Ti #Ki=0 Td = 0 N = 1000 sysp = control.tf(Kp, 1) sysi = control.tf(1, [Ti, 0]) sysd = control.tf([Td, 0], [1 / N, 1]) syspid = sysp + sysi + sysd #obtener la fdt en lazo cerrado mediante feedback syslc = control.feedback(syspid * sysga * sysgpd, sysgt) #t=np.arange(0,0.1,100) [t, y1] = control.step_response(4 * syslc, 100) # %% plt.figure(1) plt.plot(t, y1) plt.grid() plt.show()
import matplotlib.pyplot as plt
import control as ctr
import numpy as np
import math
counter = 0
Kmin = 0.09
K_p = 0.0910
kdrange = np.linspace(1, 5, 100) * Kmin
Tcrit = np.zeros(kdrange.size)
a, b = ctr.pade(0.4)
print(a, b)
sys1 = ctr.tf(a, b)
for kd in kdrange:
s = ctr.tf('s')
# G = ctr.tf([1121*math.exp(-s*0.4)*(K_p+s*kd)],[200*s**2*(1+s)])
G = (1121 * (K_p + s * kd)) / (200 * s**2 * (1 + s)) * sys1
gm, pm, Wcg, Wcp = ctr.margin(G)
Tcrit[counter] = pm * (2 * math.pi) / (360 * 0.88) + 0.4
counter += 1
fig1 = plt.figure()
plt.plot(kdrange, Tcrit)
plt.grid()
plt.show()
# help(ctr.tf)
def pade_model(tau, n=1):
approx = pade(tau, n)
approx = tf(approx[0], approx[1])
return approx
def WoodBerry():
g_tf = control.tf([12.8], [16.7, 1])
td = 1
g_delay = control.tf(control.pade(td, 1)[0], control.pade(td, 1)[1])
g11 = g_tf * g_delay
g_tf = control.tf([-18.9], [21.0, 1])
td = 3
g_delay = control.tf(control.pade(td, 1)[0], control.pade(td, 1)[1])
g12 = g_tf * g_delay
g_tf = control.tf([6.6], [10.9, 1])
td = 7
g_delay = control.tf(control.pade(td, 1)[0], control.pade(td, 1)[1])
g21 = g_tf * g_delay
g_tf = control.tf([-19.4], [14.4, 1])
td = 3
g_delay = control.tf(control.pade(td, 1)[0], control.pade(td, 1)[1])
g22 = g_tf * g_delay
g_tf = control.tf([3.8], [14.9, 1])
td = 8
g_delay = control.tf(control.pade(td, 1)[0], control.pade(td, 1)[1])
g1f = g_tf * g_delay
g_tf = control.tf([4.9], [13.2, 1])
td = 3
g_delay = control.tf(control.pade(td, 1)[0], control.pade(td, 1)[1])
g2f = g_tf * g_delay
row_1_num = [x[0][0] for x in (g11.num, g12.num, g1f.num)]
row_2_num = [x[0][0] for x in (g21.num, g22.num, g2f.num)]
row_1_den = [x[0][0] for x in (g11.den, g12.den, g1f.den)]
row_2_den = [x[0][0] for x in (g21.den, g22.den, g2f.den)]
sys = control.tf([row_1_num, row_2_num], [row_1_den, row_2_den])
R = Tag('Reflux', 'Reflux flow rate', IOtype='INPUT')
S = Tag('Steam', 'Steam flow rate', IOtype='INPUT')
F = Tag('Feed', 'Feed flow rate', IOtype='INPUT')
xD = Tag('x_D', 'Distillate purity', IOtype='OUTPUT')
xB = Tag('x_B', 'Bottoms purity', IOtype='OUTPUT')
inputs = {'R': R, 'S': S, 'F': F}
outputs = {'xD': xD, 'xB': xB}
wood_berry = Model(sys, "Wood Berry Distillation Model", inputs, outputs)
return wood_berry
def test_nyquist_arrows(arrows):
sys = ct.tf([1.4], [1, 2, 1]) * ct.tf(*ct.pade(1, 4))
plt.figure()
plt.title("L(s) = 1.4 e^{-s}/(s+1)^2 / arrows = %s" % arrows)
count = ct.nyquist_plot(sys, arrows=arrows)
assert _Z(sys) == count + _P(sys)
def model_to_acc_tf(m):
acc_tf = m['gain'] * control.tf(*control.pade(m['delay'],
1)) # order 1 approx
tf_integrator = control.tf((1), (1, 0))
return acc_tf * tf_integrator
