def run(self,
fig_num,
is_noise=True,
noise_level=10**(-2),
output_csv=OUTPUT_CSV):
"""
run simulation and save result
:return:
"""
tout, self.y, x = signal.lsim2(self.system, self.u, self.t)
noise = np.zeros(len(self.t))
if is_noise:
noise = np.random.rand(len(self.t)) * noise_level * max(self.y)
self.y += noise
mycsv.save(self.t,
self.u,
self.y,
save_name=self.path + output_csv,
header=("t", "u", "y"))
myplot.plot(fig_num, self.t, self.u)
myplot.plot(fig_num, self.t, self.y)
myplot.save(fig_num,
label=("time [s]", "u/y []"),
save_name=self.path + "Simulation_Result",
leg=("u", "y"))
def plant_data(fig, datapath=DATA):
"""
return FRF
:return:
"""
s = symbols('s')
on = 2 * np.pi * np.array([0, 3950, 5400, 6100, 7100])
kappa = [1, -1, 0.4, -1.2, 0.9]
zeta = [0, 0.035, 0.015, 0.015, 0.06]
Kp = 3.7 * 10**7
ol = np.array([])
hl = np.array([])
fig += 1
for i in range(2):
for l in PERT:
if l == 0 and i > 0:
continue
P = 0
for o, k, z in zip(on, kappa, zeta):
o *= (1 + l * (i == 0))
k *= (1 + l * (i == 1))
z *= (1 + l * (i == 2))
P += k / (s**2 + 2 * z * o * s + o**2)
P *= Kp
# calc continous
p = mysignal.symbolic_to_tf(P, s, ts=0)
o = 2 * np.pi * np.linspace(10**0, 10**4, num=F)
o, mag, phase = signal.bode(p, w=o)
theta = phase / 180 * np.pi
a = 10**(mag / 20)
h = a * np.exp(1.j * theta)
# calc discrete with 3/10 delay
h = h * mysignal.zoh_w_delay(o, TS, TD)
myplot.bodeplot(fig,
o / 2 / np.pi,
20 * np.log10(abs(h)),
np.angle(h, deg=True),
line_style="-",
xl=[10**1, 10**4])
ol = np.append(ol, o)
hl = np.append(hl, h)
myplot.save(fig,
save_name=datapath + "/" + str(fig) + "_plant",
title="plant",
leg=["plant" + str(i) for i in range(NDATA)])
mycsv.save(ol,
np.real(hl),
np.imag(hl),
save_name=datapath + "/example1_plant_frf.csv",
header=("o (rad/s)", "real(FRF)", "imag(FRF)"))
assert len(ol) == len(hl)
return fig, np.array(ol), np.array(hl)
def plant(fig, datapath=DATA):
"""
return FRF
:return:
"""
s = symbols('s')
on = 2 * np.pi * np.array([0, 3950, 5400, 6100, 7100])
kappa = [1, -1, 0.4, -1.2, 0.9]
zeta = [0, 0.035, 0.015, 0.015, 0.06]
Kp = 3.7 * 10**7
ol = np.array([])
hl = np.array([])
fig += 1
P = 0
for o, k, z in zip(on, kappa, zeta):
P += k / (s**2 + 2 * z * o * s + o**2)
P *= Kp
# calc continous
p = mysignal.symbolic_to_tf(P, s, ts=0)
o = 2 * np.pi * np.linspace(10**0, 10**4, num=F)
o, mag, phase = signal.bode(p, w=o)
h = mysignal.magphase2resp(mag, phase)
# calc discrete with 3/10 delay
h = h * mysignal.zoh_w_delay(o, TS, TD)
mag, angle = mysignal.resp2magphase(h, deg=True)
myplot.bodeplot(fig,
o / 2 / np.pi,
mag,
angle,
line_style="-",
xl=[10**1, 10**4])
ol = np.append(ol, o)
hl = np.append(hl, h)
myplot.save(fig,
save_name=datapath + "/" + str(fig) + "_plant",
title="plant",
leg=["plant" + str(i) for i in range(NDATA)])
mycsv.save(ol,
np.real(hl),
np.imag(hl),
save_name=datapath + "/example1_plant_frf.csv",
header=("o (rad/s)", "real(FRF)", "imag(FRF)"))
assert len(ol) == len(hl)
return fig, np.array(ol), np.array(hl)
def save_theta_to_csv(n_den, theta, path):
"""
save parameter to csv
:param n_den:
:param theta:
:param path:
:return:
"""
num, den = theta[n_den:][::-1], theta[:n_den][::-1]
num = np.insert(num, 0, np.zeros(len(den) - len(num))) # padding
mycsv.save(
num,
den,
save_name=path,
header=
("num", "den",
"P(s) = (s^m * num[0] + ... + s * num[-2] + num[-1]) / (s^n * den[0] + ... + s * den[-2] + den[-1])"
))
def save_to_csv(self,
noi,
input_csv=INPUT_CSV,
lines_csv=LINES_CSV,
data_csv=DATA_CSV):
"""
save data to csv file (.csv)
:param noi:
:param data_path:
:param input_csv:
:param lines_csv:
:param data_csv:
:return:
"""
mycsv.save(self.t,
self.u,
save_name=self.path + input_csv,
header=("time t [s]", "excitation input u []"))
mycsv.save(self.l_lines,
self.f_lines,
save_name=self.path + lines_csv,
header=("lines []", "frequency lines [Hz]"))
mycsv.save(
(self.fs, ), (noi, ), (self.df, ), (self.r, ),
save_name=self.path + data_csv,
header=(
"Sampling Frequency [Hz]", "Number of iterations []",
"Minimum Frequency Resolution [Hz] = 1 / (Excitation Time [s])",
"rlog []"))
def fap_data(self, var=None, save_name=""):
"""
return frequecny, amplitude in dB, phase in degrees of signal FRF data or noise
:param var: if default: return FRF, elif var=="noise": return noise
:return:
"""
freq = None
gyu = None
if var is None:
# [average(G(f)) for f in f_lines]
freq = self.f_lines
gyu = [g[self.Y] / g[self.U] for g in self.uy_av_f]
elif var == "noise":
freq = self.freq_noise
gyu = [g[self.Y] / 1 for g in self.uy_noise_f]
if save_name:
mycsv.save(freq * 2 * np.pi,
np.real(gyu),
np.imag(gyu),
save_name=save_name,
header=("o (rad/s)", "Re", "Im"))
gain = 20 * np.log10(np.abs(gyu))
phi = np.angle(gyu, True) # in degree
return freq, gain, phi
def optimize(fig, o, g, datapath=DATA):
"""
calc pids and firs
:param o:
:param g:
:return:
"""
THETA_DPM = 30 / 180 * np.pi # Phase Margin
THETA_DPM2 = 30 / 180 * np.pi # Second Phase Margin
GDB_DGM = 5 # Gain Margin in (dB)
NSTBITER = 1
if True:
# PID Triple Poleassignment 400 Hz
rho = [
np.array([
0.000101889491467777, 0.170717481532357, 107.264957164280,
9.94718394324346e-05, 1
]),
]
rho0 = rho
f = 300
_f = [0]
_c = [None]
rho_best = rho
R = 1.1
LAMBDA = (1 + R) / R / 2
tol = 20
while tol > 0:
# rho = rho0
print("Try: ", f, " Hz")
fbc = LFControllerDesign(o, g, TS, rho, ctype=["pid", 0, 0])
F_DGC = 2 * np.pi * f # Desired Cross-over Frequency (rad/s)
fbc.specification(F_DGC, THETA_DPM, GDB_DGM,
theta_dpm2=THETA_DPM2) # set constraints
fbc.controller()
for i in range(NSTBITER):
fbc.nominalcond(db=-60) # append nominal performance condition
fbc.stabilitycond() # append stability condition
fbc.gainpositivecond() # append gain constraints
fbc.gaincond()
try:
rho = fbc.optimize()
except:
tol -= 1
f = f * LAMBDA
rho = rho_best
break
fbc.reset()
fbc.controller()
taud = fbc.rho[0][-2] / fbc.rho[0][-1]
pid = btaud2pid([x / fbc.rho[0][-1] for x in fbc.rho[0][:3]], taud)
print("rho:", fbc.rho[0])
print("PIDs:", pid)
print("TAU:", taud)
print()
for ino in range(fbc.nonotch):
d1, d2, c1 = fbc.rho[1 + ino]
on = np.sqrt(d2)
zeta = c1 / 2 / on
d = d1 / c1
print("rho:", fbc.rho[1 + ino])
print("on: ", on)
print("zeta:", zeta)
print("d:", d)
print()
if fbc.nopc:
print("rho:", fbc.rho[fbc.nonotch + 1])
print("2*pi*fnum ", fbc.rho[fbc.nonotch + 1][0])
print("2*pi*fden:", fbc.rho[fbc.nonotch + 1][1])
print()
if i >= NSTBITER // 2:
if check_disk(fbc.L, fbc.rm, fbc.sigma):
print("Solver found a local minima @ iteration", i)
if f > max(_f):
rho_best = rho
print("best @", f)
print()
_f.append(f)
_c.append(fbc)
f *= R
break
else:
# rho = rho0
print("stability condition violation")
tol -= 1
print("-" * 50)
print()
for e in range(11, 0, -1):
print((11 - e) * ' ' + e * '*')
print('')
for g in range(11, 0, -1):
print(g * ' ' + (11 - g) * '*')
assert _f[-1] > 0
i_max = [i for i, f in enumerate(_f) if f == max(_f)][0]
print("Best nominal frequency:", _f[i_max], " Hz")
fbc = _c[i_max]
print("rho:", fbc.rho)
taud = fbc.rho[0][-2] / fbc.rho[0][-1]
pid = btaud2pid([x / fbc.rho[0][-1] for x in fbc.rho[0][:3]], taud)
print("PIDs:", pid)
print("TAU:", taud)
print()
for ino in range(fbc.nonotch):
d1, d2, c1 = fbc.rho[1 + ino]
on = np.sqrt(d2)
zeta = c1 / 2 / on
d = d1 / c1
print("rho:", fbc.rho[1 + ino])
print("on: ", on)
print("zeta:", zeta)
print("d:", d)
print()
if fbc.nopc:
print("rho:", fbc.rho[fbc.nonotch + 1])
print("2*pi*fnum ", fbc.rho[fbc.nonotch + 1][0])
print("2*pi*fden:", fbc.rho[fbc.nonotch + 1][1])
print()
mycsv.save([y for x in rho for y in x],
save_name=DATA + "/rho" + str(fig) + ".csv",
header=())
return fig, fbc
def optimize(fig, o, g, datapath=DATA):
"""
calc pids and firs
:param o:
:param g:
:return:
"""
THETA_DPM = 30 / 180 * np.pi # Phase Margin
THETA_DPM2 = 30 / 180 * np.pi # Second Phase Margin
GDB_DGM = 5 # Gain Margin in (dB)
NSTBITER = 4
TAUD = 5 * TS # Pseudo Differential Cut-off for D Control
NOFIR = 0 # Only PID
NOPID = "pid"
f = 10
_f = [0]
_c = [None]
rho = None
rho_best = rho
R = 1.5
LAMBDA = (1 + R) / R / 2
tol = 15
while tol > 0:
F_DGC = 2 * np.pi * f # Desired Cross-over Frequency (rad/s)
print("Try: ", f, " Hz")
for i in range(NSTBITER):
fbc = ControllerDesign(o,
g,
nopid=NOPID,
taud=TAUD,
nofir=NOFIR,
ts=TS,
tsfir=TS,
rho0=rho)
fbc.specification(F_DGC, THETA_DPM, GDB_DGM,
theta_dpm2=THETA_DPM2) # set constraints
fbc.nominalcond(db=-60) # append nominal performance condition
fbc.stabilitycond() # append stability condition
fbc.gainpositivecond() # append gain constraints
try:
rho = fbc.optimize()
except:
tol -= 1
f = f * LAMBDA
rho = rho_best
break
if i >= NSTBITER // 2 and check_disk(np.dot(fbc.X, fbc.rho),
fbc.rm, fbc.sigma):
print("Solver found a local minima @ iteration", i)
if f > max(_f):
rho_best = rho
print("best @", f)
print()
_f.append(f)
_c.append(fbc)
f *= R
break
for e in range(11, 0, -1):
print((11 - e) * ' ' + e * '*')
print('')
for g in range(11, 0, -1):
print(g * ' ' + (11 - g) * '*')
assert _f[-1] > 0
i_max = [i for i, f in enumerate(_f) if f == max(_f)][0]
print("Best nominal frequency:", _f[i_max], " Hz")
fbc = _c[i_max]
print("PIDs:", fbc.rho[:3])
print("FIRs:", fbc.rho[3:])
mycsv.save(fbc.rho,
save_name=DATA + "/rho" + str(fig) + ".csv",
header=("P,I,D, FIR(1+n) for n in range(" + str(NOFIR) + ")",
"taud (s):" + str(TAUD), "FIR sampling (s):" + str(TS)))
return fig, fbc
def optimize(fig, o, g, datapath=DATA):
"""
calc pids and firs
:param o:
:param g:
:return:
"""
THETA_DPM = 30 / 180 * np.pi # Phase Margin
THETA_DPM2 = 30 / 180 * np.pi # Second Phase Margin
GDB_DGM = 5 # Gain Margin in (dB)
NSTBITER = 1
if True:
# PID Triple Poleassignment 400 Hz
rhon = np.array(
[0.000101889491467777, 0.170717481532357, 107.264957164280])
rhod = np.array([9.94718394324346 * 10**(-5), 1])
elif False:
# PID 400 Hz + 5400 Hz Notch
rhon = np.array([
0.000178306610068610, 0.846420925630186, 368574.319724049,
1077291892.56301, 1183397129740.58
])
rhod = np.array([
5.68410511042483e-05, 1.51578813420334, 126083.578322355,
2058536168.05419
])
elif False:
rhon = np.array([
0.000127361864334721, 0.787836113815149, 676560.592842885,
2760708571.14940, 853187331902783, 1.78089599036966e+18,
1.39697403340072e+21
])
rhod = np.array([
7.95774715459477e-05, 4.43955832426420, 489464.466739361,
13540677611.4662, 630318001739791, 6.66807430879530e+18
])
else:
# PID 300 Hz + Phase Compensator + Notch
rhon = np.array([
7.64171186008328e-05, 6.23379530477791, 175952.995822072,
12564320886.3040, 15618222242330.9, 7.31297798873055e+15
])
rhod = np.array([
0.000132629119243246, 5.51210670192588, 337065.291819523,
9095778966.23193, 51352787209705.0
])
f = 300
_f = [0]
_c = [None]
rho = None
rho_best = rho
R = 1.1
LAMBDA = (1 + R) / R / 2
tol = 10
while tol > 0:
print("Try: ", f, " Hz")
fbc = IIRControllerDesign(o, g, ts=TS, rhon=rhon, rhod=rhod)
F_DGC = 2 * np.pi * f # Desired Cross-over Frequency (rad/s)
fbc.specification(F_DGC, THETA_DPM, GDB_DGM,
theta_dpm2=THETA_DPM2) # set constraints
for i in range(NSTBITER):
fbc.nominalcond(db=-60) # append nominal performance condition
fbc.stabilitycond() # append stability condition
fbc.gainpositivecond() # append gain constraints
try:
rho = fbc.optimize()
except:
tol -= 1
f = f * LAMBDA
rho = rho_best
break
rhon = fbc.rhon
rhod = fbc.rhod
fbc.controller()
fbc.reset()
taud = fbc.rhod[0] / fbc.rhod[1]
pid = btaud2pid([x / fbc.rhod[1] for x in fbc.rhon], taud)
print("rho:", fbc.rho)
print("PIDs:", pid)
print("TAU:", taud)
print()
if i >= NSTBITER // 2 and check_disk(fbc.L, fbc.rm, fbc.sigma):
print("Solver found a local minima @ iteration", i)
if f > max(_f):
rho_best = rho
print("best @", f)
print()
_f.append(f)
_c.append(fbc)
f *= R
break
for e in range(11, 0, -1):
print((11 - e) * ' ' + e * '*')
print('')
for g in range(11, 0, -1):
print(g * ' ' + (11 - g) * '*')
assert _f[-1] > 0
i_max = [i for i, f in enumerate(_f) if f == max(_f)][0]
print("Best nominal frequency:", _f[i_max], " Hz")
fbc = _c[i_max]
taud = fbc.rhod[0] / fbc.rhod[1]
pid = btaud2pid([x / fbc.rhod[1] for x in fbc.rhon], taud)
print("rho:", fbc.rho)
print("PIDs:", pid)
print("TAU:", taud)
mycsv.save([*pid, taud],
save_name=DATA + "/rho" + str(fig) + ".csv",
header=("taud (s):" + str(taud), "FIR sampling (s):" + str(TS)))
return fig, fbc
