params.xsnofix[:, t - 1], params.us[t - 1], params.h)
params.xsnofix[:, t] += random_element # actual plant
else:
params.xs[:, t] = params.cstr_model_broken.run_reactor(
params.xs[:, t - 1], params.us[t - 1], params.h)
params.xs[:, t] += random_element
params.xsnofix[:, t] = params.cstr_model.run_reactor(
params.xsnofix[:, t - 1], params.us[t - 1], params.h)
params.xsnofix[:, t] += random_element # actual plant
params.ys2[:, t] = params.C2 @ params.xs[:, t] + meas_noise_dist.rvs(
) # measured from actual plant
SPF.spf_filter(particles, params.us[t - 1], params.ys2[:, t], cstr_filter)
params.spfmeans[:, t], params.spfcovars[:, :, t] = SPF.get_stats(particles)
for k in range(2):
ind = numpy.where(particles.s == k)[0]
if ind.shape[0] != 0:
switchtrack[k, t] = numpy.sum(particles.w[ind])
maxtrack[:, t] = SPF.get_max_track(particles, numSwitches)
smoothedtrack[:, t] = RBPF.smoothed_track(numSwitches, switchtrack, t, 10)
# Plot results
Results.plot_switch_selection(numSwitches, switchtrack, params.ts, True)
Results.plot_switch_selection(numSwitches, maxtrack, params.ts, False)
Results.plot_tracking_break(params.ts, params.xs, params.xsnofix, params.ys2,
params.spfmeans, 2)
Results.calc_error(params.xs, params.spfmeans)
plt.show()
for t in range(1, params.N):
params.xs[:, t] = params.cstr_model.run_reactor(params.xs[:, t-1], params.us[t-1], params.h) # actual plant
params.xs[:, t] += state_noise_dist.rvs()
params.ys1[t] = params.C1 @ params.xs[:, t] + meas_noise_dist.rvs() # measured from actual plant
particles = RBPF.rbpf_filter(particles, params.us[t-1], params.ys1[t], models, A)
params.rbpfmeans[:, t], params.rbpfcovars[:, :, t] = RBPF.get_ave_stats(particles)
# rbpfmeans[:,t], rbpfcovars[:,:, t] = RBPF.getMLStats(particles)
for k in range(len(linsystems)):
loc = numpy.where(particles.ss == k)[0]
s = 0
for l in loc:
s += particles.ws[l]
switchtrack[k, t] = s
maxtrack[:, t] = RBPF.get_max_track(particles, numModels)
smoothedtrack[:, t] = RBPF.smoothed_track(numModels, switchtrack, t, 10)
# Plot results
Results.plot_state_space_switch(linsystems, params.xs)
plt.savefig("/home/ex/Documents/CSC/report/results/Figure_9-3_python.pdf", bbox_inches="tight")
Results.plot_switch_selection(numModels, switchtrack, params.ts, True)
plt.savefig("/home/ex/Documents/CSC/report/results/Figure_9-5_python.pdf", bbox_inches="tight")
# Results.plot_switch_selection(numModels, maxtrack, ts, false)
# Results.plot_switch_selection(numModels, smoothedtrack, ts, false)
Results.plot_tracking(params.ts, params.xs, params.ys1, params.rbpfmeans, params.us, 1)
Results.calc_error(params.xs, params.rbpfmeans)
plt.show()
print("DONE")
for k in range(len(linsystems)):
loc = numpy.where(particles.ss == k)[0]
s = 0
for l in loc:
s += particles.ws[l]
switchtrack[k, t] = s
maxtrack[:, t] = RBPF.get_max_track(particles, numModels)
# Controller Input
if t % 1 == 0:
ind = numpy.argmax(maxtrack[:, t]) # use this model and controller
params.us[t] = MPC.mpc_lqr(params.rbpfmeans[:, t] - models[ind].b,
horizon, models[ind].A,
numpy.matrix(models[ind].B),
numpy.matrix(params.QQ),
numpy.matrix(params.RR),
controllers[ind].x_off,
numpy.array([controllers[ind].u_off[0]]))
else:
params.us[t] = params.us[t - 1]
# Plot results
Results.plot_state_space_switch(linsystems, params.xs)
Results.plot_switch_selection(numModels, maxtrack, params.ts, False)
Results.plot_switch_selection(numModels, switchtrack, params.ts, True)
Results.plot_tracking1(params.ts, params.xs, params.ys2, params.rbpfmeans,
params.us, 2, setpoint)
plt.show()
isDone = False
break
return isDone, setpoint
mcN = 50
mcdists = numpy.zeros([2, mcN])
xconcen = numpy.zeros([params.N, mcN])
mcerr = numpy.zeros(mcN)
mciter = -1
while mciter < mcN - 1:
try:
res, setpoint = fun()
if res:
mciter += 1
mcerr[mciter] = Results.calc_error1(params.xs, setpoint)
Results.get_mc_res(params.xs, params.spfcovars, [10, -410],
mcdists, mciter, params.h)
xconcen[:, mciter] = params.xs[0, :]
except:
continue
mcave = sum(abs(mcerr)) / mcN
print("Monte Carlo average concentration error: ", mcave)
nocount = len(mcdists[0]) - numpy.count_nonzero(mcdists[0])
filteredResults = numpy.zeros([2, mcN - nocount])
counter = 0
for k in range(mcN):
if mcdists[0, k] != 0.0:
filteredResults[:, counter] = mcdists[:, k]
plt.ylabel(r"C$_A$ (III)")
plt.locator_params(nbins=4)
plt.subplot(4, 1, 4) # 99.9%
for k in range(cols):
plt.plot(ts, mc4[:, k], "k-", linewidth=0.5)
plt.plot(ts, numpy.ones(rows)*0.49, "g-", linewidth=3.0)
plt.ylabel(r"C$_A$ (IV)")
plt.locator_params(nbins=4)
plt.xlabel("Time [min]")
A = 412
mcerr1 = 0
for k in range(cols):
mcerr1 += abs(Results.calc_error3(mc1[-100:-1, k], A))
print("The average MC error is:", mcerr1/cols)
mcerr2 = 0
for k in range(cols):
mcerr2 += abs(Results.calc_error3(mc2[-100:-1, k], A))
print("The average MC error is:", mcerr2/cols)
mcerr3 = 0
for k in range(cols):
mcerr3 += abs(Results.calc_error3(mc3[-100:-1, k], A))
print("The average MC error is:", mcerr3/cols)
1000.0, params.Q, 4.6052, True) # get the controller input
else:
params.us[t] = params.us[t - 1]
if params.us[t] is None or numpy.isnan(params.us[t]):
isDone = False
break
return isDone, setpoint
while mciter < mcN - 1:
try:
res, setpoint = fun()
if res:
mciter += 1
mcerr[mciter] = Results.calc_error1(params.xs, setpoint[0])
Results.get_mc_res(params.xs, params.spfcovars, [aline, cline],
mcdists, mciter, params.h)
xconcen[:, mciter] = params.xs[0, :]
except:
continue
mcave = sum(abs(mcerr)) / mcN
print("Monte Carlo average concentration error: ", mcave)
nocount = len(mcdists[0]) - numpy.count_nonzero(mcdists[0])
filteredResults = numpy.zeros([2, mcN - nocount])
counter = 0
for k in range(mcN):
if mcdists[0, k] != 0.0:
filteredResults[:, counter] = mcdists[:, k]
counter += 1
# Controller Input
if t % 1 == 0:
# ind = indmax(smoothedtrack[:, t]) # use this model and controller
ind = numpy.argmax(maxtrack[:, t]) # use this model and controller
yspfix = setpoint - lin_models[ind].b
params.us[t] = MPC.mpc_mean(params.spfmeans[:, t] - lin_models[ind].b,
horizon, lin_models[ind].A,
numpy.matrix(lin_models[ind].B),
lin_models[ind].b, aline, bline, cline,
params.QQ, params.RR, yspfix, usps[ind],
15000.0,
1000.0) # get the controller input
else:
params.us[t] = params.us[t - 1]
if params.us[t] is None or numpy.isnan(params.us[t]):
break
# Plot results
Results.plot_switch_selection(numSwitches, maxtrack, params.ts, False)
# Results.plotSwitchSelection(numSwitches, switchtrack, ts, true)
Results.plot_tracking1(params.ts, params.xs, params.ys2, params.spfmeans,
params.us, 2, setpoint[0])
Results.plot_ellipses2(params.ts, params.xs, params.spfmeans, params.spfcovars,
[aline, cline], [setpoint[0], 422.6], True, 4.6052, 1,
"best")
Results.calc_error1(params.xs, setpoint[0])
Results.calc_energy(params.us, params.h)
# Results.checkConstraint(ts, xs, [aline, cline])
plt.show()
def main(mcN=1, linear=True, pf=False):
tend = 80
params = closedloop_scenarios_single.closedloop_params.Params(
tend) # end time of simulation
# Get the linear model
linsystems = params.cstr_model.get_nominal_linear_systems(
params.h) # cstr_model comes from params.jl
opoint = 1 # the specific operating point we are going to use for control
init_state = numpy.array([0.55, 450
]) # random initial point near operating point
# Set the state space model
A = linsystems[opoint].A
B = linsystems[opoint].B
b = linsystems[opoint].b # offset from the origin
# Set point
ysp = linsystems[opoint].op[0] - b[0] # Medium concentration
H = numpy.matrix([1, 0]) # only attempt to control the concentration
x_off, usp = LQR.offset(A, numpy.matrix(B), params.C2, H,
numpy.matrix([ysp])) # control offset
ysp = x_off
usp = numpy.array([usp])
# Noise distributions
state_noise_dist = scipy.stats.multivariate_normal(cov=params.Q)
meas_noise_dist = scipy.stats.multivariate_normal(cov=params.R2)
# PF functions
def f(x, u, w):
return params.cstr_model.run_reactor(x, u, params.h) + w
def g(x):
return params.C2 @ x # state observation
cstr_pf = PF.Model(f, g)
# Set up the KF
kf_cstr = LLDS.LLDS(
A, B, params.C2, params.Q,
params.R2) # set up the KF object (measuring both states)
# Setup MPC
horizon = 150
# add state constraints
aline = 10 # slope of constraint line ax + by + c = 0
if linear:
cline = -411 # negative of the y axis intercept
else:
cline = -404 # negative of the y axis intercept
bline = 1
if linear:
lim_u = 10000
else:
lim_u = 20000
mcdists = numpy.zeros([2, mcN])
xconcen = numpy.zeros([params.N, mcN])
mcerrs = numpy.zeros(mcN)
particles = None
for mciter in range(mcN):
# First time step of the simulation
if linear:
params.xs[:,
0] = init_state - b # set simulation starting point to the random initial state
params.ys2[:,
0] = params.C2 @ params.xs[:, 0] + meas_noise_dist.rvs(
) # measure from actual plant
temp = kf_cstr.init_filter(init_state - b, params.init_state_covar,
params.ys2[:, 0]) # filter
params.kfmeans[:, 0], params.kfcovars[:, :, 0] = temp
else:
params.xs[:,
0] = init_state # set simulation starting point to the random initial state
params.ys2[:,
0] = params.C2 @ params.xs[:, 0] + meas_noise_dist.rvs(
) # measure from actual plant
temp = kf_cstr.init_filter(init_state - b, params.init_state_covar,
params.ys2[:, 0] - b) # filter
params.kfmeans[:, 0], params.kfcovars[:, :, 0] = temp
if pf:
nP = 200
prior_dist = scipy.stats.multivariate_normal(
mean=init_state,
cov=params.init_state_covar) # prior distribution
particles = PF.init_pf(prior_dist, nP,
2) # initialise the particles
particles = PF.init_filter(particles, params.ys2[:, 0],
meas_noise_dist, cstr_pf)
params.pfmeans[:, 0], params.pfcovars[:, :,
0] = PF.get_stats(particles)
params.us[0] = MPC.mpc_mean(params.pfmeans[:, 0] - b, horizon, A,
numpy.matrix(B), b, aline, bline,
cline, params.QQ, params.RR, ysp,
usp[0], lim_u,
1000.0) # get the controller input
else:
params.us[0] = MPC.mpc_mean(params.kfmeans[:, 0], horizon, A,
numpy.matrix(B), b, aline, bline,
cline, params.QQ, params.RR, ysp,
usp[0], lim_u,
1000.0) # get the controller input
for t in range(1, params.N):
if linear:
params.xs[:, t] = A @ params.xs[:, t - 1] + B * params.us[
t - 1] + state_noise_dist.rvs()
params.ys2[:,
t] = params.C2 @ params.xs[:, t] + meas_noise_dist.rvs(
) # measure from actual plant
params.kfmeans[:,
t], params.kfcovars[:, :,
t] = kf_cstr.step_filter(
params.kfmeans[:,
t - 1],
params.kfcovars[:, :,
t - 1],
params.us[t - 1],
params.ys2[:, t])
else:
params.xs[:, t] = params.cstr_model.run_reactor(
params.xs[:, t - 1], params.us[t - 1], params.h)
params.xs[:, t] += state_noise_dist.rvs() # actual plant
params.ys2[:,
t] = params.C2 @ params.xs[:, t] + meas_noise_dist.rvs(
) # measure from actual plant
params.kfmeans[:,
t], params.kfcovars[:, :,
t] = kf_cstr.step_filter(
params.kfmeans[:,
t - 1],
params.kfcovars[:, :,
t - 1],
params.us[t - 1],
params.ys2[:, t] - b)
if pf:
PF.pf_filter(particles, params.us[t - 1], params.ys2[:, t],
state_noise_dist, meas_noise_dist, cstr_pf)
params.pfmeans[:,
t], params.pfcovars[:, :,
t] = PF.get_stats(particles)
if t % 10 == 0:
if pf:
params.us[t] = MPC.mpc_mean(params.pfmeans[:, t] - b,
horizon, A, numpy.matrix(B), b,
aline, bline, cline, params.QQ,
params.RR, ysp, usp[0], lim_u,
1000.0)
else:
params.us[t] = MPC.mpc_mean(params.kfmeans[:,
t], horizon, A,
numpy.matrix(B), b, aline,
bline, cline, params.QQ,
params.RR, ysp, usp[0], lim_u,
1000.0)
if params.us[t] is None or numpy.isnan(params.us[t]):
break
else:
params.us[t] = params.us[t - 1]
for i in range(len(params.kfmeans[0])):
params.kfmeans[:, i] += b
if linear:
params.xs[:, i] += b
params.ys2[:, i] += b
if mcN > 1:
xconcen[:, mciter] = params.xs[0, :]
mcerrs[mciter] = Results.calc_error1(params.xs, ysp[0] + b[0])
Results.get_mc_res(params.xs, params.kfcovars, [aline, cline],
mcdists, mciter, params.h)
if mcN == 1:
# Plot the results
if pf:
Results.plot_tracking1(params.ts, params.xs, params.ys2,
params.pfmeans, params.us, 2,
ysp[0] + b[0])
Results.plot_ellipses2(params.ts, params.xs, params.pfmeans,
params.pfcovars, [aline, cline],
linsystems[1].op, True,
-2.0 * numpy.log(1 - 0.9), 1, "best")
else:
Results.plot_tracking1(params.ts, params.xs, params.ys2,
params.kfmeans, params.us, 2,
ysp[0] + b[0])
Results.plot_ellipses2(params.ts, params.xs, params.kfmeans,
params.kfcovars, [aline, cline],
linsystems[1].op, True,
-2.0 * numpy.log(1 - 0.9), 1, "best")
Results.check_constraint(params.ts, params.xs, [aline, cline])
Results.calc_error1(params.xs, ysp[0] + b[0])
Results.calc_energy(params.us, 0.0)
plt.show()
if mcN != 1:
print("The absolute MC average error is: ", sum(abs(mcerrs)) / mcN)
if linear:
if pf:
numpy.savetxt("linmod_pf_mean_mc2.csv", xconcen, delimiter=",")
else:
numpy.savetxt("linmod_kf_mean_mc2.csv", xconcen, delimiter=",")
else:
if pf:
numpy.savetxt("nonlinmod_pf_mean_mc2.csv",
xconcen,
delimiter=",")
else:
numpy.savetxt("nonlinmod_kf_mean_mc2.csv",
xconcen,
delimiter=",")
nocount = len(mcdists[0]) - numpy.count_nonzero(mcdists[0])
filteredResults = numpy.zeros([2, mcN - nocount])
counter = 0
for k in range(mcN):
if mcdists[0, k] != 0.0:
filteredResults[:, counter] = mcdists[:, k]
counter += 1
if linear:
if pf:
numpy.savetxt("linmod_pf_mean_mc.csv",
filteredResults,
delimiter=",",
fmt="%f")
else:
numpy.savetxt("linmod_kf_mean_mc.csv",
filteredResults,
delimiter=",",
fmt="%f")
else:
if pf:
numpy.savetxt("nonlinmod_pf_mean_mc.csv",
filteredResults,
delimiter=",",
fmt="%f")
else:
numpy.savetxt("nonlinmod_kf_mean_mc.csv",
filteredResults,
delimiter=",",
fmt="%f")
t - 1] + b + state_noise_dist.rvs() # actual plant
params.ys2[:, t] = params.C2 @ params.xs[:, t] + meas_noise_dist.rvs(
) # measure from actual plant
temp = kf_cstr.step_filter(params.kfmeans[:, t - 1],
params.kfcovars[:, :, t - 1], params.us[t - 1],
params.ys2[:, t] - b)
params.kfmeans[:, t], params.kfcovars[:, :, t] = temp
# Compute controller action
if t % 10 == 0:
params.us[t] = MPC.mpc_lqr(params.kfmeans[:, t], horizon, A,
numpy.matrix(B), params.QQ, params.RR,
numpy.array([0, 0]),
numpy.array([0.0
])) # get the controller input
if params.us[t] is None or numpy.isnan(params.us[t]):
break
else:
params.us[t] = params.us[t - 1]
for i in range(len(params.kfmeans[0])):
params.kfmeans[:, i] += b
# Plot the results
Results.plot_tracking1(params.ts, params.xs, params.ys2, params.kfmeans,
params.us, 2, ysp[0] + b[0])
Results.calc_error1(params.xs, ysp[0] + b[0])
Results.calc_energy(params.us, 0.0)
plt.show()
break
for i in foils:
optics_segments_list.append(i)
# #} end of Gather the data from the worker processes
elapsed = time.time() - t
print("elapsed: {}".format(elapsed))
# try to open the file
print("Saving results")
# the results are saved to a pickle file, which alows to plot them again without raytracing
# file_name="raytracing_{}_{}.pkl".format("deployed" if optics_deployed else "retracted", scaling_factor)
file_name = "result_new.pkl"
results = Results(optics_segments_list, timepix_segments_list,
optics_point_list, timepix_point_list, source_point_list,
reflected_rays_segment_list, direct_rays_segment_list,
columns)
with open(file_name, 'wb') as direct_rays_queue:
try:
pickle.dump(results, direct_rays_queue, pickle.HIGHEST_PROTOCOL)
except:
print("file \"{}\" could not be saved".format(file_name))
# plot the results
import plot
for t in range(1, params.N):
params.xs[:, t] = params.cstr_model.run_reactor(params.xs[:, t - 1],
params.us[t - 1], params.h)
params.xs[:, t] += state_noise_dist.rvs() # actual plant
params.ys2[:, t] = params.C2 @ params.xs[:, t] + meas_noise_dist.rvs(
) # measured from actual plant
PF.pf_filter(particles, params.us[t - 1], params.ys2[:, t],
state_noise_dist, meas_noise_dist, cstr_pf)
pfmeans[:, t], pfcovars[:, :, t] = PF.get_stats(particles)
temp = lin_cstr.step_filter(kfmeans[:, t - 1], kfcovars[:, :, t - 1],
params.us[t - 1], params.ys2[:, t] - b)
kfmeans[:, t], kfcovars[:, :, t] = temp
for i in range(len(kfmeans[0])):
kfmeans[:, i] += b
# Plot Results
Results.plot_ellipse_comp(pfmeans, pfcovars, kfmeans, kfcovars, params.xs,
params.ts)
plt.savefig("/home/ex/Documents/CSC/report/results/Figure_7-7_python.pdf",
bbox_inches="tight")
Results.plot_tracking_two_filters(params.ts, params.xs, params.ys2, pfmeans,
kfmeans)
print("For the Kalman Filter:")
Results.calc_error(params.xs, kfmeans)
print("For the Particle Filter:")
Results.calc_error(params.xs, pfmeans)
plt.show()
# ind = indmax(smoothedtrack[:, t]) # use this model and controller
ind = numpy.argmax(maxtrack[:, t]) # use this model and controller
yspfix = setpoint - lin_models[ind].b
params.us[t] = MPC.mpc_var(
params.spfmeans[:, t] - lin_models[ind].b,
params.spfcovars[:, :, t], horizon, lin_models[ind].A,
numpy.matrix(lin_models[ind].B), lin_models[ind].b, aline, bline,
cline, params.QQ, params.RR, yspfix, usps[ind], 15000.0, 1000.0,
params.Q, 9.21, True) # get the controller input
else:
params.us[t] = params.us[t - 1]
if params.us[t] is None or numpy.isnan(params.us[t]):
break
# Plot results
Results.plot_switch_selection(numSwitches, maxtrack, params.ts, False)
plt.savefig("/home/ex/Documents/CSC/report/results/Figure_12-11_python.pdf",
bbox_inches="tight")
Results.plot_switch_selection(numSwitches, switchtrack, params.ts, True)
Results.plot_tracking1(params.ts, params.xs, params.ys2, params.spfmeans,
params.us, 2, setpoint[0])
plt.savefig("/home/ex/Documents/CSC/report/results/Figure_12-10_python.pdf",
bbox_inches="tight")
Results.plot_ellipses2(params.ts, params.xs, params.spfmeans, params.spfcovars,
[aline, cline], [linsystems[1].op[1], 422.6], True,
9.21, 1, "best")
plt.savefig("/home/ex/Documents/CSC/report/results/Figure_12-12_python.pdf",
bbox_inches="tight")
Results.calc_error1(params.xs, setpoint[0])
Results.calc_energy(params.us, params.h)
Results.check_constraint(params.ts, params.xs, [aline, cline])
params.ys2[:, t] = params.C2 @ params.xs[:, t] + meas_noise_dist.rvs() # measured from actual plant
SPF.spf_filter(particles, params.us[t-1], params.ys2[:, t], cstr_filter)
params.spfmeans[:, t], params.spfcovars[:, :, t] = SPF.get_stats(particles)
for k in range(numSwitches):
switchtrack[k, 0] = numpy.sum(particles.w[numpy.where(particles.s == k)[0]])
maxtrack[:, t] = SPF.get_max_track(particles, numSwitches)
smoothedtrack[:, t] = RBPF.smoothed_track(numSwitches, switchtrack, t, 40)
# Controller Input
if t % 1 == 0:
ind = numpy.argmax(maxtrack[:, t]) # use this model and controller
params.us[t] = MPC.mpc_lqr(params.spfmeans[:, t] - lin_models[ind].b, horizon, lin_models[ind].A,
numpy.matrix(lin_models[ind].B), params.QQ, params.RR,
controllers[ind].x_off, controllers[ind].u_off)
else:
params.us[t] = params.us[t-1]
if params.us[t] is None or numpy.isnan(params.us[t]):
break
# Plot results
Results.plot_switch_selection(numSwitches, maxtrack, params.ts, False)
Results.plot_switch_selection(numSwitches, switchtrack, params.ts, True)
Results.plot_tracking1(params.ts, params.xs, params.ys2, params.spfmeans, params.us, 2, setpoint)
Results.calc_error1(params.xs, setpoint)
Results.calc_energy(params.us, params.h)
plt.show()
params.init_state_covar,
params.ys1[0] - b[1])
for t in range(1, params.N):
params.xs[:, t] = params.cstr_model.run_reactor(params.xs[:, t - 1],
params.us[t - 1], params.h)
params.xs[:, t] += state_noise_dist.rvs()
params.ys1[t] = params.C1 @ params.xs[:, t] + meas_noise_dist.rvs(
) # measured from actual plant
params.linxs[:, t], _ = lin_cstr.step(params.linxs[:, t - 1],
params.us[t - 1])
temp = lin_cstr.step_filter(kfmeans[:, t - 1], kfcovars[:, :, t - 1],
params.us[t - 1], params.ys1[t] - b[1])
kfmeans[:, t], kfcovars[:, :, t] = temp
for i in range(len(params.linxs[0])):
params.linxs[:, i] += b
kfmeans[:, i] += b
# Plot results
Results.plot_ellipses1(params.ts, params.xs, kfmeans, kfcovars, "upper right")
plt.savefig("/home/ex/Documents/CSC/report/results/Figure_6-5_python.pdf",
bbox_inches="tight")
Results.plot_tracking(params.ts, params.xs, params.ys1, kfmeans, params.us, 1)
plt.savefig("/home/ex/Documents/CSC/report/results/Figure_6-4_python.pdf",
bbox_inches="tight")
plt.show()
avediff = Results.calc_error(params.xs, kfmeans)
# First time step
kfmeans[:, 0], kfcovars[:, :,
0] = lin_cstr.init_filter(init_mean,
params.init_state_covar,
params.ys2[:, 0] - b)
for t in range(1, params.N):
params.xs[:, t] = params.cstr_model.run_reactor(params.xs[:, t - 1],
params.us[t - 1], params.h)
params.xs[:, t] += state_noise_dist.rvs()
params.ys2[:, t] = params.C2 @ params.xs[:, t] + meas_noise_dist.rvs(
) # measured from actual plant
params.linxs[:, t], _ = lin_cstr.step(params.linxs[:, t - 1],
params.us[t - 1])
temp = lin_cstr.step_filter(kfmeans[:, t - 1], kfcovars[:, :, t - 1],
params.us[t - 1], params.ys2[:, t] - b)
kfmeans[:, t], kfcovars[:, :, t] = temp
for i in range(len(params.linxs[0])):
params.linxs[:, i] += b
kfmeans[:, i] += b
# Plot results
Results.plot_ellipses1(params.ts, params.xs, kfmeans, kfcovars, "lower left")
Results.plot_tracking(params.ts, params.xs, params.ys2, kfmeans, params.us, 2)
plt.show()
avediff = Results.calc_error(params.xs, kfmeans)
xtemp = params.xs[:, t-1] - b
d = params.cstr_model.run_reactor(params.xs[:, t-1], params.us[t-1], params.h)
d -= (A @ xtemp + B * params.us[t-1] + b)
else:
params.xs[:, t] = params.cstr_model_broken.run_reactor(params.xs[:, t-1], params.us[t-1], params.h)
params.xs[:, t] += state_noise_dist.rvs() # actual plant
xtemp = params.xs[:, t-1] - b
d = params.cstr_model_broken.run_reactor(params.xs[:, t-1], params.us[t-1], params.h)
d -= (A @ xtemp + B * params.us[t-1] + b)
params.ys2[:, t] = params.C2 @ params.xs[:, t] + meas_noise_dist.rvs() # measure from actual plant
PF.pf_filter(particles, params.us[t-1], params.ys2[:, t], state_noise_dist, meas_noise_dist, cstr_pf)
params.pfmeans[:, t], params.pfcovars[:, :, t] = PF.get_stats(particles)
if t % 1 == 0:
params.us[t] = MPC.mpc_mean(params.pfmeans[:, t]-b, horizon, A, numpy.matrix(B), b, aline, bline, cline,
params.QQ, params.RR, ysp, usp[0], 15000.0, 1000.0, d)
else:
params.us[t] = params.us[t-1]
if params.us[t] is None or numpy.isnan(params.us[t]):
break
# # Plot the results
Results.plot_tracking1(params.ts, params.xs, params.ys2, params.pfmeans, params.us, 2, ysp[0]+b[0])
Results.plot_ellipses2(params.ts, params.xs, params.pfmeans, params.pfcovars, [aline, cline],
[linsystems[2].op[1], 422.6], True, 4.6052, 1, "upper right")
Results.check_constraint(params.ts, params.xs, [aline, cline])
Results.calc_error1(params.xs, ysp[0]+b[0])
Results.calc_energy(params.us, params.h)
plt.show()
# Initialise the PF
nP = 200 # number of particles.
prior_dist = scipy.stats.multivariate_normal(mean=init_state, cov=params.init_state_covar) # prior distribution
particles = PF.init_pf(prior_dist, nP, 2) # initialise the particles
state_noise_dist = scipy.stats.multivariate_normal(cov=params.Q) # state distribution
meas_noise_dist = scipy.stats.multivariate_normal(cov=params.R2) # measurement distribution
# Time step 1
params.xs[:, 0] = init_state
params.ys2[:, 0] = params.C2 @ params.xs[:, 0] + meas_noise_dist.rvs() # measured from actual plant
particles = PF.init_filter(particles, params.ys2[:, 0], meas_noise_dist, cstr_pf)
params.pfmeans[:, 0], params.pfcovars[:, :, 0] = PF.get_stats(particles)
# Loop through the rest of time
for t in range(1, params.N):
params.xs[:, t] = params.cstr_model.run_reactor(params.xs[:, t - 1], params.us[t - 1], params.h)
params.xs[:, t] += state_noise_dist.rvs() # actual plant
params.ys2[:, t] = params.C2 @ params.xs[:, t] + meas_noise_dist.rvs() # measured from actual plant
particles = PF.pf_filter(particles, params.us[t-1], params.ys2[:, t], state_noise_dist, meas_noise_dist, cstr_pf)
params.pfmeans[:, t], params.pfcovars[:, :, t] = PF.get_stats(particles)
# Plot results
Results.plot_ellipses1(params.ts, params.xs, params.pfmeans, params.pfcovars, "upper right")
Results.plot_tracking(params.ts, params.xs, params.ys2, params.pfmeans, params.us, 2)
Results.calc_error(params.xs, params.pfmeans)
plt.show()
def main(nine, mcN=1, linear=True, pf=False, numerical=False):
if nine == 90:
k_squared = 4.6052
plot_setting = 0
elif nine == 99:
k_squared = 9.21
plot_setting = 1
elif nine == 999:
k_squared = 13.8155
plot_setting = 2
else:
return None
tend = 80
params = closedloop_scenarios_single.closedloop_params.Params(
tend) # end time of simulation
# Get the linear model
linsystems = params.cstr_model.get_nominal_linear_systems(
params.h) # cstr_model comes from params.jl
opoint = 1 # the specific operating point we are going to use for control
init_state = numpy.array([0.55, 450
]) # random initial point near operating point
# Set the state space model
A = linsystems[opoint].A
B = linsystems[opoint].B
b = linsystems[opoint].b # offset from the origin
# Set point
ysp = linsystems[opoint].op[0] - b[0] # Medium concentration
H = numpy.matrix([1, 0]) # only attempt to control the concentration
x_off, usp = LQR.offset(A, numpy.matrix(B), params.C2, H,
numpy.matrix([ysp])) # control offset
ysp = x_off
usp = numpy.array([usp])
# PF functions
def f(x, u, w):
return params.cstr_model.run_reactor(x, u, params.h) + w
def g(x):
return params.C2 @ x # state observation
cstr_pf = PF.Model(f, g)
nP = 200
if numerical:
nP = 500
# Set up for numerical
Ndiv = len(range(0, tend, 3))
kldiv = numpy.zeros(
Ndiv) # Kullback-Leibler Divergence as a function of time
basediv = numpy.zeros(Ndiv) # Baseline
unidiv = numpy.zeros(Ndiv) # Uniform comparison
klts = numpy.zeros(Ndiv)
ndivcounter = 0
temp_states = numpy.zeros([2, nP])
# Set up the KF
kf_cstr = LLDS.LLDS(
A, B, params.C2, params.Q,
params.R2) # set up the KF object (measuring both states)
state_noise_dist = scipy.stats.multivariate_normal(cov=params.Q)
meas_noise_dist = scipy.stats.multivariate_normal(cov=params.R2)
# Setup MPC
horizon = 150
# add state constraints
aline = 10 # slope of constraint line ax + by + c = 0
cline = -411 # negative of the y axis intercept
bline = 1
e = cline
growvar = True
if linear:
limu = 10000
else:
limu = 20000
mcdists = numpy.zeros([2, mcN])
xconcen = numpy.zeros([params.N, mcN])
mcerrs = numpy.zeros(mcN)
mciter = -1
particles = None
while mciter < mcN - 1:
# First time step of the simulation
if linear:
params.xs[:,
0] = init_state - b # set simulation starting point to the random initial state
params.ys2[:,
0] = params.C2 @ params.xs[:, 0] + meas_noise_dist.rvs(
) # measure from actual plant
temp = kf_cstr.init_filter(init_state - b, params.init_state_covar,
params.ys2[:, 0]) # filter
params.kfmeans[:, 0], params.kfcovars[:, :, 0] = temp
else:
params.xs[:,
0] = init_state # set simulation starting point to the random initial state
params.ys2[:,
0] = params.C2 @ params.xs[:, 0] + meas_noise_dist.rvs(
) # measure from actual plant
temp = kf_cstr.init_filter(init_state - b, params.init_state_covar,
params.ys2[:, 0] - b) # filter
params.kfmeans[:, 0], params.kfcovars[:, :, 0] = temp
if pf:
if linear:
prior_dist = scipy.stats.multivariate_normal(
mean=init_state - b,
cov=params.init_state_covar) # prior distribution
else:
prior_dist = scipy.stats.multivariate_normal(
mean=init_state,
cov=params.init_state_covar) # prior distribution
particles = PF.init_pf(prior_dist, nP,
2) # initialise the particles
particles = PF.init_filter(particles, params.ys2[:, 0],
meas_noise_dist, cstr_pf)
params.pfmeans[:, 0], params.pfcovars[:, :,
0] = PF.get_stats(particles)
if linear:
params.us[0] = MPC.mpc_var(params.pfmeans[:, 0],
params.kfcovars[:, :,
0], horizon, A,
numpy.matrix(B), b, aline, bline, e,
params.QQ, params.RR, ysp, usp[0],
limu, 1000.0, params.Q, k_squared,
growvar) # get controller input
else:
params.us[0] = MPC.mpc_var(params.pfmeans[:, 0] - b,
params.kfcovars[:, :,
0], horizon, A,
numpy.matrix(B), b, aline, bline, e,
params.QQ, params.RR, ysp, usp[0],
limu, 1000.0, params.Q, k_squared,
growvar) # get controller input
else:
params.us[0] = MPC.mpc_var(params.kfmeans[:, 0],
params.kfcovars[:, :, 0], horizon, A,
numpy.matrix(B), b, aline, bline, e,
params.QQ, params.RR, ysp, usp[0], limu,
1000.0, params.Q, k_squared,
growvar) # get the controller input
if numerical:
kldiv[ndivcounter] = Auxiliary.kl(particles.x, particles.w,
params.pfmeans[:, 0],
params.pfcovars[:, :,
0], temp_states)
basediv[ndivcounter] = Auxiliary.klbase(params.pfmeans[:, 0],
params.pfcovars[:, :, 0],
temp_states, nP)
unidiv[ndivcounter] = Auxiliary.kluniform(params.pfmeans[:, 0],
params.pfcovars[:, :, 0],
temp_states, nP)
klts[ndivcounter] = 0.0
ndivcounter += 1
status = True
for t in range(1, params.N):
if linear:
params.xs[:, t] = A @ params.xs[:, t - 1] + B * params.us[
t - 1] + state_noise_dist.rvs() # actual plant
params.ys2[:,
t] = params.C2 @ params.xs[:, t] + meas_noise_dist.rvs(
) # measure from actual plant
params.kfmeans[:,
t], params.kfcovars[:, :,
t] = kf_cstr.step_filter(
params.kfmeans[:,
t - 1],
params.kfcovars[:, :,
t - 1],
params.us[t - 1],
params.ys2[:, t])
else:
params.xs[:, t] = params.cstr_model.run_reactor(
params.xs[:, t - 1],
params.us[t - 1],
params.h,
)
params.xs[:, t] += state_noise_dist.rvs() # actual plant
params.ys2[:,
t] = params.C2 @ params.xs[:, t] + meas_noise_dist.rvs(
) # measure from actual plant
params.kfmeans[:,
t], params.kfcovars[:, :,
t] = kf_cstr.step_filter(
params.kfmeans[:,
t - 1],
params.kfcovars[:, :,
t - 1],
params.us[t - 1],
params.ys2[:, t] - b)
if pf:
PF.pf_filter(particles, params.us[t - 1], params.ys2[:, t],
state_noise_dist, meas_noise_dist, cstr_pf)
params.pfmeans[:,
t], params.pfcovars[:, :,
t] = PF.get_stats(particles)
if t % 10 == 0:
if pf:
if linear:
params.us[t] = MPC.mpc_var(
params.pfmeans[:,
t], params.pfcovars[:, :,
t], horizon, A,
numpy.matrix(B), b, aline, bline, e, params.QQ,
params.RR, ysp, usp[0], limu, 1000, params.Q,
k_squared, growvar) # controller input
else:
params.us[t] = MPC.mpc_var(
params.pfmeans[:, t] - b,
params.pfcovars[:, :, t], horizon, A,
numpy.matrix(B), b, aline, bline, e, params.QQ,
params.RR, ysp, usp[0], limu, 1000, params.Q,
k_squared, growvar) # controller input
else:
params.us[t] = MPC.mpc_var(params.kfmeans[:, t],
params.kfcovars[:, :,
t], horizon, A,
numpy.matrix(B), b, aline,
bline, e, params.QQ, params.RR,
ysp, usp[0], limu, 1000,
params.Q, k_squared,
growvar) # get controller input
if params.us[t] is None or numpy.isnan(params.us[t]):
status = False
break
else:
params.us[t] = params.us[t - 1]
if numerical and params.ts[t] in range(0, tend, 3):
kldiv[ndivcounter] = Auxiliary.kl(particles.x, particles.w,
params.pfmeans[:, t],
params.pfcovars[:, :, t],
temp_states)
basediv[ndivcounter] = Auxiliary.klbase(
params.pfmeans[:, t], params.pfcovars[:, :, t],
temp_states, nP)
unidiv[ndivcounter] = Auxiliary.kluniform(
params.pfmeans[:, t], params.pfcovars[:, :, t],
temp_states, nP)
klts[ndivcounter] = params.ts[t]
ndivcounter += 1
if not status:
continue
else:
mciter += 1
for i in range(len(params.kfmeans[0])):
params.kfmeans[:, i] += b
if linear:
params.xs[:, i] += b
params.ys2[:, i] += b
if mcN > 1:
xconcen[:, mciter] = params.xs[0, :]
mcerrs[mciter] = Results.calc_error1(params.xs, ysp[0] + b[0])
mcdists = Results.get_mc_res(params.xs, params.kfcovars,
[aline, cline], mcdists, mciter,
params.h)
if mcN == 1:
# Plot the results
if numerical:
Results.plot_kl_div(klts, kldiv, basediv, unidiv, False)
Results.plot_kl_div(klts, kldiv, basediv, unidiv, True)
print("The average divergence for the baseline is: ",
1.0 / len(klts) * sum(basediv))
print("The average divergence for the approximation is: ",
1.0 / len(klts) * sum(kldiv))
print("The average divergence for the uniform is: ",
1.0 / len(klts) * sum(unidiv))
elif pf:
Results.plot_tracking1(params.ts, params.xs, params.ys2,
params.pfmeans, params.us, 2,
ysp[0] + b[0])
plt.savefig(
"/home/ex/Documents/CSC/report/results/Figure_8-25_python.pdf",
bbox_inches="tight")
Results.plot_ellipses2(params.ts, params.xs, params.pfmeans,
params.pfcovars, [aline, cline],
linsystems[1].op, True,
-2.0 * numpy.log(1 - 0.9), plot_setting,
"best")
plt.savefig(
"/home/ex/Documents/CSC/report/results/Figure_8-26_python.pdf",
bbox_inches="tight")
Results.check_constraint(params.ts, params.xs, [aline, cline])
Results.calc_error1(params.xs, ysp[0] + b[0])
Results.calc_energy(params.us, 0.0)
else:
Results.plot_tracking1(params.ts, params.xs, params.ys2,
params.kfmeans, params.us, 2,
ysp[0] + b[0])
plt.savefig(
"/home/ex/Documents/CSC/report/results/Figure_8-19_python.pdf",
bbox_inches="tight")
Results.plot_ellipses2(params.ts, params.xs, params.kfmeans,
params.kfcovars, [aline, cline],
linsystems[opoint].op, True, k_squared,
plot_setting, "best")
plt.savefig(
"/home/ex/Documents/CSC/report/results/Figure_8-20_python.pdf",
bbox_inches="tight")
Results.check_constraint(params.ts, params.xs, [aline, cline])
Results.calc_error1(params.xs, ysp[0] + b[0])
Results.calc_energy(params.us, 0.0)
plt.show()
if mcN != 1:
print("The absolute MC average error is: ", sum(abs(mcerrs)) / mcN)
if linear:
if pf:
numpy.savetxt("linmod_pf_var{}_mc2.csv".format(nine),
xconcen,
delimiter=",",
fmt="%f")
else:
numpy.savetxt("linmod_kf_var{}_mc2.csv".format(nine),
xconcen,
delimiter=",",
fmt="%f")
else:
if pf:
numpy.savetxt("nonlinmod_pf_var{}_mc2.csv".format(nine),
xconcen,
delimiter=",",
fmt="%f")
else:
numpy.savetxt("nonlinmod_kf_var{}_mc2.csv".format(nine),
xconcen,
delimiter=",",
fmt="%f")
nocount = len(mcdists[0]) - numpy.count_nonzero(mcdists[0])
filteredResults = numpy.zeros([2, mcN - nocount])
counter = 0
for k in range(mcN):
if mcdists[0, k] != 0.0:
filteredResults[:, counter] = mcdists[:, k]
counter += 1
if linear:
if pf:
numpy.savetxt("linmod_pf_var{}_mc.csv".format(nine),
filteredResults,
delimiter=",",
fmt="%f")
else:
numpy.savetxt("linmod_kf_var{}_mc.csv".format(nine),
filteredResults,
delimiter=",",
fmt="%f")
else:
if pf:
numpy.savetxt("nonlinmod_pf_var{}_mc.csv".format(nine),
filteredResults,
delimiter=",",
fmt="%f")
else:
numpy.savetxt("nonlinmod_kf_var{}_mc.csv".format(nine),
filteredResults,
delimiter=",",
fmt="%f")