import numpy as np from scipy.integrate import solve_ivp from scipy.spatial.transform import Rotation as R import matplotlib.pyplot as plt # -------------------------------------------------------------------------- # # # # FUNCTIONS # # # -------------------------------------------------------------------------- # def Mean_to_true_anomaly(M, e): """ Calculate True anomaly from mean anomaly Args: M: mean anomaly (rad). e: Eccentricity. Returns: f: True anomaly (rad). """ M0 = M E = M0 # initial guess for the Eccentric anomaly for _ in range(10): # solve keplers equation E_new = M0 + e * np.sin(E) if np.abs(E_new - E) < 1e-8: break E = E_new f = 2 * np.arctan2(np.sqrt(1 - e) * np.tan(E / 2), np.sqrt(1 + e)) # true anomaly return f # -------------------------------------------------------------------------- # def cartesian_to_orbitgeometry(r_p, v_p): """ Calculate orbit geometry from cartesian Cartesian coordinates (r_p, v_p) Args: r_p (numpy.ndarray): Position vector (rx_p, ry_p, rz_p) in the perifocal frame (km). v_p (numpy.ndarray): Velocity vector (vx_p, vy_p, vz_p) in the perifocal frame (km/s). Returns: tuple: Classical orbital elements (a, e). a: Semi-major axis (km) e: Eccentricity """ # mu (float): Gravitational parameter. mu=398600.4415 # Ensure r_p and v_p are numpy arrays r_p = np.array(r_p) v_p = np.array(v_p) # Calculate magnitude of position and velocity r = np.linalg.norm(r_p) v = np.linalg.norm(v_p) # Calculate specific energy energy = (v**2) / 2 - mu / r # Calculate semi-major axis if energy < 0: a = -mu / (2 * energy) else: a = np.inf # For parabolic or hyperbolic orbits # Calculate eccentricity vector h = np.cross(r_p, v_p) # angular momentum e_vec = (1 / mu) * (np.cross(v_p, h) - mu * (r_p / r)) e = np.linalg.norm(e_vec) return a, e # -------------------------------------------------------------------------- # def classical_to_cartesian(a, M, e): """ Converts classical orbital elements to Cartesian coordinates (r, v) in the perifocal frame. Args: a (float): Semi-major axis (km) M (float): Mean anomaly (rad) e (float): Eccentricity Returns: tuple: Position (rx_p, ry_p, rz_p) and velocity (vx_p, vy_p, vz_p) in the perifocal frame. """ # mu (float): Gravitational parameter. mu=398600.4415 # Calculate true anomaly f = Mean_to_true_anomaly(M, e) # Calculate semi-latus rectum p and radius r p = a * (1 - e**2) r = p / (1 + e * np.cos(f)) # Calculate position in perifocal frame rx_p = r * np.cos(f) ry_p = r * np.sin(f) rz_p = 0.0 # Calculate velocity in perifocal frame h = np.sqrt(a * mu * (1 - e**2)) vx_p = -mu / h * np.sin(f) vy_p = mu / h * (e + np.cos(f)) vz_p = 0.0 return (rx_p, ry_p, rz_p), (vx_p, vy_p, vz_p) # -------------------------------------------------------------------------- # def rtn_to_perifocal(r_RTN, v_RTN, f): """ Transforms a state vector from the RTN frame to the perifocal frame. Args: r_RTN (np.ndarray): Position vector in RTN frame [rx, ry, rz] (km). v_RTN (np.ndarray): Velocity vector in RTN frame [vx, vy, vz] (km/s). f (float): True anomaly of the chief spacecraft (rad). Returns: tuple: Position and velocity vectors in the perifocal frame (km and km/s). """ # Rotation matrix about the z-axis (N-axis) by -f. Rz_minus_f = np.array([[np.cos(f), -np.sin(f), 0], [np.sin(f), np.cos(f), 0], [0, 0, 1]]) r_perifocal = np.dot(Rz_minus_f, r_RTN) v_perifocal = np.dot(Rz_minus_f, v_RTN) return r_perifocal, v_perifocal # -------------------------------------------------------------------------- # def perifocal_to_rtn(r_perifocal, v_perifocal, f): """ Transforms a state vector from the perifocal frame to the RTN frame. Args: r_perifocal (np.ndarray): Position vector in perifocal frame [rx, ry, rz] (km). v_perifocal (np.ndarray): Velocity vector in perifocal frame [vx, vy, vz] (km/s). f (float): True anomaly of the chief spacecraft (rad). Returns: tuple: Position and velocity vectors in the RTN frame (km and km/s). """ # Rotation matrix about the z-axis (N-axis) by +f. Rz_plus_f = np.array([[np.cos(f), np.sin(f), 0], [-np.sin(f), np.cos(f), 0], [0, 0, 1]]) r_RTN = np.dot(Rz_plus_f, r_perifocal) v_RTN = np.dot(Rz_plus_f, v_perifocal) return r_RTN, v_RTN # -------------------------------------------------------------------------- # def perifocal_to_eci(r_perifocal, v_perifocal, omega, inc, raan): """ Transforms position and velocity vectors from the perifocal frame to the ECI frame. Args: r_perifocal (numpy.ndarray): Position vector in the perifocal frame (km). v_perifocal (numpy.ndarray): Velocity vector in the perifocal frame (km/s). omega (float): Argument of perigee (rad). inc (float): Inclination (rad). raan (float): Right Ascension of the Ascending Node (rad). Returns: tuple: Two numpy arrays representing the position (r_eci) and velocity (v_eci) vectors in the ECI frame. """ # Rotation matrices R_z_raan = np.array([ [np.cos(raan), -np.sin(raan), 0], [np.sin(raan), np.cos(raan), 0], [0, 0, 1] ]) R_x_inc = np.array([ [1, 0, 0], [0, np.cos(inc), -np.sin(inc)], [0, np.sin(inc), np.cos(inc)] ]) R_z_omega = np.array([ [np.cos(omega), -np.sin(omega), 0], [np.sin(omega), np.cos(omega), 0], [0, 0, 1] ]) # Transformation matrix from perifocal to ECI Q_perifocal_to_eci = np.dot(R_z_raan, np.dot(R_x_inc, R_z_omega)) # Calculate ECI position and velocity r_eci = np.dot(Q_perifocal_to_eci, r_perifocal) v_eci = np.dot(Q_perifocal_to_eci, v_perifocal) return r_eci, v_eci # -------------------------------------------------------------------------- # def eci_to_perifocal(r_eci, v_eci, omega, inc, raan): """ Transforms position and velocity vectors from the ECI frame to the perifocal frame. Args: r_eci (numpy.ndarray): Position vector in the ECI frame (km). v_eci (numpy.ndarray): Velocity vector in the ECI frame (km/s). omega (float): Argument of perigee (rad). inc (float): Inclination (rad). raan (float): Right Ascension of the Ascending Node (rad). Returns: tuple: Two numpy arrays representing the position (r_perifocal) and velocity (v_perifocal) vectors in the perifocal frame. """ # Rotation matrices (transpose of the ECI to perifocal rotation matrices) R_z_omega_T = np.array([ [np.cos(omega), np.sin(omega), 0], [-np.sin(omega), np.cos(omega), 0], [0, 0, 1] ]) R_x_inc_T = np.array([ [1, 0, 0], [0, np.cos(inc), np.sin(inc)], [0, -np.sin(inc), np.cos(inc)] ]) R_z_raan_T = np.array([ [np.cos(raan), np.sin(raan), 0], [-np.sin(raan), np.cos(raan), 0], [0, 0, 1] ]) # Transformation matrix from ECI to perifocal (transpose of Q_perifocal_to_eci) Q_eci_to_perifocal = np.dot(R_z_omega_T, np.dot(R_x_inc_T, R_z_raan_T)) # Calculate perifocal position and velocity r_perifocal = np.dot(Q_eci_to_perifocal, r_eci) v_perifocal = np.dot(Q_eci_to_perifocal, v_eci) return r_perifocal, v_perifocal # -------------------------------------------------------------------------- # def propagate_cw(delta_r0, n, dt, num_steps): """ Propagates the relative state vector using the Clohessy-Wiltshire equations. Args: delta_r0: Initial relative state vector [x0, y0, z0, vx0, vy0, vz0] in RTN of Chief. n (float): Mean motion of the chief spacecraft (rad/s). dt (float): Time step for propagation (s). num_steps (int): Number of time steps to propagate. Returns: tuple: Arrays of propagated relative state vectors and corresponding times. """ propagated_states = [delta_r0] times = [0.0] deltaM = [0.0] current_state = np.array(delta_r0) t = 0.0 for _ in range(num_steps): t += dt nt = n * t phi = np.array([ [4 - 3*np.cos(nt), 0, 0, np.sin(nt)/n, 2*(1 - np.cos(nt))/n, 0], [6*(np.sin(nt) - nt), 1, 0, 2*(np.cos(nt) - 1)/n, (4*np.sin(nt) - 3*nt)/n, 0], [0, 0, np.cos(nt), 0, 0, np.sin(nt)/n], [3*n*np.sin(nt), 0, 0, np.cos(nt), 2*np.sin(nt), 0], [6*n*(np.cos(nt) - 1), 0, 0, -2*np.sin(nt), 4*np.cos(nt) - 3, 0], [0, 0, -n*np.sin(nt), 0, 0, np.cos(nt)] ]) current_state = np.dot(phi, delta_r0) propagated_states.append(current_state) times.append(t) return np.array(propagated_states), np.array(times) # -------------------------------------------------------------------------- # def propagate_orbit_truth(r0, v0, t_span, method='DOP853', J2=1.08263e-3, dense_output=True, t_eval=None): """ Propagates the orbit using scipy.integrate.solve_ivp with J2 perturbation, ensuring consistency with the ECI frame. Args: r0 (np.ndarray): Initial position vector [x0, y0, z0] in ECI (km). v0 (np.ndarray): Initial velocity vector [vx0, vy0, vz0] in ECI (km/s). t_span (tuple): Time span for the propagation (t0, tf) (s). method (str, optional): Numerical integration method to use. Defaults to 'DOP853'. J2 (float, optional): J2 perturbation coefficient. Defaults to 1.08263e-3. dense_output (bool, optional): Whether to use dense output. Defaults to True. t_eval (array_like, optional): Times at which to store the computed solution. Returns: scipy.integrate._ivp.OdeResult: The result of the integration. """ # mu (float): Gravitational parameter (km^3/s^2) mu = 398600.447 # R_Earth (float): Earth Radius (km) R_earth = 6378.137 y0 = np.concatenate((r0, v0)) def equations_of_motion_with_j2(t, y): """ Equations of motion with J2 perturbation in ECI frame. Args: t (float): Time (s). y (np.ndarray): State vector [x, y, z, vx, vy, vz] (km, km/s). Returns: np.ndarray: Derivative of the state vector [vx, vy, vz, ax, ay, az] (km/s, km/s^2). """ r = y[:3] v = y[3:] r_mag = np.linalg.norm(r) # Calculate the gravitational acceleration a_grav = -mu * r / r_mag**3 # Calculate J2 perturbation acceleration z = r[2] a_j2 = (3 * J2 * mu * R_earth**2 / (2 * r_mag**5)) * \ np.array([ (5 * z**2 / r_mag**2 - 1) * r[0], (5 * z**2 / r_mag**2 - 1) * r[1], (5 * z**2 / r_mag**2 - 3) * z ]) a = a_grav + a_j2 # Total acceleration return np.concatenate((v, a)) solution = solve_ivp(equations_of_motion_with_j2, t_span, y0, method=method, dense_output=dense_output, rtol=1e-13, atol=1e-16, # Increased accuracy t_eval=t_eval ) return solution # -------------------------------------------------------------------------- # def propagate_Keplerian_Motion(alpha0, dt, num_steps): """ Propagates the relative state vector using Keplerian motion. Args: alpha0: Initial Keplerian elements [a, M, e, w, i, ra] (semi-major axis, Mean anomaly, eccentricity, argument of periapsis, inclination, right ascension of the ascending node). dt (float): Time step for propagation (s). num_steps (int): Number of time steps to propagate. Returns: tuple: Arrays of propagated Keplerian elements and corresponding times. """ # mu (float): Gravitational parameter (km^3/s^2) mu = 398600.447 propagated_SingularStates = [np.array(alpha0)] # Initialize with a copy propagated_position = [] propagated_velocity = [] times = [0.0] current_state = np.array(alpha0) t = 0.0 a = alpha0[0] e = alpha0[2] M0 = alpha0[1] w = alpha0[3] inc = alpha0[4] ra = alpha0[5] n = np.sqrt(mu / a**3) r_per0, v_per0 = classical_to_cartesian(a, M0, e) r_eci0, v_eci0 = perifocal_to_eci(r_per0, v_per0, w, inc, ra) propagated_position.append(r_eci0) propagated_velocity.append(v_eci0) for _ in range(num_steps): t += dt M_new = n * t + M0 current_state[1] = M_new propagated_SingularStates.append(np.copy(current_state)) times.append(t) r_per, v_per = classical_to_cartesian(a, M_new, e) r_eci, v_eci = perifocal_to_eci(r_per, v_per, w, inc, ra) propagated_position.append(r_eci) propagated_velocity.append(v_eci) return np.array(propagated_SingularStates), np.array(propagated_position), np.array(propagated_velocity), np.array(times) # -------------------------------------------------------------------------- # # # # LECTURE EXERCISE # # # -------------------------------------------------------------------------- # def propagate_ROE(dalpha0, n, alpha_c, dt, num_steps): """ Propagates the relative state vector using the ROE model. Args: dalpha0: Initial relative state vector [da, dM, de, dw, di, dra]. n (float): Mean motion of the chief spacecraft (rad/s). alpha_c: Chief state vector [a, M, e, w, i, ra] propagation (dim. num_steps x 6) - already propagated / known. dt (float): Time step for propagation (s). num_steps (int): Number of time steps to propagate. Returns: tuple: Arrays of propagated relative state vectors and corresponding times. """ # setup propagated_ROEstates = np.tile(dalpha0, (num_steps+1, 1)) times = [0.0] deltaM = [0.0] current_ROEstate = np.array(dalpha0) propagated_da = [dalpha0[0]] # Watch out! Not all of them are needed! propagated_dM = [dalpha0[1]] propagated_de = [dalpha0[2]] propagated_dw = [dalpha0[3]] propagated_di = [dalpha0[4]] propagated_dra = [dalpha0[5]] t = 0.0 # Relative SMA and mean anomaly at the istant t = t0 [WATCH OUT! NOT all of them are needed!] da0 = dalpha0[0] dM0 = dalpha0[1] de0 = dalpha0[2] dw0 = dalpha0[3] dinc0 = dalpha0[4] dra0 = dalpha0[5] ########################################################################### # --- EXERCISE --- # """ Dear reader, your objective is to complete the ROE propagation based on what you have learnt during the lecture. Please follow the following tasks to guarantee a successful completion: (1) identify which ROEs [da, dM, de, dw, di, dra] vary when ONLY the Keplerian motion is considered. Let's call it/them generically "dx" (2) For each identified dx, write the evolution of the absolute motion dot(x)=f(alpha) as a function of time. (3) Then, since dx = x_d - x_c, dot(dx) = dot(x_d) - dot(x_c), where d and c identify Deputy and Chief. (4) Expand at the N-order "x_d = x_c + dot(x)*dx + dot2(x)*dx^2/2 + ..." as a function of the ROEs. The N-value is your choice: we advice to start with N=1. (5) Finally you may approximate dx_new = dx0 + dot(dx)*t, where dx0 is an input of this function NB: Do not forget that "da = (a_d - a_c) / a_c" is normalized by "/a_c" and it is not just "a_d - a_c"! Solution: You may find it at the end of this code. However, before looking at it, try yourself! Enjoy, The Coder """ # ROE propagation (assuming ONLY Keplerian motion) for _ in range(num_steps): t += dt # Add something here da_new = da0 dM_new = dM0 de_new = de0 dw_new = dw0 dinc_new = dinc0 dra_new = dra0 # ------------------ # Save propagated_da.append(da_new) propagated_dM.append(dM_new) propagated_de.append(de_new) propagated_dw.append(dw_new) propagated_di.append(dinc_new) propagated_dra.append(dra_new) times.append(t) # ----- END ----- # ########################################################################### # --- Do not modify --- # # Deputy state calculation propagated_ROEstates[:,0] = propagated_da propagated_ROEstates[:,1] = propagated_dM propagated_ROEstates[:,2] = propagated_de propagated_ROEstates[:,3] = propagated_dw propagated_ROEstates[:,4] = propagated_di propagated_ROEstates[:,5] = propagated_dra alpha_d = alpha_c + propagated_ROEstates alpha_d[:,0] = alpha_c[:,0] * (1 + propagated_ROEstates[:,0]) # From absolute motion of Chief and Deputy to Relative one in ECI relative_states_eci = [] for ii in range(num_steps+1): alpha_c_vec = alpha_c[ii, :] a_c = alpha_c_vec[0] M_c = alpha_c_vec[1] e_c = alpha_c_vec[2] w_c = alpha_c_vec[3] inc_c = alpha_c_vec[4] ra_c = alpha_c_vec[5] r_c_per, v_c_per = classical_to_cartesian(a_c, M_c, e_c) r_c_eci, v_c_eci = perifocal_to_eci(r_c_per, v_c_per, w_c, inc_c, ra_c) alpha_d_vec = alpha_d[ii, :] a_d = alpha_d_vec[0] M_d = alpha_d_vec[1] e_d = alpha_d_vec[2] w_d = alpha_d_vec[3] inc_d = alpha_d_vec[4] ra_d = alpha_d_vec[5] r_d_per, v_d_per = classical_to_cartesian(a_d, M_d, e_d) r_d_eci, v_d_eci = perifocal_to_eci(r_d_per, v_d_per, w_d, inc_d, ra_d) r_c_eci = np.array(r_c_eci) v_c_eci = np.array(v_c_eci) r_d_eci = np.array(r_d_eci) v_d_eci = np.array(v_d_eci) delta_r_eci = r_d_eci - r_c_eci delta_v_eci = v_d_eci - v_c_eci current_relative_state_eci = np.concatenate((delta_r_eci, delta_v_eci)) relative_states_eci.append(current_relative_state_eci) return propagated_ROEstates, np.array(relative_states_eci), np.array(times) # -------------------------------------------------------------------------- # # # # RELATIVE ORBITAL PROPAGATION # # # -------------------------------------------------------------------------- # # --- Code Setup --- # mu = 398600.447 # Gravitational parameter (km^3/s^2) step = 100 # number of points per orbit N_orbit = 1 # number of propagated orbits # --- Define Initial Conditions --- # # Chief Classical singular Equinoctial Elements a_c_singular = 7000 # SMA (km) M_c_singular = 0.0 # Mean anomaly (rad) e_c_singular = 0.0 # Eccentricity w_c_singular = 0.0 # Argument of perigee (rad) inc_c_singular = 1.0 # Inclination (rad) ra_c_singular = 0.0 # RAAN (rad) f_c_singular = Mean_to_true_anomaly(M_c_singular, e_c_singular) alpha0_c = np.array([a_c_singular, M_c_singular, e_c_singular, w_c_singular, inc_c_singular, ra_c_singular]) print(" \n Initial Chief Equinoctial State:", alpha0_c) # Relative Orbital Element (Singular form) da_singular = 1 / a_c_singular dM_singular = 0.0 de_singular = 0.0 dw_singular = 0.0 dinc_singular = 0.01 dra_singular = 0.0 dalpha0 = np.array([da_singular, dM_singular, de_singular, dw_singular, dinc_singular, dra_singular]) print(" \n Initial ROE State:", dalpha0) """ You can play around changing a little bit the orbital elements and relative one. However, do not forget that those are models valid only for CLOSE orbits, for large initial distances they will not match the truth! """ # Deputy Classical singular Equinoctial Elements a_d_singular = a_c_singular * (1 + da_singular) M_d_singular = M_c_singular + dM_singular e_d_singular = e_c_singular + de_singular w_d_singular = w_c_singular + dw_singular inc_d_singular = inc_c_singular + dinc_singular ra_d_singular = ra_c_singular + dra_singular alpha0_d = np.array([a_d_singular, M_d_singular, e_d_singular, w_d_singular, inc_d_singular, ra_d_singular]) print(" \n Initial Deputy Equinoctial State:", alpha0_d) # Convert initial equinoctial elements to Cartesian coordinates (perifocal frame) r_c_p0, v_c_p0 = classical_to_cartesian(a_c_singular, M_c_singular, e_c_singular) r_c_eci0, v_c_eci0 = perifocal_to_eci(r_c_p0, v_c_p0, w_c_singular, inc_c_singular, ra_c_singular) # in ECI r_d_pd0, v_d_pd0 = classical_to_cartesian(a_d_singular, M_d_singular, e_d_singular) # in the Deputy perifocal frame r_d_eci0, v_d_eci0 = perifocal_to_eci(r_d_pd0, v_d_pd0, w_d_singular, inc_d_singular, ra_d_singular) # in ECI # Calculate initial relative Cartesian state in the chief's perifocal frame delta_r0_p = np.array(r_d_eci0) - np.array(r_c_eci0) delta_v0_p = np.array(v_d_eci0) - np.array(v_c_eci0) delta_r0_p, delta_v0_p = eci_to_perifocal(delta_r0_p, delta_v0_p, w_c_singular, inc_c_singular, ra_c_singular) # Calculate initial relative Cartesian state in the chief's RTN frame delta_r0_rtn, delta_v0_rtn = perifocal_to_rtn(delta_r0_p, delta_v0_p, f_c_singular) # in the Chief RTN frame # -------------------------------------------------------------------------- # # --- Propagation using CW Equations --- # # Propagation setup num_steps = N_orbit*step delta_r0_cw = np.concatenate((delta_r0_rtn, delta_v0_rtn)) # Define mean motion of the chief period_chief = 2 * np.pi * np.sqrt(a_c_singular**3 / mu) n_chief = 2 * np.pi / period_chief # Define the time step dt = period_chief / step # Propagate the relative state using CW relative_states_cw, time_points_cw = propagate_cw(delta_r0_cw, n_chief, dt, num_steps) # Mapping to ECI relative_states_cw_eci = [] deltar_cw_rtn_matrix = relative_states_cw[:, 0:3] deltav_cw_rtn_matrix = relative_states_cw[:, 3:6] for ii in range(num_steps+1): MM = M_c_singular + n_chief*time_points_cw[ii] ff = Mean_to_true_anomaly(MM, e_c_singular) deltar_cw_rtn_vec = deltar_cw_rtn_matrix[ii, :] deltav_cw_rtn_vec = deltav_cw_rtn_matrix[ii, :] deltar_cw_per, deltav_cw_per = rtn_to_perifocal(deltar_cw_rtn_vec, deltav_cw_rtn_vec, ff) deltar_cw_eci, deltav_cw_eci = perifocal_to_eci(deltar_cw_per, deltav_cw_per, w_c_singular, inc_c_singular, ra_c_singular) relative_states_cw_eci.append(np.concatenate((deltar_cw_eci, deltav_cw_eci))) relative_states_cw_eci = np.array(relative_states_cw_eci) # -------------------------------------------------------------------------- # # --- Propagation using Truth Model --- # # Time span for propagation t_span = (0, period_chief * N_orbit) # Propagate for two orbits num_steps_truth = N_orbit*step+1 # Use the same t_eval for both propagations to ensure the same time points t_eval = np.linspace(t_span[0], t_span[1], num_steps_truth) # Propagate orbits for chief and deputy solution_chief = propagate_orbit_truth(r_c_eci0, v_c_eci0, t_span, t_eval=t_eval) solution_deputy = propagate_orbit_truth(r_d_eci0, v_d_eci0, t_span, t_eval=t_eval) # Calculate relative state deltar_truth_eci = solution_deputy.y[:3, :] - solution_chief.y[:3, :] deltav_truth_eci = solution_deputy.y[3:, :] - solution_chief.y[3:, :] t_truth = solution_chief.t # -------------------------------------------------------------------------- # # --- Propagation using Kepler Model --- # # Chief alpha_evo_c, r_kep_c, v_kep_c, time_points_kep = propagate_Keplerian_Motion(alpha0_c, dt, num_steps) # Deputy alpha_evo_d, r_kep_d, v_kep_d, time_points_kep = propagate_Keplerian_Motion(alpha0_d, dt, num_steps) # Relative Motion deltar_kep_eci = r_kep_d - r_kep_c deltav_kep_eci = v_kep_d - v_kep_c # -------------------------------------------------------------------------- # # --- Propagation using ROE Model --- # propagated_ROEstates, relative_states_roe_eci, time_points_roe = propagate_ROE(dalpha0, n_chief, alpha_evo_c, dt, num_steps) alpha_d = alpha_evo_c + propagated_ROEstates # -------------------------------------------------------------------------- # # # # PLOTTING # # # -------------------------------------------------------------------------- # # --- Plotting Propagation --- # plt.figure(figsize=(12, 8)) # CW Results plt.subplot(4, 1, 1) plt.plot(time_points_cw, relative_states_cw_eci[:, 0], label='CW - x') plt.plot(time_points_cw, relative_states_cw_eci[:, 1], label='CW - y') plt.plot(time_points_cw, relative_states_cw_eci[:, 2], label='CW - z') plt.title('Relative Position - CW') plt.xlabel('Time (s)') plt.ylabel('Position (km)') plt.legend() plt.grid(True) # ROE Model Results plt.subplot(4, 1, 2) plt.plot(time_points_roe, relative_states_roe_eci[:, 0], label='ROE - x') plt.plot(time_points_roe, relative_states_roe_eci[:, 1], label='ROE - y') plt.plot(time_points_roe, relative_states_roe_eci[:, 2], label='ROE - z') plt.title('Relative Position - ROE Model') plt.xlabel('Time (s)') plt.ylabel('Position (km)') plt.legend() plt.grid(True) # Keplerian Motion Results plt.subplot(4, 1, 3) plt.plot(time_points_kep, deltar_kep_eci[:, 0], label='Keplerian - x') plt.plot(time_points_kep, deltar_kep_eci[:, 1], label='Keplerian - y') plt.plot(time_points_kep, deltar_kep_eci[:, 2], label='Keplerian - z') plt.title('Relative Position - Truth (Keplerian Motion)') plt.xlabel('Time (s)') plt.ylabel('Position (km)') plt.legend() plt.grid(True) # Truth Model (w J2) Results plt.subplot(4, 1, 4) plt.plot(t_truth, deltar_truth_eci[0, :], label='Truth - x') plt.plot(t_truth, deltar_truth_eci[1, :], label='Truth - y') plt.plot(t_truth, deltar_truth_eci[2, :], label='Truth - z') plt.title('Relative Position - Truth (considering also J2)') plt.xlabel('Time (s)') plt.ylabel('Position (km)') plt.legend() plt.grid(True) plt.tight_layout() plt.show() # -------------------------------------------------------------------------- # # --- Plotting Error --- # plt.figure(figsize=(12, 8)) with np.errstate(divide='ignore', invalid='ignore'): # CW Relative Position Error in ECI plt.subplot(2, 1, 1) err_x_cw = (relative_states_cw_eci[:, 0] - deltar_kep_eci[:, 0]) / deltar_kep_eci[:, 0] * 100 err_y_cw = (relative_states_cw_eci[:, 1] - deltar_kep_eci[:, 1]) / deltar_kep_eci[:, 1] * 100 err_z_cw = (relative_states_cw_eci[:, 2] - deltar_kep_eci[:, 2]) / deltar_kep_eci[:, 2] * 100 plt.plot(time_points_cw, np.where(np.abs(deltar_kep_eci[:, 0]) > 1e-9, err_x_cw, np.nan), label='CW - err_x') plt.plot(time_points_cw, np.where(np.abs(deltar_kep_eci[:, 1]) > 1e-9, err_y_cw, np.nan), label='CW - err_y') plt.plot(time_points_cw, np.where(np.abs(deltar_kep_eci[:, 2]) > 1e-9, err_z_cw, np.nan), label='CW - err_z') plt.title('Relative Position Error in ECI - CW') plt.xlabel('Time (s)') plt.ylabel('Relative Error (%)') plt.legend() plt.grid(True) # ROE Model Relative Position Error in ECI plt.subplot(2, 1, 2) err_x_roe = (relative_states_roe_eci[:, 0] - deltar_kep_eci[:, 0]) / deltar_kep_eci[:, 0] * 100 err_y_roe = (relative_states_roe_eci[:, 1] - deltar_kep_eci[:, 1]) / deltar_kep_eci[:, 1] * 100 err_z_roe = (relative_states_roe_eci[:, 2] - deltar_kep_eci[:, 2]) / deltar_kep_eci[:, 2] * 100 plt.plot(time_points_roe, np.where(np.abs(deltar_kep_eci[:, 0]) > 1e-9, err_x_roe, np.nan), label='ROE - err_x') plt.plot(time_points_roe, np.where(np.abs(deltar_kep_eci[:, 1]) > 1e-9, err_y_roe, np.nan), label='ROE - err_y') plt.plot(time_points_roe, np.where(np.abs(deltar_kep_eci[:, 2]) > 1e-9, err_z_roe, np.nan), label='ROE - err_z') plt.title('Relative Position Error in ECI - ROE Model') plt.xlabel('Time (s)') plt.ylabel('Relative Error (%)') plt.legend() plt.grid(True) plt.tight_layout() plt.show() # -------------------------------------------------------------------------- # # --- Plotting Normalized Cumulative Relative Error --- # plt.figure(figsize=(12, 8)) with np.errstate(divide='ignore', invalid='ignore'): # Calculate the relative error, multiply by dt, and cumulative sum for CW relerr_cw_x = np.abs((relative_states_cw_eci[:, 0] - deltar_kep_eci[:, 0]) / (deltar_kep_eci[:, 0] + 1e-12)) relerr_cw_y = np.abs((relative_states_cw_eci[:, 1] - deltar_kep_eci[:, 1]) / (deltar_kep_eci[:, 1] + 1e-12)) relerr_cw_z = np.abs((relative_states_cw_eci[:, 2] - deltar_kep_eci[:, 2]) / (deltar_kep_eci[:, 2] + 1e-12)) cumulative_relerr_cw_x_dt = np.cumsum(relerr_cw_x * dt) cumulative_relerr_cw_y_dt = np.cumsum(relerr_cw_y * dt) cumulative_relerr_cw_z_dt = np.cumsum(relerr_cw_z * dt) normalized_cumulative_relerr_cw_x = cumulative_relerr_cw_x_dt / period_chief * 100 normalized_cumulative_relerr_cw_y = cumulative_relerr_cw_y_dt / period_chief * 100 normalized_cumulative_relerr_cw_z = cumulative_relerr_cw_z_dt / period_chief * 100 # Calculate the relative error, multiply by dt, and cumulative sum for ROE relerr_roe_x = np.abs((relative_states_roe_eci[:, 0] - deltar_kep_eci[:, 0]) / (deltar_kep_eci[:, 0] + 1e-12)) relerr_roe_y = np.abs((relative_states_roe_eci[:, 1] - deltar_kep_eci[:, 1]) / (deltar_kep_eci[:, 1] + 1e-12)) relerr_roe_z = np.abs((relative_states_roe_eci[:, 2] - deltar_kep_eci[:, 2]) / (deltar_kep_eci[:, 2] + 1e-12)) cumulative_relerr_roe_x_dt = np.cumsum(relerr_roe_x * dt) cumulative_relerr_roe_y_dt = np.cumsum(relerr_roe_y * dt) cumulative_relerr_roe_z_dt = np.cumsum(relerr_roe_z * dt) normalized_cumulative_relerr_roe_x = cumulative_relerr_roe_x_dt / period_chief * 100 normalized_cumulative_relerr_roe_y = cumulative_relerr_roe_y_dt / period_chief * 100 normalized_cumulative_relerr_roe_z = cumulative_relerr_roe_z_dt / period_chief * 100 # Plot the normalized cumulative relative error for the CW model plt.subplot(2, 1, 1) plt.plot(time_points_cw, normalized_cumulative_relerr_cw_x, label='CW - Sum |Rel Err| x * dt / T') plt.plot(time_points_cw, normalized_cumulative_relerr_cw_y, label='CW - Sum |Rel Err| y * dt / T') plt.plot(time_points_cw, normalized_cumulative_relerr_cw_z, label='CW - Sum |Rel Err| z * dt / T') plt.title('Normalized Cumulative Relative Position Error in ECI - CW') plt.xlabel('Time (s)') plt.ylabel('Normalized Cumulative Relative Error (%)') plt.legend() plt.grid(True) # Plot the normalized cumulative relative error for the ROE Model plt.subplot(2, 1, 2) plt.plot(time_points_roe, normalized_cumulative_relerr_roe_x, label='ROE - Sum |Rel Err| x * dt / T') plt.plot(time_points_roe, normalized_cumulative_relerr_roe_y, label='ROE - Sum |Rel Err| y * dt / T') plt.plot(time_points_roe, normalized_cumulative_relerr_roe_z, label='ROE - Sum |Rel Err| z * dt / T') plt.title('Normalized Cumulative Relative Position Error in ECI - ROE Model') plt.xlabel('Time (s)') plt.ylabel('Normalized Cumulative Relative Error (%)') plt.legend() plt.grid(True) plt.tight_layout() plt.show() # -------------------------------------------------------------------------- # # # # SOLUTION OF THE EXERCISE # # # -------------------------------------------------------------------------- # # # THE SOLUTION TO THE EXERCISE IS: # # # ROE propagation (assuming ONLY Keplerian motion) # for _ in range(num_steps): # t += dt # # da_new = da0 # # First order # dM_new = dM0 - 1.5*n*da0*t # # Second order # dM_new = dM0 - 1.5*n*da0*t + 15/8*n*da0**2*t # # Third order # dM_new = dM0 - 1.5*n*da0*t + 15/8*n*da0**2*t - 105/48*n*da0**3*t # # And so on... # de_new = de0 # dw_new = dw0 # dinc_new = dinc0 # dra_new = dra0 # # # Save # propagated_da.append(da_new) # propagated_dM.append(dM_new) # propagated_de.append(de_new) # propagated_dw.append(dw_new) # propagated_di.append(dinc_new) # propagated_dra.append(dra_new) # # times.append(t) # # Because only dM varies when ONLY Keplerian Motion is considered. # Let's derive together dot(dM): # # (1) dM = M_d - M_c # (2) dot(dM) = dot(M_d) - dot(M_c) = n_d - n_c # (3) dot(M_d) = n_d = sqrt(mu/a_d^3) # (4) dot(M_d) can be approximated with: n_d ~ n_c * ( 1 + dot(n)_c * (a_d - a_c) ) # where dot(n) = -3/2 sqrt(mu/a^5) since the only variable is a, and "_c" indicates that you need to calculate it using Chief alpha # (5) finally knowing that da = (a_d - a_c)/a_c we obtain dot(dM) ~ -3/2 n_c * da # # Then we can approximate at the first order dM(t) = dM0 + dot(dM) * t = dM0 - 1.5 * n * da0 * t # # To add more orther one shall just compute the N derivatives of dot(M) that completes the expansion. #