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