def state_dot_ref_frame(state, control, omega_r, inertia, inertia_inv):
# sigmas
a = (1 - state[0] @ state[0]) * np.identity(3) + 2 * ut.cross_product_operator(state[0]) + 2 * \
np.outer(state[0], state[0])
sigma_propagation = (1/4) * a @ (state[1] - omega_r)
# omegas
omega_propagation = inertia_inv @ ((-ut.cross_product_operator(state[1]) @ inertia @ state[1]) + control)
return np.array([sigma_propagation, omega_propagation])
def state_dot_reaction_wheels(state, control, inertia_rw, inertia_inv_rw, hs):
# sigmas
a = (1 - state[0] @ state[0]) * np.identity(3) + 2 * ut.cross_product_operator(state[0]) + 2 * \
np.outer(state[0], state[0])
sigma_propagation = (1/4) * a @ state[1]
# omegas
cross = ut.cross_product_operator(state[1])
omega_propagation = inertia_inv_rw @ ((-cross @ inertia_rw @ state[1]) - cross @ hs + control)
return np.array([sigma_propagation, omega_propagation])
def control_torque_ref_frame(sigma, omega, omega_r, omega_dot_r, inertia, K, P, i, prev_torque, max_torque=None):
# restrict control torque update to be only once per 100 iterations
if i % 100 != 0:
return prev_torque
# control torque law equation
val = -K * sigma - P * (omega - omega_r) + ut.cross_product_operator(omega) @ inertia @ omega
val += inertia @ (omega_dot_r - ut.cross_product_operator(omega) @ omega_r)
# if a max_torque is specified limit the torque along each dimension to the max torque
if max_torque is not None:
val[abs(val) > max_torque] = max_torque * np.sign(val)[abs(val) > max_torque]
return val
def aerodynamic_torque(v, rho, cubesat: CubeSat):
"""
Calculates the aerodynamic disturbance torque
:param v: velocity of spacecraft relative to air in body frame in m/s
:param rho: atmospheric density in kg/m^3
:param cubesat: CubeSat model
:return: np.ndarray. aerodynamic disturbance torque in the cubesat body frame
"""
# This function uses the equations for the free molecular flow dynamics
# (i.e. neglecting the affect of reemitted particles on the incident stream)
vm = np.linalg.norm(v)
ev = v * (1.0 / vm)
# calculate net torque
net_torque = np.zeros(3)
for face in cubesat.faces:
mu = np.dot(
ev,
face.normal) # cosine of angle between velocity and surface normal
if mu >= 0:
# units N, see eq'n 2-2 in NASA SP-8058 Spacecraft Aerodynamic Torques
force = -rho * vm**2 * (
2 * (1 - face.accommodation_coeff) * mu**2 * face.normal +
face.accommodation_coeff * mu * ev
) * face.area # units N, see eq'n 2-2 in NASA SP-8058 Spacecraft Aerodynamic Torques
# This addition is the diffuse term in Chris Robson's thesis. Just use 5 % of vm for now for the diffuse vel
# note: I think Chris is missing a second cosine in the specular reflection equation
force += face.accommodation_coeff * rho * vm * (
0.05 * vm) * face.area * mu * face.normal
net_torque += ut.cross_product_operator(
face.centroid - cubesat.center_of_mass) @ force # units N.m
return net_torque
def hysteresis_rod_torque_save(b, i, cubesat: CubeSat):
"""
This function is the same as above but should be used if you want to save the hystersis rods data.
:param b: external magnetic field in the body frame (Units: Telsa)
:param i: current index of integration
:param cubesat: CubeSat model
:return:
"""
h = b / u0
torque = np.zeros((len(cubesat.hyst_rods), 3))
# for each hysteresis rod
for j, rod in enumerate(cubesat.hyst_rods):
# propagate the magnetic field of the rod
h_proj = h @ rod.axes_alignment # calculate component of h along axis of hysteresis rod
rod.propagate_and_save_magnetization(h_proj, i)
# calculate m from b of the rod
m = rod.axes_alignment * rod.b[i] * rod.volume / u0
# calculate m x B torque
torque[j] = ut.cross_product_operator(m) @ b
return torque
def _hysteresis_rod_torque(self, b, cubesat: CubeSat, rod_function: str):
h = b / self._u0
torque = 0
# for each hysteresis rod
for rod in cubesat.hyst_rods:
# propagate the magnetic field of the rod
h_proj = h @ rod.axes_alignment # calculate component of h along axis of hysteresis rod
if rod_function == 'propagate_magnetization':
b_current = rod.propagate_magnetization(h_proj)
elif rod_function == 'propagate_and_save_magnetization':
b_current = rod.propagate_and_save_magnetization(h_proj)
elif rod_function == 'peek_magnetization':
b_current = rod.peek_magnetization(h_proj)
else:
raise ValueError(f'Unknown rod function {rod_function}')
# calculate m from b of the rod
m = rod.axes_alignment * b_current * rod.volume / self._u0
# calculate m x B torque
torque += ut.cross_product_operator(m) @ b
return torque
def solar_pressure(sun_vec, sun_vec_inertial, satellite_vec_inertial,
cubesat: CubeSat):
"""
Calculate the solar pressure torque on the cubesat.
:param sun_vec: sun unit vector in body frame
:param sun_vec_inertial: sun vector in inertial frame
:param satellite_vec_inertial: satellite position vector in inertial frame
:param cubesat: CubeSat model
"""
# calculate the solar irradiance using equation 3-53 in Chris Robson's thesis
const = a_solar_constant / (np.linalg.norm(sun_vec_inertial -
satellite_vec_inertial))**2
net_torque = np.zeros(3)
for face in cubesat.faces:
cos_theta = face.normal @ sun_vec
if cos_theta > 0:
# calculate the solar radiation pressure torque using the equation in SMAD
# net_torque += const * face.area * (1 + face.reflection_coeff) * cos_theta * \
# (ut.cross_product_operator(face.centroid - cubesat.center_of_mass) @
# -face.normal) # force is opposite to area
# Calculate the solar radiation pressure torque using equation 3.167 in Landis Markley's Fundamentals of
# Spacecraft Attitude Determination and Control textbook. This equation is equivalent to Chris Robson's
# equations 3-55 to 3-58. Note: I think Chris is missing a second cosine in the specular reflection equation
force = -const * face.area * cos_theta * \
(2*(face.diff_ref_coeff/3 + face.spec_ref_coeff*cos_theta)*face.normal
+ (1 - face.spec_ref_coeff)*sun_vec)
net_torque += ut.cross_product_operator(
face.centroid - cubesat.center_of_mass) @ force
return net_torque
def triad(v1n, v2n, v1b, v2b):
# note: first vector should be the one that is known more accurately
t1n = v1n
cross = ut.cross_product_operator(v1n) @ v2n
t2n = cross / np.linalg.norm(cross)
t3n = ut.cross_product_operator(t1n) @ t2n
matn = np.array([t1n, t2n, t3n])
t1b = v1b
cross = ut.cross_product_operator(v1b) @ v2b
t2b = cross / np.linalg.norm(cross)
t3b = ut.cross_product_operator(t1b) @ t2b
matb = np.array([t1b, t2b, t3b]).T
return matb @ matn
def gravity_gradient(self, ue, r0, cubesat: CubeSat):
"""
Calculates the gravity gradient disturbance torque
:param ue: Unit vector towards nadir in the cubesat body frame
:param r0: distance to the center of the earth
:param cubesat: CubeSat model
:return: np.ndarray. gravity gradient disturbance torque in the cubesat body frame
"""
return (self._a_gravity_gradient_constant/(r0**3)) * (ut.cross_product_operator(ue) @ cubesat.inertia @ ue)
def total_magnetic(self, b, cubesat: CubeSat):
"""
This function calculates the total magnetic torque exhibited on the satellite from magnetic moments defined in
the CubeSat model
:param b: external magnetic field in the body frame (Units: Telsa)
:param cubesat: CubeSat model
:return: np.ndarray. magnetic torque in the cubesat body frame
"""
return ut.cross_product_operator(cubesat.total_magnetic_moment) @ b # both vectors must be in the body frame
def state_dot_mrp(state, control, inertia, inertia_inv):
"""
Differential attitude equation for the Modified Rodriguez Parameters (MRP) attitude parametrization, with eulers
rotational equation of motion.
:param state: np.ndarray. The current attitude state (first index is attitude, second index is angular velocity)
:param control: np.ndarray. The current applied torque to the spacecraft in the body frame
:param inertia: np.ndarray. Moment of inertial tensor of the spacecraft.
:param inertia_inv: np.ndarray. Inverse of the moment of inertial tensor of the spacecraft.
:return: np.ndarray. First index is value of attitude differential equation, second is value of angular velocity
differential equation
"""
# sigmas
a = (1 - state[0] @ state[0]) * np.identity(3) + 2 * ut.cross_product_operator(state[0]) + 2 * \
np.outer(state[0], state[0])
sigma_propagation = (1/4) * a @ state[1]
# omegas
omega_propagation = inertia_inv @ ((-ut.cross_product_operator(state[1]) @ inertia @ state[1]) + control)
return np.array([sigma_propagation, omega_propagation])
def state_dot_mrp(time: float, state: np.ndarray, attitude: AttitudeData, orbit: OrbitData, cubesat: CubeSat, disturbance_torques: DisturbanceTorques):
"""
Differential attitude equation for the Modified Rodriguez Parameters (MRP) attitude parametrization, with eulers
rotational equation of motion.
:param state: np.ndarray. The current attitude state (first index is attitude, second index is angular velocity)
:param
:return: np.ndarray. First index is value of attitude differential equation, second is value of angular velocity
differential equation
"""
# sigmas
a = (1 - state[0] @ state[0]) * np.identity(3) + 2 * ut.cross_product_operator(state[0]) + 2 * \
np.outer(state[0], state[0])
sigma_propagation = (1/4) * a @ state[1]
control = disturbance_torques.torque(time, state, attitude, orbit, cubesat)
# omegas
omega_propagation = cubesat.inertia_inv @ ((-ut.cross_product_operator(state[1]) @ cubesat.inertia @ state[1]) + control)
return np.array([sigma_propagation, omega_propagation])
def crp_to_dcm(q):
"""
This function generates the DCM coordinates corresponding to the DCM input
Taken from Part1/7_Classical-Rodrigues-Parameters-_CRP_.pdf page 82
note: this function has a SINGULARITY
:param q: CRP coordinate vector
:return: DCM
"""
s = q @ q
return (1/(1 + s))*((1 - s)*np.identity(3) + 2*np.outer(q, q) - 2*ut.cross_product_operator(q))
def mrp_to_dcm(sigma):
"""
This function generates the DCM coordinates corresponding to the MRP input
Taken from Part1/8_Modified-Rodrigues-Parameters-_MRP_.pdf page 97
:param sigma: MRP coordinate vector
:return: DCM
"""
sigma_cross = ut.cross_product_operator(sigma)
s = sigma @ sigma
return np.identity(3) + (8*(sigma_cross @ sigma_cross) - 4*(1 - s)*sigma_cross)/(1 + s)**2
def reaction_wheel_control(sigma, omega, inertia_rw, K, P, i, prev_torque, hs, max_torque=None):
# restrict control torque update to be only once per 100 iterations
if i % 100 != 0:
return prev_torque
# control torque law equation
val = -K * sigma - P * omega + ut.cross_product_operator(omega) @ (inertia_rw @ omega + hs)
# if a max_torque is specified limit the torque along each dimension to the max torque
if max_torque is not None:
val[abs(val) > max_torque] = max_torque * np.sign(val)[abs(val) > max_torque]
return val
def test_initial_align_gravity_stabilization():
p = np.random.random(3)
p = p/np.linalg.norm(p)
v = np.random.random(3)
v = v / np.linalg.norm(v)
dcm = ut.initial_align_gravity_stabilization(p, v)
cross_track = ut.cross_product_operator(p) @ v
cross_track = cross_track/np.linalg.norm(cross_track)
np.testing.assert_almost_equal(dcm @ p, np.array([0, 0, 1]))
np.testing.assert_almost_equal(dcm @ cross_track, np.array([0, 1, 0]))
def hysteresis_rod_torque(b, cubesat: CubeSat):
"""
This function calculates the torque exerted on the spacecraft from any hysteresis rods in the cubesat model
:param b: external magnetic field in the body frame (Units: Telsa)
:param cubesat: CubeSat model
:return: np.ndarray. magnetic torque from hysteresis rods in the cubesat body frame
"""
h = b / u0
torque = 0
# for each hysteresis rod
for rod in cubesat.hyst_rods:
# propagate the magnetic field of the rod
h_proj = h @ rod.axes_alignment # calculate component of h along axis of hysteresis rod
rod.propagate_magnetization(h_proj)
# calculate m from b of the rod
m = rod.axes_alignment * rod.b_current * rod.volume / u0
# calculate m x B torque
torque += ut.cross_product_operator(m) @ b
return torque
dcm[0] = tr.mrp_to_dcm(states[0][0])
mag = np.zeros((len(time), 3))
m = np.zeros((len(time), 3))
# create magnetic field vector
mag_vec_def = np.array([1, 2, 2])
mag_vec_def = mag_vec_def / np.linalg.norm(mag_vec_def) * 50 * (10**-6)
for i in range(len(time) - 1):
# get magnetometer measurement
# mag_vec[i+1] = dcm_mag_vec @ mag_vec[i]
mag[i] = dcm[i] @ mag_vec_def
# get control torques
m[i] = cl.b_dot(mag[i], mag[i - 1], K, i, m[i - 1], time_step * 1)
controls[i] = ut.cross_product_operator(m[i]) @ mag[i]
# propagate attitude state
states[i + 1] = it.rk4(st.state_dot_mrp, time_step, states[i], controls[i],
inertia, inertia_inv)
# do 'tidy' up things at the end of integration (needed for many types of attitude coordinates)
states[i + 1] = ic.mrp_switching(states[i + 1])
dcm[i + 1] = tr.mrp_to_dcm(states[i + 1][0])
if __name__ == "__main__":
omegas = states[:, 1]
sigmas = states[:, 0]
def _plot(data, title, ylabel):
plt.figure()
def omega_dot(omega):
return np.linalg.inv(inertia) @ (
-ut.cross_product_operator(omega) @ inertia @ omega)
def sigma_dot(sigma, omega):
A = (1 - sigma @ sigma) * np.identity(3) + 2 * ut.cross_product_operator(
sigma) + 2 * np.outer(sigma, sigma)
return (1 / 4) * A @ omega
for i in range(len(time) - 1):
# mag_vec = np.array([np.interp(i*0.1, xp, mag_vec_def[:, 0]), np.interp(i*0.1, xp, mag_vec_def[:, 1]), np.interp(i*0.1, xp, mag_vec_def[:, 2])])
# mag_vec = mag_vec_magnitude*mag_vec/np.linalg.norm(mag_vec)
# mag[i] = dcm[i] @ mag_vec
mag[i] = dcm[i] @ mag_vec_def
# get reference frame
sigma_br = rf.get_mrp_br(
dcm_rn, states[i]
[0]) # note: angular velocities need to be added to reference frame
# get the hs variable needed for control torque calculation and state propagation
hs = reaction_wheel_inertia[0] * (wheel_angular_vel[i])
# get magnetic moment and wheel angular acceleration that create approx zero control torque
m[i] = -K * (ut.cross_product_operator(mag[i]) @ hs)
omega_dot = (states[i + 1][1, 0] - states[i][1, 0]) / time_step
wheel_angular_accel[i] = (ut.cross_product_operator(m[i])
@ mag[i]) / reaction_wheel_inertia[0] - omega_dot
# get real controls corresponding to this
real_controls[i] = ut.cross_product_operator(
m[i]) @ mag[i] + reaction_wheel_inertia[0] * (wheel_angular_accel[i] +
omega_dot)
# propagate attitude state
states[i + 1] = it.rk4(st.state_dot_mrp, time_step, states[i],
real_controls[i], inertia, inertia_inv)
# get wheel angular velocity
wheel_angular_vel[
