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