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] = []