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 constant acceleration as MEE time derivatives.
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:
Xdot: ndarray
(m, 6) array of state derivatives.
"""
m = T.shape[0]
u = np.tile(self.vector, (m, 1, 1))
G = GVE(mu=self.mu)(T, X)
self.u = u.reshape((m, 3))
return (G @ u).reshape((m, 6))
def __call__(self, T, X):
"""Calculate zonal gravity perturations for MEEs with Ml0.
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:
Xdot: ndarray
(m, 6) array of state derivatives.
"""
super().lvlh_acceleration(T, convert.rv_mee(convert.mee_meeMl0(T, X)))
G = GVE()(T, X)
m = T.shape[0]
return (G @ self.a_lvlh.reshape((m, 3, 1))).reshape((m, 6))
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))
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:
Xdot: ndarray
(m, 6) array of state derivatives.
"""
# state errors
m = T.shape[0]
Y = self.Y_func(T)
self.E = mod_angles(X - Y)
# delta control
G_r = GVE(mu=self.mu)(T, Y)
self.du = -(self.a_t * G_r.T @ self.E.reshape((m, 6, 1))).reshape(
(m, 3))
self.u = self.U_func(T) + self.du
# lyapunov function and derivative
self.V = (self.E.reshape(
(m, 1, 6)) @ np.tile(self.W, (m, 1, 1)) @ self.E.reshape(
(m, 6, 1))).reshape((m, 1))
self.dVdt = (self.E.reshape(
(m, 1, 6)) @ np.tile(self.W, (m, 1, 1)) @ G_r @ self.du.reshape(
(m, 3, 1))).reshape((m, 1))
self.u.shape = (m, 3)
return self.a_t * (G_r @ self.u.reshape((m, 3, 1))).reshape((m, 6))
def __call__(self, T, X, a_d):
"""Calculate GVESSMs for MEEs with mean longitude at epoch.
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
a_d: ndarray
(m, 3) generalized LVLH perturbation vector.
Returns:
dGdx: ndarray
(m, 6, 6) array of STMs, or an (m, 36, 36) block diagonal of
STMs, depending on the value of vectorize.
"""
m = T.shape[0]
Xh = (np.tile(X.reshape((m, 1, 6)), (1, 6, 1)) +
np.tile(np.identity(6), (m, 1, 1))*self.h)
G_func = GVE(mu=self.mu)
G = G_func(T, X)
a_d = a_d.reshape((m, 3, 1))
return np.concatenate(
[(G_func(T, x.reshape((m, 6))) - G)/self.h @ a_d
for x in Xh.transpose((1, 0, 2))],
axis=2
)
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:
Xdot: ndarray
(m, 6) array of state derivatives.
"""
m = T.shape[0]
G = GVE(mu=self.mu)(T, X)
# state errors
Y = self.Y_func(T)
self.E = mod_angles(X - Y)
# control
EW = (self.E @ self.W).reshape((m, 1, 6))
c = EW @ G
if self.steering:
self.u = np.array([-x / np.linalg.norm(x, axis=1) for x in c])
else:
self.u = np.zeros((m, 3))
for i, x in enumerate(c):
x_norm = np.linalg.norm(x, axis=1)
if x_norm > 1:
self.u[i] = -x / x_norm
elif x_norm < self.lower_bound:
self.u[i] = -self.lower_bound * x / x_norm
else:
self.u[i] = -x
if self.sliding_mag:
E_norm = np.tile(
np.linalg.norm(self.E, axis=1).reshape((m, 1, 1)), (1, 1, 3))
self.u = self.u * E_norm / E_norm[0, 0, 0]
# lyapunov function and derivative
self.V = (self.E.reshape(
(m, 1, 6)) @ np.tile(self.W, (m, 1, 1)) @ self.E.reshape(
(m, 6, 1))).reshape((m, 1))
kepdyn = KeplerianDynamics()
a_d = self.a_t * G @ self.u.reshape((m, 3, 1))
dXdt = kepdyn(T, X) + a_d.reshape((m, 6))
dYdt = kepdyn(T, Y)
dEdt = dXdt - dYdt
self.dVdt = (self.E.reshape(
(m, 1, 6)) @ np.tile(self.W, (m, 1, 1)) @ dEdt.reshape(
(m, 6, 1))).reshape((m, 1))
self.u.shape = (m, 3)
return self.a_t * (G @ self.u.reshape((m, 3, 1))).reshape((m, 6))