horizon = 150
params.us[0] = MPC.mpc_lqr(params.rbpfmeans[:, 0] - 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]]))
# 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
RBPF.rbpf_filter(particles, params.us[t - 1], params.ys2[:, t], models, A)
params.rbpfmeans[:,
t], params.rbpfcovars[:, :,
t] = RBPF.get_ave_stats(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)
# Controller Input
if t % 1 == 0:
for l in loc:
s += particles.ws[l]
switchtrack[k, 0] = s
maxtrack[:, 0] = RBPF.get_max_track(particles, numModels)
smoothedtrack[:, 0] = RBPF.smoothed_track(numModels, switchtrack, 1, 10)
# 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) # 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)