from astropy.coordinates import SkyCoord, ITRS, EarthLocation, AltAz from astropy import units as u import numpy as np from datetime import datetime, timedelta import time from numpy.core._multiarray_umath import ndarray from scipy.spatial import distance from scipy.linalg import det print("Running Test Cases...") # !------------ Test 1 - GCRS to ITRS from envs.transformations import gcrs2irts_matrix_a, get_eops, gcrs2irts_matrix_b eop = get_eops() xyz1: ndarray = np.array([1285410, -4797210, 3994830], dtype=np.float64) t = datetime(year=2007, month=4, day=5, hour=12, minute=0, second=0) object = SkyCoord(x=xyz1[0] * u.m, y=xyz1[1] * u.m, z=xyz1[2] * u.m, frame='gcrs', representation_type='cartesian', obstime=t) # just for astropy itrs = object.transform_to(ITRS) test1a_error = distance.euclidean(itrs.cartesian.xyz._to_value(u.m), gcrs2irts_matrix_a(t, eop) @ xyz1) test1b_error = distance.euclidean(itrs.cartesian.xyz._to_value(u.m), gcrs2irts_matrix_b(t, eop) @ xyz1)
def __init__(self, config=env_config): # super(SSA_Tasker_Env, self).__init__() s = time.time() self.runtime = {'__init__': 0, 'reset': 0, 'step': 0, 'step prep': 0, 'propagate next true state': 0, 'perform predictions': 0, 'update with observation': 0, 'Observations and Reward': 0, 'filter_error': 0, 'visible_objects': 0, 'object_visibility': 0, 'anees': 0, 'failed_filters': 0, 'plot_sigma_delta': 0, 'plot_rewards': 0, 'plot_anees': 0, 'plot_actions': 0, 'all_true_obs': 0, 'plot_visibility': 0, 'predict method': 0} """Simulation configuration""" self.t_0 = config['t_0'] # time at start of simulation self.dt = config['time_step'] # length of time steps [s] self.n = config['steps'] # max run steps self.m = config['rso_count'] # number of Resident Space Object (RSO) to include in the simulation self.obs_limit = np.radians(config['obs_limit']) # don't observe objects below this elevation [rad] self.obs_returned = config['obs_returned'] self.reward_type = config['reward_type'] # configuration parameters for RSOs; sma: Semi-major axis [m], ecc: Eccentricity [u], inc: Inclination (rad), # raan: Right ascension of the ascending node (rad), argp: Argument of perigee (rad), nu: True anomaly (rad) self.orbits = config['orbits'] # orbits to sample from self.obs_lla = np.array(config['observer']) * [deg2rad, deg2rad, 1] # Observer coordinates - lat, lon, height (deg, deg, m) self.obs_itrs = lla2ecef(self.obs_lla) # Observer coordinates in ITRS (m) self.update_interval = config['update_interval'] # how often an observation should be taken self.i = 0 """Filter configuration""" # standard deviation of noise added to observations [rad, rad, m] self.obs_type = config['obs_type'] if self.obs_type == 'aer': self.z_sigma = config['z_sigma'] * np.array([arcsec2rad, arcsec2rad, 1]) elif self.obs_type == 'xyz': self.z_sigma = config['z_sigma'] else: print('Invalid Observation Type: ' + str(config['obs_type'])) exit() # standard deviation of noise added to initial state estimates; [m, m, m, m/s, m/s, m/s] self.x_sigma = np.array(config['x_sigma']) self.Q = Q_noise_fn(dim=2, dt=self.dt, var=config['q_sigma'] ** 2, block_size=3, order_by_dim=False) self.eops = get_eops() self.fx = config['fx'] self.hx = config['hx'] self.mean_z = config['mean_z'] self.residual_z = config['residual_z'] self.msqrt = config['msqrt'] self.alpha, self.beta, self.kappa = config['alpha'], config['beta'], config[ 'kappa'] # sigma point configuration parameters """Prep arrays""" # variables for the filter x_dim = int(6) z_dim = int(3) if config['P_0'] is None: self.P_0 = np.copy(np.diag(self.x_sigma ** 2)) # Assumed covariance of the estimates at simulation start else: self.P_0 = np.copy(config['P_0']) if config['R'] is None: self.R = np.diag(self.z_sigma ** 2) # Noise added to the filter during observation updates else: self.R = np.copy(config['R']) self.x_true = np.empty(shape=(self.n, self.m, x_dim)) # true means for all objects at each time step self.x_filter = np.empty(shape=(self.n, self.m, x_dim)) # filter means for all objects at each time step self.P_filter = np.empty(shape=(self.n, self.m, x_dim, x_dim)) # covariances for all objects at each time step self.obs = np.empty(shape=(self.n, self.m, x_dim * 2)) # observations for all objects at each time step self.time = [self.t_0 + (timedelta(seconds=self.dt) * i) for i in range(self.n)] # time for all time steps self.trans_matrix = gcrs2irts_matrix_b(self.time, self.eops) # used for celestial to terrestrial self.z_noise = np.empty(shape=(self.n, self.m, z_dim)) # array to contain the noise added to each observation self.z_true = np.empty(shape=(self.n, self.m, z_dim)) # array to contain the true observations which are made self.y = np.empty(shape=(self.n, self.m, z_dim)) # array to contain the innovation of each observation self.S = np.empty(shape=(self.n, self.m, z_dim, z_dim)) # array to contain the innovation covariance self.x_noise = np.empty(shape=(self.m, x_dim)) # array to contain the noise added to each RSO self.filters = [] # creates a list for ukfs """variables for environment performance""" self.delta_pos = np.empty((self.n, self.m)) # euclidean distance between true and filter mean position elements self.delta_vel = np.empty((self.n, self.m)) # euclidean distance between true and filter mean velocity elements self.sigma_pos = np.empty((self.n, self.m)) # euclidean magnitude of diagonal position elements of covariance self.sigma_vel = np.empty((self.n, self.m)) # euclidean magnitude of diagonal velocity elements of covariance self.scores = np.empty((self.n, self.m)) # score of each RSO at each time step self.rewards = np.empty(self.n) # reward for each time step self.failed_filters_id = [] # prep list for failed states self.failed_filters_msg = ["None"] * self.m # prep list for failure messages self.actions = np.empty(self.n, dtype=int) # prep variable for keeping track of all previous actions self.obs_taken = np.empty(self.n, dtype=bool) # prep variable for keeping track of which obs were actually taken self.x_failed = np.array([1e20, 1e20, 1e20, 1e12, 1e12, 1e12]) # failed filter will be set to this value self.P_failed = np.diag([1e20, 1e20, 1e20, 1e12, 1e12, 1e12]) # failed filter will be set to this value self.nees = np.empty((self.n, self.m)) # used for normalized estimation error squared (NEES) and its average self.visibility = [] # used to store a log of visible objects at each time step self.sigmas_h = np.empty((self.n, x_dim * 2 + 1, z_dim)) # used to store sigmas points used in updates """Define Gym spaces""" self.action_space = gym.spaces.Discrete(self.m) # the action is choosing which RSO to look at if self.obs_returned == 'flatten': self.observation_space = gym.spaces.Box(low=np.tile(-np.inf, (self.m * 12)), # flat 1d array for MLP high=np.tile(np.inf, (self.m * 12)), dtype=np.float64) # obs is x [6] and diag(P) [6] for each RSO [m] elif self.obs_returned == 'aer': self.observation_space = gym.spaces.Box(low=np.tile(-np.inf, (self.m * 4)), # flat 1d array for MLP high=np.tile(np.inf, (self.m * 4)), dtype=np.float64) # obs is aer [3] and trace(P) [1] for each RSO [m] self.observation = np.zeros(self.m * 4) else: self.observation_space = gym.spaces.Box(low=np.tile(-np.inf, (self.m, 12)), # 2d array for readability high=np.tile(np.inf, (self.m, 12)), dtype=np.float64) # obs is x [6] and diag(P) [6] for each RSO [m] """Initial reset and seed calls""" self.np_random = None self.init_seed = self.seed() self.reset() e = time.time() self.runtime['__init__'] += e - s
samples = 20000 step_size = 60 * 2.5 # seconds max_gap = 1.5 # hours duration = 4 # hours obs_limit = np.radians(15) first_window = 45 # minutes n = int(np.ceil(duration * 60 * 60 / step_size)) observer = (38.828198, -77.305352, 20.0) # lat, lon, height (deg, deg, m) obs_lla = np.array(observer) * [deg2rad, deg2rad, 1 ] # lat, lon, height (deg, deg, m) obs_itrs = lla2ecef(obs_lla) # lat, lon, height (deg, deg, m) to ITRS (m) RE = Earth.R_mean.to_value(u.m) t_0 = datetime(2020, 5, 4, 0, 0, 0) t_samples = [t_0 + timedelta(seconds=step_size) * i for i in range(n)] eops = get_eops() trans_matrix = gcrs2irts_matrix_a(t=t_samples, eop=eops) hx_kwargs = [{ "trans_matrix": trans_matrix[i], "observer_itrs": obs_itrs, "observer_lla": obs_lla } for i in range(n)] candidates = [] seed = 0 random_state = np.random.RandomState(seed) for i in tqdm(range(samples)): regime = random_state.choice(a=['LEO', 'MEO', 'GEO', 'Tundra', 'Molniya'], p=[1 / 3, 1 / 3, 1 / 9, 1 / 9, 1 / 9]) accepted = False