def continue_sim(sim_dataset, num_iter, file_name):
import copy
original_params = eval(sim_dataset.simulation_parameters)
sim_params = copy.deepcopy(original_params)
sim_params['end_time_index'] = num_iter
sim_params['start_time'] = sim_params['final_time']
last_data = sim_dataset.isel(time=-1)
sim_params[
'omega0_body'] = last_data.dcm_bn.values @ last_data.angular_vel.values
sim_params['sigma0'] = tr.dcm_to_mrp(last_data.dcm_bn.values)
cubesat_params = eval(sim_dataset.cubesat_parameters)
new_data = sim_attitude(sim_params,
cubesat_params,
'easter_egg',
save=False,
ret=True)
new_data['time'] = np.arange(
len(sim_dataset.time),
len(sim_dataset.time) +
sim_params['end_time_index'] * sim_params['save_every'], 1)
a = xr.concat([sim_dataset, new_data], dim='time')
true_sim_params = eval(a.simulation_parameters)
true_sim_params[
'end_time_index'] = original_params['end_time_index'] + num_iter
true_sim_params['final_time'] = eval(
new_data.simulation_parameters)['final_time']
a.attrs['simulation_parameters'] = str(true_sim_params)
a.to_netcdf(
os.path.join(os.path.dirname(__file__), f'../../{file_name}.nc'))
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)
def get_mrp_br(dcm_rn, sigma):
dcm_br = tr.mrp_to_dcm(sigma) @ dcm_rn.T
return tr.dcm_to_mrp(dcm_br)
save_data = xr.open_dataset('orbit_pre_process.nc')
time_tracks = [datetime(2019, 3, 24, 18, 35, 1, tzinfo=utc) + timedelta(seconds=time_step*i) for i in range(len(time))]
saved_data = save_data.interp(time=time_tracks)
sun_vec = saved_data.sun.values
mag_field = saved_data.mag.values
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)):
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,
inertia_inv)
# do 'tidy' up things at the end of integration (needed for many types of attitude coordinates)
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)
