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)
ARMAX_orders=[na_ords, nb_ords, nc_ords, theta],
ARMAX_max_iterations=n_iter)
# SS - mimo
# choose method
method = 'PARSIM-K'
SS_ord = 2
Id_SS = system_identification(Y, U, method, SS_fixed_order=SS_ord)
# GETTING RESULTS (Y_id)
# ARX
Y_arx = Id_ARX.Yid
# ARMAX
Y_armax = Id_ARMAX.Yid
# SS
x_ss, Y_ss = fsetSIM.SS_lsim_process_form(Id_SS.A, Id_SS.B, Id_SS.C, Id_SS.D,
U, Id_SS.x0)
##### PLOTS
# Input
plt.close('all')
plt.figure(1)
str_input = [
'F [m$^3$/min]', 'W [kg/min]', 'Ca$_{in}$ [kg/m$^3$]', 'T$_{in}$ [$^o$C]'
]
for i in range(m):
plt.subplot(m, 1, i + 1)
plt.plot(Time, U[i, :])
plt.ylabel("Input " + str(i + 1))
plt.ylabel(str_input[i])
def do_sys_id(self):
num_poles = 2
num_zeros = 1
if not self._use_subspace:
method = 'ARMAX'
#sys_id = system_identification(self._y.T, self._u[0], method, IC='BIC', na_ord=[0, 5], nb_ord=[1, 5], nc_ord=[0, 5], delays=[0, 5], ARMAX_max_iterations=300, tsample=self._Ts, centering='MeanVal')
sys_id = system_identification(self._y.T,
self._u[0],
method,
ARMAX_orders=[num_poles, 1, 1, 0],
ARMAX_max_iterations=300,
tsample=self._Ts,
centering='MeanVal')
print(sys_id.G)
if self._verbose:
print(sys_id.G)
print("System poles of discrete G: ", cnt.pole(sys_id.G))
# Convert to continuous tf
G = harold.Transfer(sys_id.NUMERATOR,
sys_id.DENOMINATOR,
dt=self._Ts)
G_cont = harold.undiscretize(G, method='zoh')
self._sys_tf = G_cont
self._A, self._B, self._C, self._D = harold.transfer_to_state(
G_cont, output='matrices')
if self._verbose:
print("Continuous tf:", G_cont)
# Convert to state space, because ARMAX gives transfer function
ss_roll = cnt.tf2ss(sys_id.G)
A = np.asarray(ss_roll.A)
B = np.asarray(ss_roll.B)
C = np.asarray(ss_roll.C)
D = np.asarray(ss_roll.D)
if self._verbose:
print(ss_roll)
# simulate identified system using input from data
xid, yid = fsetSIM.SS_lsim_process_form(A, B, C, D, self._u)
y_error = self._y - yid
self._fitness = 1 - (y_error.var() / self._y.var())**2
if self._verbose:
print("Fittness %", self._fitness * 100)
if self._plot:
plt.figure(1)
plt.plot(self._t[0], self._y[0])
plt.plot(self._t[0], yid[0])
plt.xlabel("Time")
plt.title("Time response Y(t)=U*G(t)")
plt.legend([
self._y_name, self._y_name + '_identified: ' +
'{:.3f} fitness'.format(self._fitness)
])
plt.grid()
plt.show()
else:
sys_id = system_identification(self._y,
self._u,
self._subspace_method,
SS_fixed_order=num_poles,
SS_p=self._subspace_p,
SS_f=50,
tsample=self._Ts,
SS_A_stability=True,
centering='MeanVal')
#sys_id = system_identification(self._y, self._u, self._subspace_method, SS_orders=[1,10], SS_p=self._subspace_p, SS_f=50, tsample=self._Ts, SS_A_stability=True, centering='MeanVal')
if self._verbose:
print("x0", sys_id.x0)
print("A", sys_id.A)
print("B", sys_id.B)
print("C", sys_id.C)
print("D", sys_id.D)
A = sys_id.A
B = sys_id.B
C = sys_id.C
D = sys_id.D
# Get discrete transfer function from state space
sys_tf = cnt.ss2tf(A, B, C, D)
if self._verbose:
print("TF ***in z domain***", sys_tf)
# Get numerator and denominator
(num, den) = cnt.tfdata(sys_tf)
# Convert to continuous tf
G = harold.Transfer(num, den, dt=self._Ts)
if self._verbose:
print(G)
G_cont = harold.undiscretize(G, method='zoh')
self._sys_tf = G_cont
self._A, self._B, self._C, self._D = harold.transfer_to_state(
G_cont, output='matrices')
if self._verbose:
print("Continuous tf:", G_cont)
# get zeros
tmp_tf = cnt.ss2tf(self._A, self._B, self._C, self._D)
self._zeros = cnt.zero(tmp_tf)
# simulate identified system using discrete system
xid, yid = fsetSIM.SS_lsim_process_form(A, B, C, D, self._u,
sys_id.x0)
y_error = self._y - yid
self._fitness = 1 - (y_error.var() / self._y.var())**2
if self._verbose:
print("Fittness %", self._fitness * 100)
if self._plot:
plt.figure(1)
plt.plot(self._t[0], self._y[0])
plt.plot(self._t[0], yid[0])
plt.xlabel("Time")
plt.title("Time response Y(t)=U*G(t)")
plt.legend([
self._y_name, self._y_name + '_identified: ' +
'{:.3f} fitness'.format(self._fitness)
])
plt.grid()
plt.show()
# 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")
plt.grid()
plt.xlabel("Time")
#
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)
