q_cov = {}
for j in range(1, ntrays + 1):
q_cov[("x", (j, ))] = 1e-05
q_cov[("M", (j, ))] = 1
c.make_noisy(q_cov)
for i in range(1, 1000):
c.solve_d(c.d1, stop_if_nopt=True, o_tee=True)
# Dot_sens
with open("debug1.txt", "w") as f:
c.d1.w_pnoisy.display(ostream=f)
c.randomize_noize(q_cov)
c.update_state_real() # update the current state
c.update_soi_sp_nmpc()
c.predictor_step(c.d1, "real")
c.update_state_predicted()
c.compute_offset_state("real")
c.initialize_olnmpc(c.d2, "predicted")
c.load_init_state_nmpc(src_kind="predicted")
c.solve_d(c.olnmpc, stop_if_nopt=True, skip_update=False, iter_max=1000)
c.update_u(c.olnmpc)
c.print_r_nmpc()
c.cycle_ics(plant_step=True)
# c.cycle_ics_noisy()
c.plant_input_gen(c.d1, src_kind="dict")
# c.init_step_mhe(dum, c.nfe_t) # Initialize next time-slot
# c.update_state_mhe()
# Prior-Covariance stuff
# c.check_active_bound_noisy()
# c.load_covariance_prior()
# c.set_state_covariance()
# c.regen_objective_fun()
# Update prior-state
# c.set_prior_state_from_prior_mhe()
#
# c.print_r_mhe()
# Compute the controls
c.initialize_olnmpc(c.d1, "real")
c.load_init_state_nmpc(src_kind="state_dict", state_dict="real") # for good measure
# if i == 5:
# with open("somefilc.txt", "w") as f:
# c.olnmpc.R_nmpc.display(ostream=f)
# c.olnmpc.Q_nmpc.display(ostream=f)
# f.close()
stat_nmpc = c.solve_d(c.olnmpc, skip_update=False)
if stat_nmpc != 0:
c.olnmpc.write_nl(name='nmpc_failed.nl')
stat_nmpc = c.solve_d(c.olnmpc, stop_if_nopt=True, skip_update=False, iter_max=300)
c.update_u(c.olnmpc)
c.print_r_nmpc()
# c.shift_mhe()
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()
# sys.exit()
c.compute_QR_nmpc(n=-1)
c.initialize_olnmpc(c.d1)
c.olnmpc.display(filename="somefile0.txt")
c.create_suffixes()
# c.solve_k_aug_nmpc()
# c.olnmpc.write_nl()
i = 1
k = 1
while k != 0:
if i == 1:
k = c.solve_d(c.olnmpc, max_cpu_time=60 * i)
else:
k = c.solve_d(c.olnmpc, max_cpu_time=60 * i, warm_start=True)
c.olnmpc.ipopt_zL_in.update(c.olnmpc.ipopt_zL_out)
c.olnmpc.ipopt_zU_in.update(c.olnmpc.ipopt_zU_out)
i += 1