def forward(self, x_init, ft, C, c, current = True, n_iters=20):
T, n_batch, n_dist = ft.shape
if current == True:
F_hat = self.F_hat
Bd_hat = self.Bd_hat
else:
F_hat = self.F_hat_old
Bd_hat = self.Bd_hat_old
x_lqr, u_lqr, objs_lqr = mpc.MPC(n_state=self.n_state,
n_ctrl=self.n_ctrl,
T=self.T,
u_lower= self.u_lower.repeat(self.T, n_batch, 1),
u_upper= self.u_upper.repeat(self.T, n_batch, 1),
lqr_iter=n_iters,
backprop = True,
verbose=0,
exit_unconverged=False,
)(x_init.double(), QuadCost(C.double(), c.double()),
LinDx(F_hat.repeat(self.T-1, n_batch, 1, 1), ft.double()))
return x_lqr, u_lqr
def forward(self, x_init, C, c, cur_time):
dt = np.array(self.disturbance[cur_time:cur_time + pd.Timedelta(
seconds=(T - 2) * step)]) # T-1 x n_dist
if len(dt) < T - 1:
dt = np.pad(dt, ((0, T - 1 - len(dt)), (0, 0)), 'edge')
dt = torch.tensor(dt).transpose(0, 1) # n_dist x T-1
ft = torch.mm(self.Bd_hat, dt).transpose(0, 1) # T-1 x n_state
ft = ft.unsqueeze(1) # T-1 x 1 x n_state
x_pred, u_pred, _ = mpc.MPC(
n_state=self.n_state,
n_ctrl=self.n_ctrl,
T=T,
u_lower=self.u_lower,
u_upper=self.u_upper,
lqr_iter=20,
verbose=0,
exit_unconverged=False,
)(x_init, QuadCost(C.double(), c.double()),
LinDx(self.F_hat.repeat(T - 1, 1, 1, 1), ft))
return x_pred[1, 0, :], u_pred[0, 0, :] # Dim.