from nmpc_mhe.dync.NMPCGen import NmpcGen
from nmpc_mhe.mods.bfb.bfb_abs7momdt_ht2 import bfb_dae
from snap_shot import snap
import sys
from numpy.random import normal as npm
from pyutilib.common._exceptions import ApplicationError
states = [
"Ngb", "Hgb", "Ngc", "Hgc", "Nsc", "Hsc", "Nge", "Hge", "Nse", "Hse", "mom"
]
u = ["u1"]
u_bounds = {"u1": (162.183495794 * 0.0005, 162.183495794 * 10000)}
ref_state = {("c_capture", ((), )): 0.63}
c = NmpcGen(d_mod=bfb_dae,
u=u,
states=states,
ref_state=ref_state,
u_bounds=u_bounds,
nfe_t=5,
_t=100)
c.ss.dref = snap
c.load_iguess_ss()
# sys.exit()
# c.ss.lydot.display()
# sys.exit()
c.solve_ss()
c.solve_d(c.ss)
c.ss.write_nl()
# sys.exit()
c.load_d_s(c.d1)
"T": [(i, ) for i in range(1, ntrays + 1)],
"Mv": [(i, ) for i in range(2, ntrays)],
"Mv1": [((), )],
"Mvn": [((), )]
}
x_vars = {
"x": [(i, ) for i in range(1, ntrays + 1)],
"M": [(i, ) for i in range(1, ntrays + 1)]
}
nfet = 10
tfe = [i for i in range(1, nfet + 1)]
c = NmpcGen(d_mod=DistDiehlNegrete,
u=u,
states=states,
ref_state=ref_state,
u_bounds=u_bounds)
c.load_iguess_ss()
c.solve_ss()
c.load_d_s(c.d1)
c.solve_d(c.d1)
c.update_state_real() # update the current state
c.find_target_ss()
c.create_nmpc()
c.update_targets_nmpc()
c.compute_QR_nmpc(n=-1)
c.new_weights_olnmpc(10000, 1e+06)
from __future__ import print_function
from __future__ import division
from pyomo.environ import *
from nmpc_mhe.dync.NMPCGen import NmpcGen
from nmpc_mhe.mods.bfb.bfb_abs7momdt_nmpc import bfb_dae
from snap_shot import snap
from pyomo.core.base import value
from numpy.random import normal as npm
from pyutilib.common._exceptions import ApplicationError
import sys
states = ["Ngb", "Hgb", "Ngc", "Hgc", "Nsc", "Hsc", "Nge", "Hge", "Nse", "Hse", "mom"]
u = ["u1"]
u_bounds = {"u1":(162.183495794 * 0.5, 162.183495794 * 1.6)}
ref_state = {("c_capture", ((),)): 0.52}
c = NmpcGen(d_mod=bfb_dae, u=u, states=states, ref_state=ref_state, u_bounds=u_bounds)
c.ss.dref = snap
c.load_iguess_ss()
c.solve_ss()
c.load_d_s(c.d1)
c.solve_d(c.d1)
c.update_state_real() # update the current state
c.find_target_ss()
c.create_nmpc()
c.update_targets_nmpc()
c.compute_QR_nmpc(n=-1)
c.new_weights_olnmpc(1000, 1e+06)
from pyomo.environ import *
from nmpc_mhe.dync.NMPCGen import NmpcGen
from nmpc_mhe.mods.bfb.bfb_abs_v3 import *
import sys
states = ["Ngb", "Hgb", "Ngc", "Hgc", "Nsc", "Hsc", "Nge", "Hge", "Nse", "Hse"]
u = ["u1"]
u_bounds = {
"u1": (0.0001, 9286.03346463 * 100),
"u2": (0.0001, 9286.03346463 * 100)
}
ref_state = {("c_capture", ((), )): 0.50}
# weights = {("c_capture", ((),)): 1E+05}
c = NmpcGen(d_mod=bfb_dae,
u=u,
states=states,
ref_state=ref_state,
u_bounds=u_bounds)
c.load_iguess_ss()
c.ss.create_bounds()
c.solve_ss()
c.load_d_s(c.d1)
c.solve_d(c.d1, iter_max=100)
c.find_target_ss()
c.ss2.write_nl()
sys.exit(-1)
# c.plant_input_gen(c.ss2, 1)
c.create_nmpc()
c.update_targets_nmpc()
def __init__(self, **kwargs):
NmpcGen.__init__(self, **kwargs)
self.int_file_mhe_suf = int(time.time()) - 1
# Need a list of relevant measurements y
self.y = kwargs.pop('y', [])
self.y_vars = kwargs.pop('y_vars', {})
# Need a list or relevant noisy-states z
self.x_noisy = kwargs.pop('x_noisy', [])
self.x_vars = kwargs.pop('x_vars', {})
self.deact_ics = kwargs.pop('del_ics', True)
self.diag_Q_R = kwargs.pop(
'diag_QR',
True) #: By default use diagonal matrices for Q and R matrices
self.u = kwargs.pop('u', [])
self.IgnoreProcessNoise = kwargs.pop('IgnoreProcessNoise', False)
print("-" * 120)
print("I[[create_lsmhe]] lsmhe (full) model created.")
print("-" * 120)
nstates = sum(len(self.x_vars[x]) for x in self.x_noisy)
self.journalizer("I", self._c_it, "MHE with \t",
str(nstates) + "states")
self.journalizer("I", self._c_it, "MHE with \t",
str(nstates * self.nfe_t * self.ncp_t) + "noise vars")
self.lsmhe = self.d_mod(self.nfe_t, self.ncp_t, _t=self._t)
self.lsmhe.name = "LSMHE (Least-Squares MHE)"
self.lsmhe.create_bounds()
#: create x_pi constraint
#: Create list of noisy-states vars
self.xkN_l = []
self.xkN_nexcl = []
self.xkN_key = {}
k = 0
for x in self.x_noisy:
n_s = getattr(self.lsmhe, x) #: Noisy-state
for jth in self.x_vars[x]: #: the jth variable
self.xkN_l.append(n_s[(1, 0) + jth])
self.xkN_nexcl.append(
1) #: non-exclusion list for active bounds
self.xkN_key[(x, jth)] = k
k += 1
self.lsmhe.xkNk_mhe = Set(initialize=[
i for i in range(0, len(self.xkN_l))
]) #: Create set of noisy_states
self.lsmhe.x_0_mhe = Param(self.lsmhe.xkNk_mhe,
initialize=0.0,
mutable=True) #: Prior-state
self.lsmhe.wk_mhe = Param(self.lsmhe.fe_t, self.lsmhe.cp_ta, self.lsmhe.xkNk_mhe, initialize=0.0) \
if self.IgnoreProcessNoise else Var(self.lsmhe.fe_t, self.lsmhe.cp_ta, self.lsmhe.xkNk_mhe, initialize=0.0) #: Model disturbance
self.lsmhe.PikN_mhe = Param(self.lsmhe.xkNk_mhe,
self.lsmhe.xkNk_mhe,
initialize=lambda m, i, ii: 1.
if i == ii else 0.0,
mutable=True) #: Prior-Covariance
self.lsmhe.Q_mhe = Param(range(1, self.nfe_t), self.lsmhe.xkNk_mhe, initialize=1, mutable=True) if self.diag_Q_R\
else Param(range(1, self.nfe_t), self.lsmhe.xkNk_mhe, self.lsmhe.xkNk_mhe,
initialize=lambda m, t, i, ii: 1. if i == ii else 0.0, mutable=True) #: Disturbance-weight
#: Create list of measurements vars
self.yk_l = {}
self.yk_key = {}
k = 0
self.yk_l[1] = []
for y in self.y:
m_v = getattr(self.lsmhe, y) #: Measured "state"
for jth in self.y_vars[y]: #: the jth variable
self.yk_l[1].append(m_v[(1, self.ncp_t) + jth])
self.yk_key[(
y, jth
)] = k #: The key needs to be created only once, that is why the loop was split
k += 1
for t in range(2, self.nfe_t + 1):
self.yk_l[t] = []
for y in self.y:
m_v = getattr(self.lsmhe, y) #: Measured "state"
for jth in self.y_vars[y]: #: the jth variable
self.yk_l[t].append(m_v[(t, self.ncp_t) + jth])
self.lsmhe.ykk_mhe = Set(initialize=[
i for i in range(0, len(self.yk_l[1]))
]) #: Create set of measured_vars
self.lsmhe.nuk_mhe = Var(self.lsmhe.fe_t,
self.lsmhe.ykk_mhe,
initialize=0.0) #: Measurement noise
self.lsmhe.yk0_mhe = Param(self.lsmhe.fe_t,
self.lsmhe.ykk_mhe,
initialize=1.0,
mutable=True)
self.lsmhe.hyk_c_mhe = Constraint(
self.lsmhe.fe_t,
self.lsmhe.ykk_mhe,
rule=lambda mod, t, i: mod.yk0_mhe[t, i] - self.yk_l[t][
i] - mod.nuk_mhe[t, i] == 0.0)
self.lsmhe.hyk_c_mhe.deactivate()
self.lsmhe.R_mhe = Param(self.lsmhe.fe_t, self.lsmhe.ykk_mhe, initialize=1.0, mutable=True) if self.diag_Q_R else \
Param(self.lsmhe.fe_t, self.lsmhe.ykk_mhe, self.lsmhe.ykk_mhe,
initialize=lambda mod, t, i, ii: 1.0 if i == ii else 0.0, mutable=True)
f = open("file_cv.txt", "w")
f.close()
#: Constraints for the input noise
for u in self.u:
# cv = getattr(self.lsmhe, u) #: Get the param
# c_val = [value(cv[i]) for i in cv.keys()] #: Current value
# self.lsmhe.del_component(cv) #: Delete the param
# self.lsmhe.add_component(u + "_mhe", Var(self.lsmhe.fe_t, initialize=lambda m, i: c_val[i-1]))
self.lsmhe.add_component("w_" + u + "_mhe",
Var(self.lsmhe.fe_t,
initialize=0.0)) #: Noise for input
self.lsmhe.add_component("w_" + u + "c_mhe",
Constraint(self.lsmhe.fe_t))
self.lsmhe.equalize_u(direction="r_to_u")
# cc = getattr(self.lsmhe, u + "_c") #: Get the constraint for input
con_w = getattr(self.lsmhe,
"w_" + u + "c_mhe") #: Get the constraint-noisy
var_w = getattr(self.lsmhe,
"w_" + u + "_mhe") #: Get the constraint-noisy
ce = getattr(self.lsmhe, u + "_e") #: Get the expression
cp = getattr(self.lsmhe, u) #: Get the param
con_w.rule = lambda m, i: cp[i] == ce[i] + var_w[i]
con_w.reconstruct()
con_w.deactivate()
# con_w.rule = lambda m, i: cp[i] == cv[i] + var_w[i]
# con_w.reconstruct()
# with open("file_cv.txt", "a") as f:
# cc.pprint(ostream=f)
# con_w.pprint(ostream=f)
# f.close()
self.lsmhe.U_mhe = Param(range(1, self.nfe_t + 1),
self.u,
initialize=1,
mutable=True)
#: Deactivate icc constraints
if self.deact_ics:
pass
# for i in self.states:
# self.lsmhe.del_component(i + "_icc")
#: Maybe only for a subset of the states
else:
for i in self.states:
if i in self.x_noisy:
ic_con = getattr(self.lsmhe, i + "_icc")
for k in self.x_vars[i]:
ic_con[k].deactivate()
#: Put the noise in the continuation equations (finite-element)
j = 0
self.lsmhe.noisy_cont = ConstraintList()
for i in self.x_noisy:
# cp_con = getattr(self.lsmhe, "cp_" + i)
cp_exp = getattr(self.lsmhe, "noisy_" + i)
# self.lsmhe.del_component(cp_con)
for k in self.x_vars[i]: #: This should keep the same order
for t in range(1, self.nfe_t):
self.lsmhe.noisy_cont.add(cp_exp[t, k] == 0.0)
# self.lsmhe.noisy_cont.add(cp_exp[t, k] == 0.0)
j += 1
# cp_con.reconstruct()
j = 0
self.lsmhe.noisy_cont.deactivate()
self.lsmhe.wk_mhe.pprint()
for i in self.x_noisy:
de_exp = getattr(self.lsmhe, "de_" + i)
for k in self.x_vars[i]:
for tfe in range(1, self.nfe_t + 1):
for tcp in range(1, self.ncp_t + 1):
print(de_exp[(tfe, tcp) + k]._body)
de_exp[(tfe, tcp) + k]._body += self.lsmhe.wk_mhe[tfe,
tcp,
j]
j += 1
#: Expressions for the objective function (least-squares)
self.lsmhe.Q_e_mhe = 0.0 if self.IgnoreProcessNoise else Expression(
expr=0.5 * sum(
sum(
sum(self.lsmhe.Q_mhe[1, k] * self.lsmhe.wk_mhe[i, j, k]**2
for k in self.lsmhe.xkNk_mhe)
for j in range(1, self.ncp_t + 1))
for i in range(1, self.nfe_t + 1))
) if self.diag_Q_R else Expression(expr=sum(
sum(self.lsmhe.wk_mhe[i, j] *
sum(self.lsmhe.Q_mhe[i, j, k] * self.lsmhe.wk_mhe[i, 1, k]
for k in self.lsmhe.xkNk_mhe) for j in self.lsmhe.xkNk_mhe)
for i in range(1, self.nfe_t)))
self.lsmhe.R_e_mhe = Expression(expr=0.5 * sum(
sum(self.lsmhe.R_mhe[i, k] * self.lsmhe.nuk_mhe[i, k]**2
for k in self.lsmhe.ykk_mhe)
for i in self.lsmhe.fe_t)) if self.diag_Q_R else Expression(
expr=sum(
sum(self.lsmhe.nuk_mhe[i, j] * sum(
self.lsmhe.R_mhe[i, j, k] * self.lsmhe.nuk_mhe[i, k]
for k in self.lsmhe.ykk_mhe)
for j in self.lsmhe.ykk_mhe) for i in self.lsmhe.fe_t))
expr_u_obf = 0
for i in self.lsmhe.fe_t:
for u in self.u:
var_w = getattr(self.lsmhe,
"w_" + u + "_mhe") #: Get the constraint-noisy
expr_u_obf += self.lsmhe.U_mhe[i, u] * var_w[i]**2
self.lsmhe.U_e_mhe = Expression(expr=0.5 *
expr_u_obf) # how about this
# with open("file_cv.txt", "a") as f:
# self.lsmhe.U_e_mhe.pprint(ostream=f)
# f.close()
self.lsmhe.Arrival_e_mhe = Expression(expr=0.5 * sum(
(self.xkN_l[j] - self.lsmhe.x_0_mhe[j]) *
sum(self.lsmhe.PikN_mhe[j, k] *
(self.xkN_l[k] - self.lsmhe.x_0_mhe[k])
for k in self.lsmhe.xkNk_mhe) for j in self.lsmhe.xkNk_mhe))
self.lsmhe.Arrival_dummy_e_mhe = Expression(expr=100000.0 * sum(
(self.xkN_l[j] - self.lsmhe.x_0_mhe[j])**2
for j in self.lsmhe.xkNk_mhe))
self.lsmhe.obfun_dum_mhe_deb = Objective(sense=minimize,
expr=self.lsmhe.Q_e_mhe)
self.lsmhe.obfun_dum_mhe = Objective(sense=minimize,
expr=self.lsmhe.R_e_mhe +
self.lsmhe.Q_e_mhe +
self.lsmhe.U_e_mhe) # no arrival
self.lsmhe.obfun_dum_mhe.deactivate()
self.lsmhe.obfun_mhe_first = Objective(
sense=minimize,
expr=self.lsmhe.Arrival_dummy_e_mhe + self.lsmhe.Q_e_mhe)
self.lsmhe.obfun_mhe_first.deactivate()
self.lsmhe.obfun_mhe = Objective(
sense=minimize,
expr=self.lsmhe.Arrival_dummy_e_mhe + self.lsmhe.R_e_mhe +
self.lsmhe.Q_e_mhe + self.lsmhe.U_e_mhe)
self.lsmhe.obfun_mhe.deactivate()
# with open("file_cv.txt", "a") as f:
# self.lsmhe.obfun_mhe.pprint(ostream=f)
# f.close()
self._PI = {} #: Container of the KKT matrix
self.xreal_W = {}
self.curr_m_noise = {} #: Current measurement noise
self.curr_y_offset = {} #: Current offset of measurement
for y in self.y:
for j in self.y_vars[y]:
self.curr_m_noise[(y, j)] = 0.0
self.curr_y_offset[(y, j)] = 0.0
self.s_estimate = {}
self.s_real = {}
for x in self.x_noisy:
self.s_estimate[x] = []
self.s_real[x] = []
self.y_estimate = {}
self.y_real = {}
self.y_noise_jrnl = {}
self.yk0_jrnl = {}
for y in self.y:
self.y_estimate[y] = []
self.y_real[y] = []
self.y_noise_jrnl[y] = []
self.yk0_jrnl[y] = []