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[