def test_mrp_valid_dcm():
dcm1 = ut.random_dcm()
b = tr.dcm_to_mrp(dcm1)
dcm2 = tr.mrp_to_dcm(b)
np.testing.assert_almost_equal(dcm2 @ dcm2.T, np.identity(3))
np.testing.assert_almost_equal(np.linalg.det(dcm2), 1)
for i in (0, 1, 2):
np.testing.assert_almost_equal(np.linalg.norm(dcm2[i]), 1)
np.testing.assert_almost_equal(np.linalg.norm(dcm2[:, i]), 1)
def mrp_triad_with_noise(sigma, v1n, v2n, arbitrary_error1, arbitrary_error2):
# TODO: make the error not arbitrary (e.g. This function could produce a random unit vector in a conical section defined by some angle input)
dcm_bn = tr.mrp_to_dcm(sigma)
v1b = dcm_bn @ v1n + np.random.random_sample(3) * arbitrary_error1
v2b = dcm_bn @ v2n + np.random.random_sample(3) * arbitrary_error2
v1b = v1b / np.linalg.norm(v1b)
v2b = v2b / np.linalg.norm(v2b)
dcm_bn_triad = triad(v1n, v2n, v1b, v2b)
return tr.dcm_to_mrp(dcm_bn_triad)
sigmas[0] = np.array([0.60, -0.4, 0.2])
omegas[0] = np.array([0.1, 0.01, -0.05])
for i in range(len(time) - 1):
omegas[i + 1] = omegas[i] + time_step * omega_dot(omegas[i])
sigmas[i + 1] = sigmas[i] + time_step * sigma_dot(sigmas[i], omegas[i])
# 3.0 plot angular velocites
plt.figure()
plt.plot(time, omegas)
plt.title('angular velocity components')
# 3.1 plot attitude representation components
plt.figure()
plt.plot(time, sigmas)
plt.title('attitude representation components')
# 4.0 Animate this result so we can actually see what is going on
cubesat = CubeSatAerodynamicEx1()
# need the 3x3 matrix representation of attitude (Directional Cosine Matrix (DCM)) for the animation routine.
dcm = np.zeros((len(time), 3, 3))
for i in range(len(time)):
dcm[i] = tr.mrp_to_dcm(sigmas[i])
ref = DrawingVectors(dcm[::100],
'axes',
color=['C0', 'C1', 'C2'],
label=['Body x', 'Body y', 'Body z'],
length=0.2)
a = AnimateAttitude(dcm[::100], draw_vector=ref, cubesat_model=cubesat)
a.animate()
def sim_attitude(sim_params, cubesat_params, file_name, save=True, ret=False):
if isinstance(sim_params, str):
sim_params = eval(sim_params)
save_every = sim_params[
'save_every'] # only save the data every number of iterations
# declare time step for integration
time_step = sim_params['time_step']
end_time = sim_params['end_time_index']
time = np.arange(0, end_time, time_step)
le = int(len(time) / save_every)
# create the CubeSat model
cubesat = CubeSat.fromdict(cubesat_params)
# declare memory
states = np.zeros((le + 1, 2, 3))
dcm_bn = np.zeros((le, 3, 3))
dcm_on = np.zeros((le, 3, 3))
dcm_bo = np.zeros((le, 3, 3))
controls = np.zeros((le, 3))
nadir = np.zeros((le, 3))
sun_vec = np.zeros((le, 3))
sun_vec_body = np.zeros((le, 3))
lons = np.zeros(le)
lats = np.zeros(le)
alts = np.zeros(le)
positions = np.zeros((le, 3))
velocities = np.zeros((le, 3))
aerod = np.zeros((le, 3))
gravityd = np.zeros((le, 3))
solard = np.zeros((le, 3))
magneticd = np.zeros((le, 3))
density = np.zeros(le)
mag_field = np.zeros((le, 3))
mag_field_body = np.zeros((le, 3))
solar_power = np.zeros(le)
is_eclipse = np.zeros(le)
hyst_rod = np.zeros((le, len(cubesat.hyst_rods), 3))
h_rods = np.zeros((le, len(cubesat.hyst_rods)))
b_rods = np.zeros((le, len(cubesat.hyst_rods)))
# load saved data
saved_data = xr.open_dataset(
os.path.join(os.path.dirname(__file__), '../../orbit_pre_process.nc'))
start_time = datetime.strptime(sim_params['start_time'],
"%Y/%m/%d %H:%M:%S")
start_time = start_time.replace(tzinfo=utc)
final_time = start_time + timedelta(seconds=time_step * len(time))
orbit_data = saved_data.sel(time=slice(None, final_time))
x = np.linspace(0, len(time), len(orbit_data.time))
a = len(orbit_data.time)
ab = np.concatenate(
(orbit_data.sun.values, orbit_data.mag.values,
orbit_data.atmos.values.reshape(
a, 1), orbit_data.lons.values.reshape(
a, 1), orbit_data.lats.values.reshape(a, 1),
orbit_data.alts.values.reshape(
a, 1), orbit_data.positions.values, orbit_data.velocities.values),
axis=1)
interp_data = interp1d(x, ab.T)
def interp_info(i):
ac = interp_data(i)
return ac[0:3], ac[3:6], ac[6], ac[7], ac[8], ac[9], ac[10:13], ac[
13:16]
# initialize attitude so that z direction of body frame is aligned with nadir
sun_vec[0], mag_field[0], density[0], lons[0], lats[0], alts[0], positions[
0], velocities[0] = interp_info(0)
sigma0 = np.array(sim_params['sigma0'])
dcm0 = tr.mrp_to_dcm(sigma0)
omega0_body = np.array(sim_params['omega0_body'])
omega0 = dcm0.T @ omega0_body
states[0] = state = [sigma0, omega0]
dcm_bn[0] = tr.mrp_to_dcm(states[0][0])
dcm_on[0] = ut.inertial_to_orbit_frame(positions[0], velocities[0])
dcm_bo[0] = dcm_bn[0] @ dcm_on[0].T
# Put hysteresis rods in an initial state that is reasonable. (Otherwise you can get large magnetization from the rods)
mag_field_body[0] = (
dcm_bn[0] @ mag_field[0]) * 10**-9 # in the body frame in units of T
for rod in cubesat.hyst_rods:
rod.define_integration_size(le + 1)
axes = np.argwhere(rod.axes_alignment == 1)[0][0]
rod.h[0] = rod.h_current = mag_field_body[0][axes] / cubesat.hyst_rods[
0].u0
rod.b[0] = rod.b_current = rod.b_field_bottom(rod.h_current)
# the integration
k = 0
for i in tqdm(range(len(time) - 1)):
# do the interpolation for various things
sun_vec[k], mag_field[k], density[k], lons[k], lats[k], alts[
k], positions[k], velocities[k] = interp_info(i)
# keep track of dcms for various frame
dcm_bn[k] = tr.mrp_to_dcm(state[0])
dcm_on[k] = ut.inertial_to_orbit_frame(positions[k], velocities[k])
dcm_bo[k] = dcm_bn[k] @ dcm_on[k].T
# get magnetic field in inertial frame (for show right now)
mag_field_body[k] = (dcm_bn[k] @ mag_field[k]
) * 10**-9 # in the body frame in units of T
# get unit vector towards nadir in body frame (for gravity gradient torque)
R0 = np.linalg.norm(positions[k])
nadir[k] = -positions[k] / R0
ue = dcm_bn[k] @ nadir[k]
# get sun vector in inertial frame (GCRS) (for solar pressure torque)
sun_vec_norm = sun_vec[k] / np.linalg.norm(sun_vec[k])
sun_vec_body[k] = dcm_bn[k] @ sun_vec_norm # in the body frame
# get atmospheric density and velocity vector in body frame (for aerodynamic torque)
vel_body = dcm_bn[k] @ dt.get_air_velocity(velocities[k], positions[k])
# check if satellite is in eclipse (spherical earth approximation)
theta = np.arcsin(6378000 / (6378000 + alts[k]))
angle_btw = np.arccos(
nadir[k]
@ sun_vec_norm) # note: these vectors will be normalized already.
if angle_btw < theta:
is_eclipse[k] = 1
# get disturbance torque
aerod[k] = dt.aerodynamic_torque(vel_body, density[k], cubesat)
if not is_eclipse[k]:
solard[k] = dt.solar_pressure(sun_vec_body[k], sun_vec[k],
positions[k], cubesat)
gravityd[k] = dt.gravity_gradient(ue, R0, cubesat)
magneticd[k] = dt.total_magnetic(mag_field_body[k], cubesat)
hyst_rod[k] = dt.hysteresis_rod_torque_save(mag_field_body[k], k,
cubesat)
controls[k] = aerod[k] + solard[k] + gravityd[k] + magneticd[
k] + hyst_rod[k].sum(axis=0)
# calculate solar power
if not is_eclipse[k]:
solar_power[k] = dt.solar_panel_power(sun_vec_body[k], sun_vec[k],
positions[k], cubesat)
# propagate attitude state
states[k + 1] = it.rk4(st.state_dot_mrp, time_step, state, controls[k],
cubesat.inertia, cubesat.inertia_inv)
# do 'tidy' up things at the end of integration (needed for many types of attitude coordinates)
states[k + 1] = state = ic.mrp_switching(states[k + 1])
if not i % save_every:
k += 1
if k >= le:
break
states = np.delete(states, 1, axis=0)
for i, rod in enumerate(cubesat.hyst_rods):
b_rods[:, i] = rod.b = np.delete(rod.b, 1, axis=0)
h_rods[:, i] = rod.h = np.delete(rod.h, 1, axis=0)
omegas = states[:, 1]
sigmas = states[:, 0]
# save the data
sim_params_dict = {
'time_step': time_step,
'save_every': save_every,
'end_time_index': end_time,
'start_time': start_time.strftime('%Y/%m/%d %H:%M:%S'),
'final_time': final_time.strftime('%Y/%m/%d %H:%M:%S'),
'omega0_body': omega0_body.tolist(),
'sigma0': sigma0.tolist()
}
a = xr.Dataset(
{
'sun': (['time', 'cord'], sun_vec),
'mag': (['time', 'cord'], mag_field),
'atmos': ('time', density),
'lons': ('time', lons),
'lats': ('time', lats),
'alts': ('time', alts),
'positions': (['time', 'cord'], positions),
'velocities': (['time', 'cord'], velocities),
'dcm_bn': (['time', 'dcm_mat_dim1', 'dcm_mat_dim2'], dcm_bn),
'dcm_bo': (['time', 'dcm_mat_dim1', 'dcm_mat_dim2'], dcm_bo),
'angular_vel': (['time', 'cord'], omegas),
'controls': (['time', 'cord'], controls),
'gg_torque': (['time', 'cord'], gravityd),
'aero_torque': (['time', 'cord'], aerod),
'solar_torque': (['time', 'cord'], solard),
'magnetic_torque': (['time', 'cord'], magneticd),
'hyst_rod_torque': (['time', 'hyst_rod', 'cord'], hyst_rod),
'hyst_rod_magnetization': (['time', 'hyst_rod'], b_rods),
'hyst_rod_external_field': (['time', 'hyst_rod'], h_rods),
'nadir': (['time', 'cord'], nadir),
'solar_power': ('time', solar_power)
},
coords={
'time': np.arange(0, le, 1),
'cord': ['x', 'y', 'z'],
'hyst_rod': [f'rod{i}' for i in range(len(cubesat.hyst_rods))]
},
attrs={
'simulation_parameters':
str(sim_params_dict),
'cubesat_parameters':
str(cubesat.asdict()),
'description':
'University of kentucky attitude propagator software '
'(they call it SNAP) recreation'
})
# Note: the simulation and cubesat parameter dictionaries are saved as strings for the nc file. If you wish
# you could just eval(a.cubesat_parameters) to get the dictionary back.
if save:
a.to_netcdf(
os.path.join(os.path.dirname(__file__), f'../../{file_name}.nc'))
dcm_to_stk_simple(
time[::save_every], dcm_bn,
os.path.join(os.path.dirname(__file__), f'../../{file_name}.a'))
if ret:
return a
inertia = np.diag([2 * (10**-3), 8 * (10**-3), 6 * (10**-3)])
inertia_inv = np.linalg.inv(inertia)
omega0 = np.array([0.1, 0.1, 0.1])
sigma0 = np.array([0.20, -0.4, 0.2])
K = 10000
max_torque = None
# The integration
time_step = 0.1
end_time = 4000
time = np.arange(0, end_time, time_step)
states = np.zeros((len(time), 2, 3))
controls = np.zeros((len(time), 3))
states[0] = [sigma0, omega0]
dcm = np.zeros((len(time), 3, 3))
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]
def sim_attitude(sim_params, cubesat_params, file_name, save=True, ret=False):
if isinstance(sim_params, str):
sim_params = eval(sim_params)
save_every = sim_params['save_every'] # only save the data every number of iterations
# declare time step for integration
time_step = sim_params['time_step']
end_time = sim_params['duration']
time = np.arange(0, end_time, time_step)
le = (len(time) - 1) // save_every + 1 # Number of time steps where data points are saved
num_simulation_data_points = int(sim_params['duration'] // sim_params['time_step']) + 1
start_time = datetime.strptime(sim_params['start_time'], "%Y/%m/%d %H:%M:%S")
start_time = start_time.replace(tzinfo=utc)
final_time = start_time + timedelta(seconds=sim_params['time_step']*num_simulation_data_points)
# create the CubeSat model
cubesat = CubeSat.fromdict(cubesat_params)
states = np.zeros((le, 2, 3))
dcm_bn = np.zeros((le, 3, 3))
dcm_on = np.zeros((le, 3, 3))
dcm_bo = np.zeros((le, 3, 3))
controls = np.zeros((le, 3))
nadir = np.zeros((le, 3))
sun_vec = np.zeros((le, 3))
sun_vec_body = np.zeros((le, 3))
lons = np.zeros(le)
lats = np.zeros(le)
alts = np.zeros(le)
positions = np.zeros((le, 3))
velocities = np.zeros((le, 3))
aerod = np.zeros((le, 3))
gravityd = np.zeros((le, 3))
solard = np.zeros((le, 3))
magneticd = np.zeros((le, 3))
density = np.zeros(le)
mag_field = np.zeros((le, 3))
mag_field_body = np.zeros((le, 3))
solar_power = np.zeros(le)
is_eclipse = np.zeros(le)
hyst_rod = np.zeros((le, len(cubesat.hyst_rods), 3))
h_rods = np.zeros((le, len(cubesat.hyst_rods)))
b_rods = np.zeros((le, len(cubesat.hyst_rods)))
# load saved orbit and environment data
with xr.open_dataset(os.path.join(os.path.dirname(__file__), '../../orbit_pre_process.nc')) as saved_data:
orbit = OrbitData(sim_params, saved_data)
# allocate space for attitude data
attitude = AttitudeData(cubesat)
# initialize attitude so that z direction of body frame is aligned with nadir
# sun_vec[0], mag_field[0], density[0], lons[0], lats[0], alts[0], positions[0], velocities[0] = interp_info(0)
attitude.interp_orbit_data(orbit, 0.0)
sun_vec[0], mag_field[0], density[0], lons[0], lats[0], alts[0], positions[0], velocities[0] = attitude.temp.sun_vec, attitude.temp.mag_field, attitude.temp.density, attitude.temp.lons, attitude.temp.lats, attitude.temp.alts, attitude.temp.positions, attitude.temp.velocities
sigma0 = np.array(sim_params['sigma0'])
omega0_body = np.array(sim_params['omega0_body'])
states[0] = state = [sigma0, omega0_body]
dcm_bn[0] = tr.mrp_to_dcm(states[0][0])
dcm_on[0] = ut.inertial_to_orbit_frame(positions[0], velocities[0])
dcm_bo[0] = dcm_bn[0] @ dcm_on[0].T
# Put hysteresis rods in an initial state that is reasonable. (Otherwise you can get large magnetization from the rods)
mag_field_body[0] = (dcm_bn[0] @ mag_field[0]) * 10 ** -9 # in the body frame in units of T
for rod in cubesat.hyst_rods:
rod.define_integration_size(le)
axes = np.argwhere(rod.axes_alignment == 1)[0][0]
rod.h[0] = rod.h_current = mag_field_body[0][axes]/cubesat.hyst_rods[0].u0
rod.b[0] = rod.b_current = rod.b_field_bottom(rod.h_current)
# initialize the disturbance torque object
disturbance_torques = dt.DisturbanceTorques(**{torque: True for torque in sim_params['disturbance_torques']})
if 'aerodynamic' in sim_params['disturbance_torques']:
cubesat.create_aerodynamic_table(disturbance_torques.aerodynamic_torque, 101, 101)
if 'solar' in sim_params['disturbance_torques']:
cubesat.create_solar_table(disturbance_torques.solar_pressure, 101, 101)
if sim_params['calculate_power']:
cubesat.create_power_table(disturbance_torques.solar_panel_power, 101, 101)
disturbance_torques.save_hysteresis = True
# the integration
k = 0
for i in tqdm(range(len(time) - 1)):
# propagate attitude state
disturbance_torques.propagate_hysteresis = True # should propagate the hysteresis history, bringing it up to the current position
disturbance_torques.save_torques = True
state = it.rk4(st.state_dot_mrp, time[i], state, time_step, attitude, orbit, cubesat, disturbance_torques)
# controls[k] = ...
# do 'tidy' up things at the end of integration (needed for many types of attitude coordinates)
state = ic.mrp_switching(state)
if not (i + 1) % save_every:
k += 1
states[k] = state
dcm_bn[k] = attitude.save.dcm_bn
dcm_on[k] = attitude.save.dcm_on
dcm_bo[k] = attitude.save.dcm_bo
controls[k] = attitude.save.controls
nadir[k] = attitude.save.nadir
sun_vec[k] = attitude.save.sun_vec
sun_vec_body[k] = attitude.save.sun_vec_body
lons[k] = attitude.save.lons
lats[k] = attitude.save.lats
alts[k] = attitude.save.alts
positions[k] = attitude.save.positions
velocities[k] = attitude.save.velocities
aerod[k] = attitude.save.aerod
gravityd[k] = attitude.save.gravityd
solard[k] = attitude.save.solard
magneticd[k] = attitude.save.magneticd
density[k] = attitude.save.density
mag_field[k] = attitude.save.mag_field
mag_field_body[k] = attitude.save.mag_field_body
solar_power[k] = attitude.save.solar_power
is_eclipse[k] = attitude.save.is_eclipse
hyst_rod[k] = attitude.save.hyst_rod
disturbance_torques.save_hysteresis = True
if k >= le - 1:
break
for i, rod in enumerate(cubesat.hyst_rods):
b_rods[:, i] = rod.b
h_rods[:, i] = rod.h
omegas = states[:, 1]
sigmas = states[:, 0]
# save the data
sim_params_dict = {'time_step': time_step, 'save_every': save_every, 'duration': end_time,
'start_time': start_time.strftime('%Y/%m/%d %H:%M:%S'),
'final_time': final_time.strftime('%Y/%m/%d %H:%M:%S'), 'omega0_body': omega0_body.tolist(),
'sigma0': sigma0.tolist()}
a = xr.Dataset({'sun': (['time', 'cord'], sun_vec),
'mag': (['time', 'cord'], mag_field),
'atmos': ('time', density),
'lons': ('time', lons),
'lats': ('time', lats),
'alts': ('time', alts),
'positions': (['time', 'cord'], positions),
'velocities': (['time', 'cord'], velocities),
'dcm_bn': (['time', 'dcm_mat_dim1', 'dcm_mat_dim2'], dcm_bn),
'dcm_bo': (['time', 'dcm_mat_dim1', 'dcm_mat_dim2'], dcm_bo),
'angular_vel': (['time', 'cord'], omegas),
'controls': (['time', 'cord'], controls),
'gg_torque': (['time', 'cord'], gravityd),
'aero_torque': (['time', 'cord'], aerod),
'solar_torque': (['time', 'cord'], solard),
'magnetic_torque': (['time', 'cord'], magneticd),
'hyst_rod_torque': (['time', 'hyst_rod', 'cord'], hyst_rod),
'hyst_rod_magnetization': (['time', 'hyst_rod'], b_rods),
'hyst_rod_external_field': (['time', 'hyst_rod'], h_rods),
'nadir': (['time', 'cord'], nadir),
'solar_power': ('time', solar_power)},
coords={'time': np.arange(0, le, 1), 'cord': ['x', 'y', 'z'], 'hyst_rod': [f'rod{i}' for i in range(len(cubesat.hyst_rods))]},
attrs={'simulation_parameters': str(sim_params_dict), 'cubesat_parameters': str(cubesat.asdict()),
'description': 'University of kentucky attitude propagator software '
'(they call it SNAP) recreation'})
# Note: the simulation and cubesat parameter dictionaries are saved as strings for the nc file. If you wish
# you could just eval(a.cubesat_parameters) to get the dictionary back.
if save:
a.to_netcdf(os.path.join(os.path.dirname(__file__), f'../../{file_name}.nc'))
dcm_to_stk_simple(time[::save_every], dcm_bn, os.path.join(os.path.dirname(__file__), f'../../{file_name}.a'))
if ret:
return a
def get_mrp_br(dcm_rn, sigma):
dcm_br = tr.mrp_to_dcm(sigma) @ dcm_rn.T
return tr.dcm_to_mrp(dcm_br)
def get_prvs(data):
angle = np.zeros(len(time))
e = np.zeros((len(time), 3))
for i in range(len(time)):
angle[i], e[i] = tr.dcm_to_prv(tr.mrp_to_dcm(data[i]))
return angle, e
density = saved_data.atmos.values
lons = saved_data.lons.values
lats = saved_data.lats.values
alts = saved_data.alts.values
positions = saved_data.positions.values
velocities = saved_data.velocities.values
# initialize attitude so that z direction of body frame is aligned with nadir
dcm0 = ut.initial_align_gravity_stabilization(positions[0], velocities[0])
sigma0 = tr.dcm_to_mrp(dcm0)
# initialize angular velocity so that it is approximately the speed of rotation around the earth
omega0_body = np.array([0, -0.1, 0.05])
omega0 = dcm0.T @ omega0_body
states[0] = [sigma0, omega0]
dcm_bn[0] = tr.mrp_to_dcm(states[0][0])
dcm_on[0] = ut.inertial_to_orbit_frame(positions[0], velocities[0])
dcm_bo[0] = dcm_bn[0] @ dcm_on[0].T
# get initial b field so that hysteresis rods can be initialized properly
mag_field[0] = geomag.GeoMag(np.array([lats[0], lons[0], alts[0]]), time_tracks[0], output_format='inertial')
mag_field_body[0] = (dcm_bn[0] @ mag_field[0]) * 10 ** -9 # in the body frame in units of T
# cubesat.hyst_rods[0].h_current = mag_field_body[0][0]/cubesat.hyst_rods[0].u0
# the integration
for i in tqdm(range(len(time) - 1)):
# keep track of dcms for various frame
dcm_bn[i] = tr.mrp_to_dcm(states[i][0])
dcm_on[i] = ut.inertial_to_orbit_frame(positions[i], velocities[i])
dcm_bo[i] = dcm_bn[i] @ dcm_on[i].T
# states_br = np.zeros((len(time), 2, 3))
states_bn = np.zeros((len(time), 2, 3))
states_bn[0] = [sigma0, omega0]
controls = np.zeros((len(time), 3))
# states_br[0] = [sigma0, omega0]
dcm_br = np.zeros((len(time), 3, 3))
# dcm_br[0] = tr.mrp_to_dcm(states_br[0][0])
dcm_bn = np.zeros((len(time), 3, 3))
for i in range(len(time) - 1):
# do attitude determination (this needs to change now that the state is defined in the reference frame)
# sigma_estimated = ae.mrp_triad_with_noise(states_br[i][0], sun_vec, mag_vec, 0.01, 0.01)
# sigma_estimated = ae.mrp_triad_with_noise(states_br[i][0], sun_vec, mag_vec, 0.0, 0.0)
# sigma_estimated = states_br[i][0]
dcm_bn[i] = tr.mrp_to_dcm(states_bn[i][0])
dcm_rn[i] = rf.get_dcm_rn(time[i])
dcm_br[i] = dcm_bn[i] @ dcm_rn[i].T
sigma_br = tr.dcm_to_mrp(dcm_br[i])
# get reference frame velocities
omega_r = dcm_br[i] @ rf.get_omega_r(time[i]) # convert from reference frame to body frame
omega_dot_r = dcm_br[i] @ rf.get_omega_r_dot(time[i]) # convert from reference frame to body frame
# next step: numerically approximate these from reference frame position wrt time?
# get control torques
controls[i] = cl.control_torque_ref_frame(sigma_br, states_bn[i][1], omega_r, omega_dot_r, inertia, K, P, i,
controls[i - 1], max_torque=max_torque)
# propagate attitude state
states_bn[i + 1] = it.rk4(st.state_dot_ref_frame, time_step, states_bn[i], controls[i], omega_r, inertia,
def test_mrp_reverse():
dcm1 = ut.random_dcm()
b = tr.dcm_to_mrp(dcm1)
dcm2 = tr.mrp_to_dcm(b)
np.testing.assert_almost_equal(dcm1, dcm2)
def torque(self, time: float, state: np.ndarray, attitude: AttitudeData, orbit: OrbitData, cubesat: CubeSat):
# interpolate all the pre-calculated data
attitude.interp_orbit_data(orbit, time, save=self.save_torques)
if self.save_torques:
c = attitude.save
self.save_torques = False
else:
c = attitude.temp
c.controls = np.zeros(3)
c.dcm_bn = mrp_to_dcm(state[0])
c.dcm_on = inertial_to_orbit_frame(c.positions, c.velocities)
c.dcm_bo = c.dcm_bn @ c.dcm_on.T
R0 = np.linalg.norm(c.positions)
c.nadir = -c.positions / R0
c.mag_field_body = (c.dcm_bn @ c.mag_field) * 1e-9 # body frame, units T
# the request for propagation should come at the beginning of each step
if self.propagate_hysteresis:
h = c.mag_field_body / self._u0
for rod in cubesat.hyst_rods:
h_proj = h @ rod.axes_alignment
# the request to save should come in every X steps
if self.save_hysteresis:
rod.propagate_and_save_magnetization(h_proj)
else:
rod.propagate_magnetization(h_proj)
self.save_hysteresis = False
self.propagate_hysteresis = False
if self._include_torques['gravity']:
ue = c.dcm_bn @ c.nadir
c.gravityd = self.gravity_gradient(ue, R0, cubesat)
c.controls += c.gravityd
if self._include_torques['aerodynamic']:
vel_body = c.dcm_bn @ self.get_air_velocity(c.velocities, c.positions)
c.aerod = self.aerodynamic_torque(vel_body, c.density, cubesat)
c.controls += c.aerod
if self._include_torques['solar']:
sun_vec_norm = c.sun_vec / np.linalg.norm(c.sun_vec)
c.sun_vec_body = c.dcm_bn @ sun_vec_norm
theta = np.arcsin(6.378e6 / (6.378e6 + c.alts))
angle_btw = np.arccos(c.nadir @ sun_vec_norm)
if angle_btw < theta:
c.is_eclipse = 1
else:
c.solard = self.solar_pressure(c.sun_vec_body, c.sun_vec, c.positions, cubesat)
c.controls += c.solard
if self._include_torques['magnetic']:
c.magneticd = self.total_magnetic(c.mag_field_body, cubesat)
c.controls += c.magneticd
if self._include_torques['hysteresis']:
c.hyst_rod = self.hysteresis_rod_torque_peek(c.mag_field_body, cubesat)
c.controls += c.hyst_rod
return c.controls
