def __call__(self, T, X):
"""Calculate control.
Args:
T: ndarray
(m, 1) array of times.
X: ndarray
(m, 6) array of modified equinoctial elements ordered as
(p, f, g, h, k, Ml0), where
p = semi-latus rectum
f = 1-component of eccentricity vector in perifocal frame
g = 2-component of eccentricity vector in perifocal frame
h = 1-component of the ascending node vector in equ. frame
k = 2-component of the ascending node vector in equ. frame
Ml0 = mean longitude at epoch
Returns:
u: ndarray
(m, 3) array of control vectors.
"""
m = T.shape[0]
G = GVE(mu=self.mu)(T, self.X(T))
GTG_inv = np.array([np.linalg.inv(g.T @ g)
for g in G])
return (
GTG_inv @ G.transpose((0, 2, 1)) @
self.Xdot(T).reshape((m, 6, 1))
).reshape((m, 3))
def __call__(self, T, X):
"""Calculate control.
Args:
T: ndarray
(m, 1) array of times.
X: ndarray
(m, 6) array of modified equinoctial elements ordered as
(p, f, g, h, k, Ml0), where
p = semi-latus rectum
f = 1-component of eccentricity vector in perifocal frame
g = 2-component of eccentricity vector in perifocal frame
h = 1-component of the ascending node vector in equ. frame
k = 2-component of the ascending node vector in equ. frame
Ml0 = mean longitude at epoch
Returns:
u: ndarray
(m, 3) array of control vectors.
"""
m = T.shape[0]
G = GVE(mu=self.mu)(T, X)
GTG_inv = np.array([np.linalg.inv(g.T @ g) for g in G])
self.E = np.tile(self.Xf, T.shape) - X
u = (GTG_inv @ G.transpose((0, 2, 1)) @ self.E.reshape(
(m, 6, 1))).reshape((m, 3))
# u = np.array([u_i / np.linalg.norm(u_i) if np.linalg.norm(u_i) > 1
# else u_i for u_i in u])
u = u / np.tile(np.linalg.norm(u, axis=1, keepdims=True), (1, 3))
self.u = u
return self.a_t * (G @ u.reshape((m, 3, 1))).reshape((m, 6))