def zeqns(self, t, zz, Al, Bl, a, b, Psol, Rsol, Rn, Qn):
if t > self.time[-1] or t < self.time[0]:
return 0
vmateq = self.veqns(zz, Al(t),
Bl(t).T, a(t), b(t), Psol(t), Rsol(t), Rn, Qn)
matdiffeq = matmult(Al(t), zz) + matmult(Bl(t), vmateq)
return matdiffeq.flatten()
def reqns(self, t, rr, Al, Bl, a, b, Psol, Rn, Qn):
if t > self.time[-1] or t < self.time[0]:
return np.array([0.0])
t = self.time[-1] - t
matdiffeq = (matmult((Al(t) - matmult(Bl(t), Bl(t), Psol(t))), rr) +
a(t) - matmult(Psol(t), Bl(t), b(t)))
return matdiffeq.flatten()
def peqns(self, t, pp, Al, Bl, Rn, Qn):
if t > self.time[-1] or t < self.time[0]:
return 0
pp = np.array(pp).flatten()
pp = pp.reshape(self.nx, self.nx)
matdiffeq = (matmult(pp, Al(t)) + matmult(Al(t), pp) -
matmult(pp, Bl(t), Bl(t), pp) + Qn)
return matdiffeq.flatten()
def dcost(self, descdir):
# evaluate directional derivative
dX = descdir[0]
dU = descdir[1]
time = self.time
dc = np.empty(time.shape[0])
for tindex, _ in np.ndenumerate(time):
dc[tindex] = matmult(self.a_current[tindex], dX[tindex]) + matmult(
self.b_current[tindex], dU[tindex])
intdcost = trapz(dc, time)
return intdcost
def proj(self, t, X, K, mu, alpha):
if type(X) is float:
X = np.array(X)
if t > self.time[-1] or t < self.time[0]:
return 0
uloc = mu(t) + matmult(K(t), (alpha(t).T - X.T))
return uloc
def Ksol(self, X, U):
time = self.time
P1 = np.array([1.0])
soln = self.odeIntegrator(
lambda t, y: self.peqns(t, y, self.A_interp, self.B_interp, self.
Rk, self.Qk),
P1,
)
# Convert the list to a numpy array.
psoln = np.array(soln).reshape(len(soln), 1)
K = np.empty((time.shape[0], self.nx))
for tindex, t in np.ndenumerate(time):
K[tindex, :] = matmult(self.B_current[tindex, 0], psoln[tindex])
self.K = K
return K
def descentdirection(self):
# solve for the descent direction by
X = self.X_current
U = self.U_current
time = self.time
Ps = self.Psol(X, U, time)
self.P_current = Ps
P_interp = interp1d(time, Ps.T)
Rs = self.Rsol(X, U, P_interp, time).flatten()
self.R_current = Rs
r_interp = interp1d(time, Rs.T)
zinit = -matmult(P_interp(0)**-1, r_interp(0))
soln = self.odeIntegrator(
lambda t, y: self.zeqns(
t,
y,
self.A_interp,
self.B_interp,
self.a_interp,
self.b_interp,
P_interp,
r_interp,
self.Rn,
self.Qn,
),
zinit,
)
# Convert the list to a numpy array.
zsoln = np.array(soln)
zsoln = zsoln.reshape(time.shape[0], 1)
vsoln = np.empty(U.shape)
for tindex, t in np.ndenumerate(time):
vsoln[tindex] = self.veqns(
zsoln[tindex],
self.A_current[tindex],
self.B_current[tindex],
self.a_current[tindex],
self.b_current[tindex],
Ps[tindex],
Rs[tindex],
self.Rn,
self.Qn,
)
return [zsoln, vsoln]
def veqns(self, zz, Al, Bl, a, b, Psol, Rsol, Rn, Qn):
vmatdiffeq = matmult(-Bl, Psol, zz) - matmult(Bl, Rsol) - b
return vmatdiffeq
def projcontrol(self, X, K, mu, alpha):
uloc = mu + matmult(K, (alpha.T - X.T))
return uloc
def dldu_pointwise(self, x, u):
# return the pointwise linearized cost WRT state
return matmult(self.R, u)
def cost_pointwise(self, x, u):
R = self.R
return 0.5 * matmult(u, R, u)