# -*- coding: utf-8 -*- """ Level control of buffer tank with PI controller + Kalman filter for estimation of assumed unknown inflow. The full kalman filter algorithm is used, but after the for loop the steady state Kalman gain is also calculated with control.dlqe. Object-oriented programming (OOP) is used. Process model: dh_dt = (1/A_area)*(F_in - F_out) where: - h [m] is the water level in the tank - F_in [m^3/s] is water inflow - F_out [m^3/s] is water outflow - A_area [m^2] is cross-sesctional area of tank. The level control signal to the outlet pump is u = F_out [m^3/s] The controller is a standard PI controller tuned with the Skogestad method. The level meas has uniformly distributed random noise. Random process disturbance is not included in the simulator. --------------- Finn Aakre Haugen, finn@techteach.no 2025 04 12 """ #%% Imports: import matplotlib.pyplot as plt import numpy as np import control #%% Tank class class Tank: # Class constructor: def __init__(self, h_init, h_min, h_max, A_area, ts): self.h = h_init self.h_min = h_min self.h_max = h_max self.A_area = A_area self.ts = ts # Class method to predict h: def level_prediction(self, u, F_in): self.u = u self.F_in = F_in dh_dt = (self.F_in - self.u)/self.A_area self.h += self.ts * dh_dt self.h = np.clip(self.h, self.h_min, self.h_max) return self.h #%% PI controller class class PI_controller: # Class constructor: def __init__(self, Kc, Ti, u_man, u_min, u_max, u_i_init, ts): self.Kc = Kc self.Ti = Ti self.u_man = u_man self.u_min = u_min self.u_max = u_max self.u_i = u_i_init self.ts = ts # Class method to calculate controller output, u: def output_u(self, r, y): self.r = r self.y = y e = r - y # Control error u_p = Kc * e # P term self.u_i += (Kc*ts/Ti)*e # I term u_i_min = self.u_min - self.u_man u_i_max = self.u_max - self.u_man self.u_i = np.clip(self.u_i, u_i_min, u_i_max) # Anti windup (limit ui) self.u = self.u_man + u_p + self.u_i # PI term + man control self.u = np.clip(self.u, self.u_min, self.u_max) # Limitation of control return self.u #%% Kalman filter class class Kalman_filter: # Class constructor: def __init__(self, n_states, h_pred_init, F_in_pred_init, P_pred_init, G, Q, R, A_area, ts): self.n_states = n_states self.h_pred = h_pred_init self.F_in_pred = F_in_pred_init self.P_pred = P_pred_init self.G = G self.Q = Q self.R = R self.A_area = A_area self.ts = ts # Class method to estimate states: def state_estim(self, u, h_meas): # The A matrix in the linearized model: A_cont = np.array([[0, 1/self.A_area], [0, 0]]) A_disc = np.eye(self.n_states) + self.ts*A_cont A = A_disc # The C matrix in the linearized model: C = np.array([[1, 0]]) # Kalman gain: self.K=(self.P_pred@C.T)@(np.linalg.inv(C@self.P_pred@(C.T) +self.R)) # Innovation process: e = h_meas - self.h_pred # Meas-corrected estimates, used as applied estim: self.h_corr = self.h_pred + self.K[0,0] * e self.F_in_corr = self.F_in_pred + self.K[1,0] * e # Model-predicted estimates: dh_corr_dt = (self.F_in_corr - u)/self.A_area dF_in_corr_dt = 0 self.h_pred = self.h_corr + self.ts * dh_corr_dt self.F_in_pred = self.F_in_corr + self.ts * dF_in_corr_dt # Auto-covariance of error of meas-corrected estimate: self.P_corr = (np.eye(self.n_states)-self.K@C) @ self.P_pred # Auto-covariance of error of model-predicted estimate: self.P_pred = A@self.P_corr@(A.T) + self.G @ self.Q @ self.G.T return (self.h_corr, self.F_in_corr) #%% Function for SIMC PI tuning def fun_pi_tuning_integr_simc(Ki, Tc): Kc = 1/(Ki*Tc) # [(m3/s)/m] Controller gain Ti = 2*Tc # [s] Integrator gain return (Kc, Ti) #%% Time settings ts = 1 # Sim time-step [s] t_start = 0.0 # [s] t_stop = 8000 # [s] N_sim = int((t_stop-t_start)/ts) + 1 #%% Preallocation of arrays for plotting t_array = np.zeros(N_sim) h_meas_array = np.zeros(N_sim) r_array = np.zeros(N_sim) h_est_array = np.zeros(N_sim) u_array = np.zeros(N_sim) F_in_array = np.zeros(N_sim) F_in_est_array = np.zeros(N_sim) #%% Model parameters A_area = 2000 # [m2] h_min = 0 # [m] Max level av water h_max = 4 # [m] Min level av water #%% Controller parameters tcc = 1000 # [s] Specified closed loop time constant Ki = -1/A_area # Process integrator gain (Kc, Ti) = fun_pi_tuning_integr_simc(Ki, tcc) u_man = 3 # [m3/s] u_min = 0 # [m3/s] u_max = 8 # [m3/s] u_i_min = -8 # [m3/s] u_i_max = 8 # [m3/s] #%% Settings of Kalman Filter n_states = 2 # Number of states # Disturbance settings: G = np.eye(n_states) std_w_h = 0.001 # [m] kq = 1 # Alternative values: 0.2, 1.0, 5.0 std_w_Fin = 0.01*kq # [m3/s] Q = np.diag([std_w_h**2, std_w_Fin**2]) # Q_22 = 10 # Alternatives: 1 and 100 # Q = np.diag([1.0, Q_22]) # Measurement noise settings: ampl_h_meas_noise = 0.01 # [m] std_h_meas_noise = ampl_h_meas_noise/np.sqrt(3) # [m] R = np.diag([std_h_meas_noise**2]) #%% Initialization of Kalman Filter h_pred_init = 2.0 # [m] F_in_pred_init = 3.0 # [m3/s] P_pred_init = np.diag([0.01, 0.01]) #%% Initial states h = h_init = 2 # [m] Level u_i_init = 0 # [m3/s] I-term of PI controller #%% Class instantiations (creating objects) tank_1 = Tank(h_init, h_min, h_max, A_area, ts) pi_controller_1 = PI_controller(Kc, Ti, u_man, u_min, u_max, u_i_init, ts) kalman_filter_1 = Kalman_filter(n_states, h_pred_init, F_in_pred_init, P_pred_init, G, Q, R, A_area, ts) #%% For-loop for simulation for k in range(0, N_sim): t = k*ts # Selecting inputs: if (t < 1000): r = 2.0 F_in = 3.0 elif (t >= 1000): r = 2.0 F_in = 4.0 # Adding uniformly distributed meas noise: h_meas_noise = np.random.uniform(-ampl_h_meas_noise, ampl_h_meas_noise, 1)[0] h_meas = h + h_meas_noise # Level PI controller: u = pi_controller_1.output_u(r, h_meas) # Kalman filtering: (h_corr, F_in_corr) = kalman_filter_1.state_estim(u, h_meas) # Process simulation: h = tank_1.level_prediction(u, F_in) # Arrays for plotting: t_array[k] = t r_array[k] = r u_array[k] = u h_meas_array[k] = h_meas h_est_array[k] = h_corr F_in_array[k] = F_in F_in_est_array[k] = F_in_corr #%% Steady state Kalman gain print('Final value of K in kalman_filter_1 object:') print('Ks_final_iteration =', kalman_filter_1.K) print('Calculated by control.dlqe:') A_cont = np.array([[0, 1/A_area], [0, 0]]) A = np.eye(n_states) + ts*A_cont C = np.array([[1, 0]]) (Ks, P_corr, E) = control.dlqe(A, G, C, Q, R) print('Ks_dlqe =', Ks) #%% Plotting plt.close("all") plt.figure(num=1, figsize=(12, 9)) plt.subplot(3, 1, 1) plt.plot(t_array, r_array, 'g', label='r') plt.plot(t_array, h_meas_array, 'b', label='h_meas') plt.plot(t_array, h_est_array, 'r', label='h_est') plt.legend() plt.grid() plt.ylim(1.5, 2.5) plt.xlim(t_start, t_stop) # plt.xlabel('t [s]') plt.ylabel('[m]') plt.subplot(3, 1, 2) plt.plot(t_array, u_array, 'k', label='u = F_out') plt.legend() plt.grid() plt.ylim(2.5, 4.5) plt.xlim(t_start, t_stop) # plt.xlabel('t [s]') plt.ylabel('[m3/s]') plt.subplot(3, 1, 3) plt.plot(t_array, F_in_array, 'b', label='F_in') plt.plot(t_array, F_in_est_array, 'r', label='F_in_est') plt.legend() plt.grid() plt.ylim(2.5, 4.5) plt.xlim(t_start, t_stop) plt.xlabel('t [s]') plt.ylabel('[m3/s]') # plt.savefig('kalman_pi_buffer_tank_kq_0_2.pdf') plt.savefig('kalman_pi_buffer_tank_kq_1_0.pdf') # plt.savefig('kalman_pi_buffer_tank_kq_5_0.pdf') plt.show()