Esempio n. 1
0
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.,
Esempio n. 3
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
Esempio n. 4
0
# 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")
Esempio n. 5
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_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)
Esempio n. 6
0
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)
Esempio n. 7
0
    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
Esempio n. 8
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, 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))
Esempio n. 9
0
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)
Esempio n. 10
0
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)
Esempio n. 11
0
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.
]
Esempio n. 12
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))