loc = numpy.where(particles.ss == k)[0]
s = 0
for l in loc:
s += particles.ws[l]
switchtrack[k, 0] = s
maxtrack[:, 0] = RBPF.get_max_track(particles, numModels)
# Controller Input
ind = numpy.argmax(maxtrack[:, 0]) # use this model and controller
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)
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)
# First time step of the simulation
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
horizon = 150
params.us[0] = MPC.mpc_lqr(params.kfmeans[:, 0], horizon, A, numpy.matrix(B),
params.QQ, params.RR, numpy.array([0, 0]),
numpy.array([0.0])) # get the controller input
for t in range(1, params.N):
params.xs[:, t] = A @ (params.xs[:, t - 1] - b) + B * params.us[
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:
def fun():
isDone = True
init_state = numpy.array([0.55, 450]) # initial state
# Setup Switching Particle Filter
A = numpy.array([[0.999, 0.001], [0.001, 0.999]])
def fun1(x, u, w):
return params.cstr_model.run_reactor(x, u, params.h) + w
def fun2(x, u, w):
return params.cstr_model_broken.run_reactor(x, u, params.h) + w
def gs(x):
return params.C2 @ x
F = [fun1, fun2]
G = [gs, gs]
numSwitches = 2
ydists = numpy.array([
scipy.stats.multivariate_normal(cov=params.R2),
scipy.stats.multivariate_normal(cov=params.R2)
])
xdists = numpy.array([
scipy.stats.multivariate_normal(cov=params.Q),
scipy.stats.multivariate_normal(cov=params.Q)
])
cstr_filter = SPF.Model(F, G, A, xdists, ydists)
nP = 500 # number of particles
xdist = scipy.stats.multivariate_normal(mean=init_state,
cov=params.init_state_covar)
sdist = [0.9, 0.1]
particles = SPF.init_spf(xdist, sdist, nP, 2)
switchtrack = numpy.zeros([2, params.N])
maxtrack = numpy.zeros([numSwitches, params.N])
smoothedtrack = numpy.zeros([numSwitches, params.N])
state_noise_dist = scipy.stats.multivariate_normal(cov=params.Q)
meas_noise_dist = scipy.stats.multivariate_normal(cov=params.R2)
# Setup control (use linear control)
linsystems = params.cstr_model.get_nominal_linear_systems(params.h)
linsystems_broken = params.cstr_model_broken.get_nominal_linear_systems(
params.h)
opoint = 1 # the specific linear model we will use
lin_models = [None] * 2 # type: typing.List[RBPF.Model]
lin_models[0] = RBPF.Model(linsystems[opoint].A, linsystems[opoint].B,
linsystems[opoint].b, params.C2, params.Q,
params.R2)
lin_models[1] = RBPF.Model(linsystems_broken[opoint].A,
linsystems_broken[opoint].B,
linsystems_broken[opoint].b, params.C2,
params.Q, params.R2)
H = numpy.matrix([1, 0]) # only attempt to control the concentration
setpoint = 0.49
controllers = [None] * 2 # type: typing.List[LQR.Controller]
for k in range(2):
ysp = setpoint - lin_models[k].b[0] # set point is set here
x_off, u_off = LQR.offset(lin_models[k].A,
numpy.matrix(lin_models[k].B), params.C2, H,
numpy.matrix([ysp]))
K = LQR.lqr(lin_models[k].A, numpy.matrix(lin_models[k].B), params.QQ,
params.RR)
controllers[k] = LQR.Controller(K, x_off, u_off)
# Setup simulation
params.xs[:, 0] = init_state
params.ys2[:, 0] = params.C2 @ params.xs[:, 0] + meas_noise_dist.rvs(
) # measured from actual plant
SPF.init_filter(particles, params.ys2[:, 0], cstr_filter)
for k in range(numSwitches):
switchtrack[k, 0] = numpy.sum(
particles.w[numpy.where(particles.s == k)[0]])
maxtrack[:, 0] = SPF.get_max_track(particles, numSwitches)
smoothedtrack[:, 0] = RBPF.smoothed_track(numSwitches, switchtrack, 1, 10)
params.spfmeans[:, 0], params.spfcovars[:, :, 0] = SPF.get_stats(particles)
# Controller Input
ind = numpy.argmax(maxtrack[:, 0]) # use this model and controller
horizon = 150
params.us[0] = MPC.mpc_lqr(params.spfmeans[:, 0] - 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)
# Loop through the rest of time
for t in range(1, params.N):
random_element = state_noise_dist.rvs()
if params.ts[t] < 100: # break here
params.xs[:, t] = params.cstr_model.run_reactor(
params.xs[:, t - 1], params.us[t - 1],
params.h) + random_element
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.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]):
isDone = False
break
return isDone, setpoint
SPF.init_filter(particles, params.ys2[:, 0], cstr_filter)
for k in range(numSwitches):
switchtrack[k, 0] = numpy.sum(particles.w[numpy.where(particles.s == k)[0]])
maxtrack[:, 0] = SPF.get_max_track(particles, numSwitches)
smoothedtrack[:, 0] = RBPF.smoothed_track(numSwitches, switchtrack, 1, 10)
params.spfmeans[:, 0], params.spfcovars[:, :, 0] = SPF.get_stats(particles)
# Controller Input
ind = numpy.argmax(maxtrack[:, 0]) # use this model and controller
horizon = 150
params.us[0] = MPC.mpc_lqr(params.spfmeans[:, 0] - 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)
# Loop through the rest of time
for t in range(1, params.N):
random_element = state_noise_dist.rvs()
if params.ts[t] < 100: # break here
params.xs[:, t] = params.cstr_model.run_reactor(params.xs[:, t-1], params.us[t-1], params.h) + random_element
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.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)