def experiment(cfg): control(cfg) ts = 1.0 A = np.array([[0.89, 0.], [0., 0.45]]) B = np.array([[0.3], [2.5]]) C = np.array([[0.7, 1.]]) D = np.array([[0.0]]) tfin = 500 npts = int(old_div(tfin, ts)) + 1 Time = np.linspace(0, tfin, npts) # Input sequence U = np.zeros((1, npts)) U[0] = fset.GBN_seq(npts, 0.05) ##Output x, yout = fsetSIM.SS_lsim_process_form(A, B, C, D, U) # measurement noise noise = fset.white_noise_var(npts, [0.05]) # Output with noise y_tot = yout + noise ##System identification method = 'PARSIM-S' sys_id = system_identification(y_tot, U, method, SS_fixed_order=2)
from sippy import functionset as fset import numpy as np import control.matlab as cnt import matplotlib.pyplot as plt ## TEST RECURSIVE IDENTIFICATION METHODS # Define sampling time and Time vector sampling_time = 1. # [s] end_time = 400 # [s] npts = int(old_div(end_time, sampling_time)) + 1 Time = np.linspace(0, end_time, npts) # Define Generalize Binary Sequence as input signal switch_probability = 0.08 # [0..1] [Usim, _, _] = fset.GBN_seq(npts, switch_probability, Range=[-1, 1]) # Define white noise as noise signal white_noise_variance = [0.01] e_t = fset.white_noise_var(Usim.size, white_noise_variance)[0] # ## Define the system (ARMAX model) # ### Numerator of noise transfer function has two roots: nc = 2 NUM_H = [1., 0.3, 0.2, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.] # ### Common denominator between input and noise transfer functions has 4 roots: na = 4 DEN = [ 1., -2.21, 1.7494, -0.584256, 0.0684029, 0., 0., 0., 0., 0., 0., 0., 0.,
fx = np.append(dx_0, dx_1) return fx # Build input sequences U = np.zeros((m, npts)) # manipulated inputs as GBN # Input Flow rate Fin = F = U[0] [m^3/min] prob_switch_1 = 0.05 F_min = 0.4 F_max = 0.6 Range_GBN_1 = [F_min, F_max] U[0, :] = fset.GBN_seq(npts, prob_switch_1, Range=Range_GBN_1) # Steam Flow rate W = U[1] [kg/min] prob_switch_2 = 0.05 W_min = 20 W_max = 40 Range_GBN_2 = [W_min, W_max] U[1, :] = fset.GBN_seq(npts, prob_switch_2, Range=Range_GBN_2) # disturbance inputs as RW (random-walk) # Input Concentration Ca_in = U[2] [kg salt/m^3 solution] Ca_0 = 10.0 # initial condition sigma_Ca = 0.01 # variation U[2, :] = fset.RW_seq(npts, Ca_0, sigma=sigma_Ca) # Input Temperature T_in [°C] Tin_0 = 25.0 # initial condition
# sample time ts = 1.0 # SISO SS system (n = 2) A = np.array([[0.89, 0.], [0., 0.45]]) B = np.array([[0.3], [2.5]]) C = np.array([[0.7, 1.]]) D = np.array([[0.0]]) tfin = 500 npts = int(old_div(tfin, ts)) + 1 Time = np.linspace(0, tfin, npts) # Input sequence U = np.zeros((1, npts)) [U[0],_,_] = fset.GBN_seq(npts, 0.05) ##Output x, yout = fsetSIM.SS_lsim_process_form(A, B, C, D, U) # measurement noise noise = fset.white_noise_var(npts, [0.15]) # Output with noise y_tot = yout + noise # plt.close("all") plt.figure(0) plt.plot(Time, U[0]) plt.ylabel("input")
g_sample32 = cnt.tf(NUM32, DEN3, ts) g_sample33 = cnt.tf(NUM33, DEN3, ts) g_sample34 = cnt.tf(NUM34, DEN3, ts) H_sample1 = cnt.tf(H1, DEN1, ts) H_sample2 = cnt.tf(H2, DEN2, ts) H_sample3 = cnt.tf(H3, DEN3, ts) # tfin = 400 npts = int(old_div(tfin, ts)) + 1 Time = np.linspace(0, tfin, npts) # #INPUT# Usim = np.zeros((4, npts)) Usim_noise = np.zeros((4, npts)) Usim[0, :] = fset.GBN_seq(npts, 0.03, [-0.33, 0.1]) Usim[1, :] = fset.GBN_seq(npts, 0.03) Usim[2, :] = fset.GBN_seq(npts, 0.03, [2.3, 5.7]) Usim[3, :] = fset.GBN_seq(npts, 0.03, [8., 11.5]) # Adding noise err_inputH = np.zeros((4, npts)) err_inputH = fset.white_noise_var(npts, var_list) err_outputH = np.ones((3, npts)) err_outputH1, Time, Xsim = lsim(H_sample1, err_inputH[0, :], Time) err_outputH2, Time, Xsim = lsim(H_sample2, err_inputH[1, :], Time) err_outputH3, Time, Xsim = lsim(H_sample3, err_inputH[2, :], Time) Yout11, Time, Xsim = lsim(g_sample11, Usim[0, :], Time)
from sippy import functionsetSIM as fsetSIM ts = 1.0 A = np.array([[0.89, 0.], [0., 0.45]]) B = np.array([[0.3], [2.5]]) C = np.array([[0.7, 1.]]) D = np.array([[0.0]]) tfin = 500000 npts = int(old_div(tfin, ts)) + 1 Time = np.linspace(0, tfin, npts) # Input sequence U = np.zeros((1, npts)) U[0] = fset.GBN_seq(npts, 0.05) ##Output x, yout = fsetSIM.SS_lsim_process_form(A, B, C, D, U) # measurement noise noise = fset.white_noise_var(npts, [0.15]) # Output with noise y_tot = yout + noise ##System identification method = 'N4SID' sys_id = system_identification(y_tot, U, method, SS_fixed_order=2) xid, yid = fsetSIM.SS_lsim_process_form(sys_id.A, sys_id.B, sys_id.C, sys_id.D, U, sys_id.x0)
dx_1 = (ro*cp*U[0]*U[3] - ro*cp*U[0]*X[1] + U[1]*Lam)/(V*ro*cp) fx = np.append(dx_0,dx_1) return fx # Build input sequences U = np.zeros((m,npts)) # manipulated inputs as GBN # Input Flow rate Fin = F = U[0] [m^3/min] prob_switch_1 = 0.05 F_min = 0.4 F_max = 0.6 Range_GBN_1 = [F_min,F_max] [U[0,:],_,_] = fset.GBN_seq(npts, prob_switch_1, Range = Range_GBN_1) # Steam Flow rate W = U[1] [kg/min] prob_switch_2 = 0.05 W_min = 20 W_max = 40 Range_GBN_2 = [W_min,W_max] [U[1,:],_,_] = fset.GBN_seq(npts, prob_switch_2, Range = Range_GBN_2) # disturbance inputs as RW (random-walk) # Input Concentration Ca_in = U[2] [kg salt/m^3 solution] Ca_0 = 10.0 # initial condition sigma_Ca = 0.01 # variation U[2,:] = fset.RW_seq(npts, Ca_0, sigma = sigma_Ca) # Input Temperature T_in [°C] Tin_0 = 25.0 # initial condition
g_sample32 = cnt.tf(NUM32, DEN3, ts) g_sample33 = cnt.tf(NUM33, DEN3, ts) g_sample34 = cnt.tf(NUM34, DEN3, ts) H_sample1 = cnt.tf(H1, DEN1, ts) H_sample2 = cnt.tf(H2, DEN2, ts) H_sample3 = cnt.tf(H3, DEN3, ts) # tfin = 400 npts = int(old_div(tfin, ts)) + 1 Time = np.linspace(0, tfin, npts) # #INPUT# Usim = np.zeros((4, npts)) Usim_noise = np.zeros((4, npts)) [Usim[0, :], _, _] = fset.GBN_seq(npts, 0.03, Range=[-0.33, 0.1]) [Usim[1, :], _, _] = fset.GBN_seq(npts, 0.03) [Usim[2, :], _, _] = fset.GBN_seq(npts, 0.03, Range=[2.3, 5.7]) [Usim[3, :], _, _] = fset.GBN_seq(npts, 0.03, Range=[8., 11.5]) # Adding noise err_inputH = np.zeros((4, npts)) err_inputH = fset.white_noise_var(npts, var_list) err_outputH1, Time, Xsim = cnt.lsim(H_sample1, err_inputH[0, :], Time) err_outputH2, Time, Xsim = cnt.lsim(H_sample2, err_inputH[1, :], Time) err_outputH3, Time, Xsim = cnt.lsim(H_sample3, err_inputH[2, :], Time) # OUTPUTS Yout = np.zeros((3, npts))
def control(cfg): log.info("============= Configuration =============") log.info(f"Config:\n{cfg.pretty()}") log.info("=========================================") dt = cfg.sys.params.dt # System matrices A = np.mat(cfg.sys.params.A) B = np.mat(cfg.sys.params.B) C = np.mat(cfg.sys.params.C) D = np.mat(cfg.sys.params.D) system = StateSpace(A, B, C, D, dt) dx = np.shape(A)[0] # Check controllability Wc = ctrb(A, B) print("Wc = ", Wc) print(f"Rank of controllability matrix is {np.linalg.matrix_rank(Wc)}") # Check Observability Wo = obsv(A, C) print("Wo = ", Wo) print(f"Rank of observability matrix is {np.linalg.matrix_rank(Wo)}") fdbk_poles = np.random.uniform(0, .1, size=(np.shape(A)[0])) obs_poles = np.random.uniform(0, .01, size=(np.shape(A)[0])) observer = full_obs(system, obs_poles) K = place(system.A, system.B, fdbk_poles) low = -np.ones((dx, 1)) high = np.ones((dx, 1)) x0 = np.random.uniform(low, high) env = gym.make(cfg.sys.name) env.setup(cfg) y = env.reset() x_obs = x0 # code for testing observers... they work! # for t in range(100): # action = controller(K).get_action(env._get_state()) # y, rew, done, _ = env.step(action) # x_obs = np.matmul(observer.A, x_obs) + np.matmul(observer.B, np.concatenate((action, y))) # print(f"Time {t}, mean sq state error is {np.mean(env._get_state()-x_obs)**2}") log.info(f"Running rollouts on system {cfg.sys.name}") n_random = 1000 # states_r, actions_r, reward_r = rollout(env, random_controller(env), n_random) # log.info(f"Random rollout, reward {np.sum(reward_r)}") states_r = [] actions_r = [] reward_r = [] # Input sequence U = np.zeros((1, n_random)) U[0] = fset.GBN_seq(n_random, 0.05) ##Output x, yout = fsetSIM.SS_lsim_process_form(A, B, C, D, U) # measurement noise noise = fset.white_noise_var(n_random, [0.15]) # Output with noise y_tot = yout + noise ##System identification method = 'N4SID' system_initial = system_identification( y_tot, U, method, SS_fixed_order=np.shape(A)[0]) #, tsample=dt) system_initial.dt = dt print(A) print(np.round(system_initial.A, 2)) xid, yid = fsetSIM.SS_lsim_process_form(system_initial.A, system_initial.B, system_initial.C, system_initial.D, U, system_initial.x0) # system_initial = sys_id(actions_r, states_r, dt, method='N4SID', order=dx, SS_fixed_order=np.shape(A)[0]) # system_initial.dt = dt observer = full_obs(system_initial, obs_poles) K = place(system_initial.A, system_initial.B, fdbk_poles) # print(np.array(cfg.sys.params.A)) # TODO integrate observer so that we can use state feedback log.info( f"Running {cfg.experiment.num_trials} trials with SI and feedback control" ) for i in range(cfg.experiment.num_trials): # Rollout at this step # states, actions, reward = rollout(env, controller(K), cfg.experiment.trial_timesteps) states, actions, reward = rollout_observer( env, controller(K), observer, cfg.experiment.trial_timesteps) [states_r.append(s) for s in states] [actions_r.append(a) for a in actions] [reward_r.append(r) for r in reward] # Run system ID on the dataset # system = sys_id(actions, states, order=dx) # partial system = sys_id(actions_r, states_r, dt, order=dx) # full system.dt = dt observer = full_obs(system, obs_poles) print(system.A.round(2)) # Create controller K = place(system.A, system.B, fdbk_poles) log.info(f"Rollout {i}: reward {np.sum(reward)}") plot_rollout(states_r, actions_r)
g_sample33 = cnt.tf(NUM33, DEN3, ts) g_sample34 = cnt.tf(NUM34, DEN3, ts) H_sample1 = cnt.tf(H1, DEN1, ts) H_sample2 = cnt.tf(H2, DEN2, ts) H_sample3 = cnt.tf(H3, DEN3, ts) # time tfin = 400 npts = int(old_div(tfin, ts)) + 1 Time = np.linspace(0, tfin, npts) #INPUT# Usim = np.zeros((4, npts)) Usim_noise = np.zeros((4, npts)) [Usim[0, :], _, _] = fset.GBN_seq(npts, 0.03, Range=[-0.33, 0.1]) [Usim[1, :], _, _] = fset.GBN_seq(npts, 0.03) [Usim[2, :], _, _] = fset.GBN_seq(npts, 0.03, Range=[2.3, 5.7]) [Usim[3, :], _, _] = fset.GBN_seq(npts, 0.03, Range=[8., 11.5]) # Adding noise err_inputH = np.zeros((4, npts)) err_inputH = fset.white_noise_var(npts, var_list) err_outputH = np.ones((3, npts)) err_outputH1, Time, Xsim = cnt.lsim(H_sample1, err_inputH[0, :], Time) err_outputH2, Time, Xsim = cnt.lsim(H_sample2, err_inputH[1, :], Time) err_outputH3, Time, Xsim = cnt.lsim(H_sample3, err_inputH[2, :], Time) # Noise-free output Yout11, Time, Xsim = cnt.lsim(g_sample11, Usim[0, :], Time)
from sippy import * import numpy as np import control.matlab as cnt import matplotlib.pyplot as plt # ## Define sampling time and Time vector sampling_time = 1. # [s] end_time = 400 # [s] npts = int(old_div(end_time, sampling_time)) + 1 Time = np.linspace(0, end_time, npts) # ## Define pseudo random binary sequence as input signal and white noise as noise signal switch_probability = 0.08 # [0..1] Usim = fset.GBN_seq(npts, switch_probability) white_noise_variance = [0.005] e_t = fset.white_noise_var(Usim.size, white_noise_variance)[0] # ## Define the system # ### Numerator of noise transfer function has only one root: nc = 1 NUM_H = [1., 0.3, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.] # ### Common denominator between input and noise transfer functions has 4 roots: na = 4 DEN = [ 1., -2.21, 1.7494, -0.584256, 0.0684029, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0. ]
g_sample32 = cnt.tf(NUM32, DEN3, ts) g_sample33 = cnt.tf(NUM33, DEN3, ts) g_sample34 = cnt.tf(NUM34, DEN3, ts) H_sample1 = cnt.tf(H1, DEN1, ts) H_sample2 = cnt.tf(H2, DEN2, ts) H_sample3 = cnt.tf(H3, DEN3, ts) # tfin = 400 npts = int(old_div(tfin, ts)) + 1 Time = np.linspace(0, tfin, npts) # #INPUT# Usim = np.zeros((4, npts)) Usim_noise = np.zeros((4, npts)) Usim[0, :] = fset.GBN_seq(npts, 0.03, [-0.33, 0.1]) Usim[1, :] = fset.GBN_seq(npts, 0.03) Usim[2, :] = fset.GBN_seq(npts, 0.03, [2.3, 5.7]) Usim[3, :] = fset.GBN_seq(npts, 0.03, [8., 11.5]) # Adding noise err_inputH = np.zeros((4, npts)) err_inputH = fset.white_noise_var(npts, var_list) err_outputH1, Time, Xsim = cnt.lsim(H_sample1, err_inputH[0, :], Time) err_outputH2, Time, Xsim = cnt.lsim(H_sample2, err_inputH[1, :], Time) err_outputH3, Time, Xsim = cnt.lsim(H_sample3, err_inputH[2, :], Time) # OUTPUTS Yout = np.zeros((3, npts))