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)
def sys_id(actions, measurements, dt, method='PARSIM-K', order=None):
actions = np.stack(actions)[:, :, 0]
measurements = np.stack(measurements)[:, :, 0]
assert np.shape(measurements)[1] == np.shape(actions)[1]
system = system_identification(
measurements.T,
actions.T,
method, # SS_f=5,
SS_fixed_order=order, # SS_threshold=0.01,
tsample=1)
return system
def chemReactorGP(path,
first_outputs=3,
randSeed=0,
resampling=5,
ma_smoother=14,
data_extension_percentage=50,
minSS_orders=5,
maxSS_orders=8,
useLinear=True,
samples_num=1000,
particles_num=30,
bases_num=4,
ratio_L=1,
Kn=10,
burnPercentage=25,
lQ=100,
ell=1.,
Vgain=1e3):
np.random.seed(randSeed)
data = sio.loadmat('ForIdentification.mat')
if resampling > 1:
u = data['u'][::resampling, :]
y = data['y'][::resampling, :first_outputs]
yVal = data['yVal'][::resampling, :first_outputs]
else:
u = data['u']
y = data['y'][:, :first_outputs]
yVal = data['yVal'][:, :first_outputs]
u = u.T #u=u[np.newaxis,:]
y = y.T #y=y[np.newaxis,:]
yVal = yVal.T
# #%% Scaling
# y = y-y[:,-1][:,np.newaxis]
# yVal = yVal-yVal[:,-1][:,np.newaxis]
# y = y/(np.max(y,axis=1)-np.min(y,axis=1)) #scaled to 0-1
# yVal = y/(np.max(yVal,axis=1)-np.min(yVal,axis=1)) #scaled to 0-1
# u = u - np.mean(u,axis=1)[:,np.newaxis]
#%%
#Extend u and y
extension = data_extension_percentage # 50% extension
if extension != 0:
y_extend = np.zeros(
(y.shape[0], (y.shape[1] * (100 + extension) // 100)))
yVal_extend = np.zeros(
(y.shape[0], (yVal.shape[1] * (100 + extension) // 100)))
u_extend = np.zeros(
(u.shape[0], (u.shape[1] * (100 + extension) // 100)))
shift = extension * y.shape[1] // 200
y_extend[:, shift:shift + y.shape[1]] = y
yVal_extend[:, shift:shift + yVal.shape[1]] = yVal
u_extend[:, shift:shift + u.shape[1]] = u
else:
y_extend = y
yVal_extend = yVal
u_extend = u
# y_ma = util.moving_average(y_extend.T,ma_smoother).T
y_extend = util.moving_average(y_extend.T, ma_smoother).T
yVal_extend = util.moving_average(yVal_extend.T, ma_smoother).T
#%% Scaling
y_extend = scale_To_zero_one(y_extend)
yVal_extend = scale_To_zero_one(yVal_extend)
u_extend = u_extend - np.mean(u, axis=1)[:, np.newaxis]
# plt.plot(y.T)
# plt.plot(y_ma.T)
# %%
# T_test = T
u_test = u_extend
y_test = yVal_extend
u_train = u_extend
y_train = y_extend
t = np.arange(u_extend.shape[1])
#%%
sys_id = sippy.system_identification(
y_train,
u_train,
'N4SID'
# ,centering='InitVal'
# ,SS_p=horizon,SS_f=horizon
,
SS_A_stability=True,
IC='AIC',
SS_orders=[minSS_orders, maxSS_orders])
## Linear system identification validation for comparison
xid, yid = sippy.functionsetSIM.SS_lsim_predictor_form(sys_id.A_K,\
sys_id.B_K,\
sys_id.C,\
sys_id.D,\
sys_id.K,\
y_test,\
u_test,sys_id.x0)
#%%
nx = sys_id.A.shape[0]
iA = sys_id.A #np.random.randn(nx,nx)
iB = sys_id.B #np.ones((nx,1))
#%%
# nbases=4
L = y.shape[1] / ratio_L
steps = samples_num
sim = a.Simulate(steps,
nx,
u_train,
y_train,
bases_num,
L,
PFweightNum=particles_num)
if useLinear:
sim.iA = iA
sim.iB = iB
if sim.nx > sim.ny:
sim.iC = np.hstack((np.zeros(
(sim.ny, sim.nx - sim.ny)), np.eye(sim.ny)))
else:
sim.iC = np.eye(sim.ny)
#experimental
# sim.iC = sys_id.C
sim.burnInPercentage = burnPercentage
sim.lQ = lQ #for prior QR
sim.ell = ell
sim.Vgain = Vgain
sim.R = 1e-3
sim.run()
#%%
sim.evaluate(y_test, u_test, Kn=Kn)
y_test_mea = np.mean(sim.y_test_sim, axis=0)
y_test_med = np.median(sim.y_test_sim, axis=0)
y_test_loQ = np.quantile(sim.y_test_sim, 0.025, axis=0)
y_test_hiQ = np.quantile(sim.y_test_sim, 0.975, axis=0)
sim.save(str(simResultPath / 'result.hdf5'))
#%%
for i in range(sim.ny):
fig = plt.figure(figsize=(20, 10))
plt.plot(t, yid[i, :], color='r', linewidth=1, label='Linear System')
plt.plot(t, y_test[i, :], color='k', linewidth=1, label='Ground Truth')
# plt.plot(t,y_test_sim[:,i,:].T,color='b',linewidth=0.2,alpha=0.01)
plt.plot(t, y_test_mea[i, :], color='b', linewidth=0.5)
plt.fill_between(t,
y_test_loQ[i, :],
y_test_hiQ[i, :],
color='b',
alpha=.1,
label=r'95 \% confidence')
plt.legend()
filename = str(simResultPath / 'delta_T_{}.png'.format(i))
plt.savefig(filename)
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)
from scipy.sparse.linalg import lsqr
dat = pd.read_csv('data_set2_013121_1100.txt')
Time = dat['Time (sec)'].values
Q1 = dat[' Heater 1 (%)'].values
Q2 = dat[' Heater 2 (%)'].values
T1 = dat[' Temperature 1 (degC)'].values - dat[' Temperature 1 (degC)'].values[
0]
T2 = dat[' Temperature 2 (degC)'].values - dat[' Temperature 2 (degC)'].values[
0]
U = np.vstack([[Q1], [Q2]])
Y = np.vstack([[T1], [T2]])
##System identification
method = 'N4SID'
sys_id = sp.system_identification(Y, U, method)
xid, yid = SS_lsim(sys_id.A, sys_id.B, sys_id.C, sys_id.D, U, sys_id.x0)
#%%
plt.close("all")
plt.figure()
plt.subplot(2, 1, 1)
plt.plot(Time, Y[0])
plt.plot(Time, yid[0])
plt.ylabel("[C]")
plt.grid()
plt.title("Temperature 1")
plt.legend(['Exper. Data', 'Identified system, ' + method])
plt.subplot(2, 1, 2)