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)
# 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.PRBS_seq(npts, 0.03, [-0.33, 0.1]) Usim[1, :] = fset.PRBS_seq(npts, 0.03) Usim[2, :] = fset.PRBS_seq(npts, 0.03, [2.3, 5.7]) Usim[3, :] = fset.PRBS_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)) Yout11, Time, Xsim = cnt.lsim(g_sample11, Usim[0, :], Time) Yout12, Time, Xsim = cnt.lsim(g_sample12, Usim[1, :], Time) Yout13, Time, Xsim = cnt.lsim(g_sample13, Usim[2, :], Time) Yout14, Time, Xsim = cnt.lsim(g_sample14, Usim[3, :], Time) Yout21, Time, Xsim = cnt.lsim(g_sample21, Usim[0, :], Time) Yout22, Time, Xsim = cnt.lsim(g_sample22, Usim[1, :], Time) Yout23, Time, Xsim = cnt.lsim(g_sample23, Usim[2, :], Time)
## 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.,
0., 0.
]
# ### Numerator of input transfer function has 3 roots: nb = 3
dt = ts / Mx # integration step
# Output & Input
X0k = X[:, j]
Uk = U[:, j]
# Integrate the model
for i in range(Mx):
k1 = Fdyn(X0k, Uk)
k2 = Fdyn(X0k + dt / 2.0 * k1, Uk)
k3 = Fdyn(X0k + dt / 2.0 * k2, Uk)
k4 = Fdyn(X0k + dt * k3, Uk)
Xk_1 = X0k + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4)
X[:, j + 1] = Xk_1
# Add noise (with assigned variances)
var = [0.001, 0.001]
noise = fset.white_noise_var(npts, var)
# Build Output
Y = X + noise
#### IDENTIFICATION STAGE
# ARX - mimo
na_ords = [5, 5]
nb_ords = [[3, 1, 3, 1], [3, 3, 1, 3]]
theta = [[0, 0, 0, 0], [0, 0, 0, 0]]
# call id
Id_ARX = system_identification(Y,
U,
'ARX',
centering='MeanVal',
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")
plt.grid()
plt.xlabel("Time")
#
plt.figure(1)
plt.plot(Time, y_tot[0])
plt.ylabel("y_tot")
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)