import control import numpy as np import matplotlib.pyplot as plt import math def tanks2i_rhs(t, x, u, params): F1, F2, k11, k22 = 0.5, 0.6, 0.8, 0.5 h1, h2 = x q01, q02 = u h2 = 0 if h2 < 0 else h2 # numerical issues h1 = h2 if h1 < h2 else h1 dh1 = (q01 - k11 * math.sqrt(h1 - h2)) / F1 dh2 = (q02 + k11 * math.sqrt(h1 - h2) - k22 * math.sqrt(h2)) / F2 return [dh1, dh2] tanks2i_io = control.NonlinearIOSystem( tanks2i_rhs, None, inputs=('q01','q02'), outputs=('h1', 'h2'), states=('h1', 'h2'), name='tanks2i') F1, F2, k11, k22 = 0.5, 0.6, 0.8, 0.5 q01s, q02s = 0.9, 0.0 h2s = ((q01s + q02s) / k22)**2 h1s = (q01s / k11)**2 + h2s q0s = np.array([q01s, q02s]) hs = np.array([h1s, h2s]) k1 = k11 / (2 * math.sqrt(h1s-h2s)) k2 = k22 / (2 * math.sqrt(h2s)) A = np.array([[-k1/F1, k1/F1], [k1/F2, -(k1+k2)/F2]]) Bbar = np.array([[1/F1, 0], [0, 1/F2]]) B = np.array([[1/F1], [0]]); Bd = np.array([[0], [1/F2]]) C = np.array([[0, 1]]) Dbar = np.array([[0, 0]]); D = 0 ssc = control.ss(A, B, C, D) ts = 1.5 SSd = control.sample_system(ssc, ts) Ad = SSd.A; Bd = SSd.B; Cd=SSd.C; Dd = SSd.D; # Dominant CL poles sig = 0.1; teps = 10 xi = abs(math.log(sig)) / math.sqrt(math.pi**2 + (math.log(sig))**2) om0 = 4 / (teps * xi) pp = np.array([1, 2 * xi * om0, om0**2]) controots = np.roots(pp) discroots = np.exp(controots * ts) K = control.place(Ad, Bd, discroots) Kw = 1 / (Cd @ np.linalg.inv(np.eye(2) - Ad + Bd @ K) @ Bd) fbff_ss_controller = control.NonlinearIOSystem( None, lambda t, x, u, params: np.clip(-K @ (u[1:] - hs) + Kw * (u[0] - hs[1]) + q01s, 0.05, 2.0), inputs=('w', 'h1', 'h2'), outputs=('q01'), name='control', dt = ts) clsys = control.interconnect( (tanks2i_io, fbff_ss_controller), name='clsio', connections=( ['tanks2i.q01', 'control.q01'], ['control.h1', 'tanks2i.h1'], ['control.h2', 'tanks2i.h2']), inplist=('control.w', 'tanks2i.q02'), inputs=('w', 'q02'), outlist=('tanks2i.h2', 'control.q01'), outputs=('h2', 'u')) T = np.linspace(0, 60, 200) w = [ 1.2 * hs[1] if t <= 25 else 0.9 * hs[1] for t in T] q02 = [ 0 if t <= 40 else .1 for t in T] ucl = np.vstack((np.array(w), np.array(q02))) t, y, x = control.input_output_response(clsys, T, ucl, hs, return_x=True) # plt.close('all') # plt.figure(1) # plt.subplot(121) # plt.step(T, w, label='setpoint'); plt.plot(t, y[0], label='h2'); plt.legend() # plt.xlabel("time"); plt.ylabel("$h_2^w, h_2$"); # plt.subplot(122) # plt.plot(T, y[1], label='q01'); plt.step(T, q02, label='q02'); plt.legend() # plt.xlabel("time"); plt.ylabel("$q_0$"); # plt.show(block=False) # iam_save_pdf(plt, "nlinfb.pdf", 22, 11)