def test_cross():
"""Tests all overloads of the cross product function.
"""
u = lin.Vector3([1.0, 0.0, 0.0])
v = lin.Vector3([0.0, 1.0, 0.0])
w = lin.cross(u, v)
assert type(w) is lin.Vector3
assert (w[0] == 0.0) and (w[1] == 0.0) and (w[2] == 1.0)
v = lin.RowVector3([0.0, 1.0, 0.0])
w = lin.cross(u, v)
assert type(w) is lin.Vector3
assert (w[0] == 0.0) and (w[1] == 0.0) and (w[2] == 1.0)
u = lin.RowVector3([1.0, 0.0, 0.0])
w = lin.cross(u, v)
assert type(w) is lin.RowVector3
assert (w[0] == 0.0) and (w[1] == 0.0) and (w[2] == 1.0)
v = lin.Vector3([0.0, 1.0, 0.0])
w = lin.cross(u, v)
assert type(w) is lin.RowVector3
assert (w[0] == 0.0) and (w[1] == 0.0) and (w[2] == 1.0)
def propagate_orbits(self, orbit, propagation_time=10 * 60 * 1000000000):
# get default sim configs
configs = ["sensors/base", "truth/base", "truth/detumble"]
configs = [
"lib/common/psim/config/parameters/" + f + ".txt" for f in configs
]
# should we use the default truth.dt.ns, or should it depend on something like prop_time?
config = Configuration(configs)
# update values to current (sim assumes leader, works equally for follower)
config["truth.leader.orbit.r"] = lin.Vector3(orbit.pos)
config["truth.leader.orbit.v"] = lin.Vector3(orbit.vel)
config["truth.t.ns"] = GPSTime(*(orbit.time)).to_pan_ns()
# step sim to desired time
sim = Simulation(SingleOrbitGnc, config)
while sim["truth.t.ns"] < config["truth.t.ns"] + propagation_time:
sim.step()
# return the sim propagated orbit
propagatedOrbit = OrbitData(
list(sim["truth.leader.orbit.r"]),
list(sim["truth.leader.orbit.v"]),
GPSTime(sim["truth.t.ns"]).to_list(),
)
return propagatedOrbit
def __actuators(self, fc, satellite):
"""Reads actuator outputs from a flight computer into the simulation for
the specified satellite name.
Currently, we're only looking at the magnetorquer and wheel commands.
"""
self.__sim[f"truth.{satellite}.wheels.t"] = lin.Vector3(
fc.smart_read("adcs_cmd.rwa_torque_cmd"))
self.__sim[f"truth.{satellite}.magnetorquers.m"] = lin.Vector3(
fc.smart_read("adcs_cmd.mtr_cmd"))
def run(self):
print("Running Monte Carlo")
# Setup
self.leader = self.radio_leader
self.follower = self.radio_follower
# Read the orbit data from each satellite from database
downlinked_data_vals_leader = self.readDownlinkData(self.leader)
downlinked_data_vals_follower = self.readDownlinkData(self.follower)
pos_sigma, vel_sigma = self.get_sigmas()
mc_start_time = time.time()
monte_carlo_position_runs = self.generate_monte_carlo_data(downlinked_data_vals_leader,
downlinked_data_vals_follower,
pos_sigma,
vel_sigma)
mc_finish_time = time.time()
orbits_no_noise = self.run_psim(downlinked_data_vals_leader,
downlinked_data_vals_follower,
follower_r_noise=lin.Vector3(),
follower_v_noise=lin.Vector3())
times = np.array([x.time for x in orbits_no_noise]) # list of GPSTime objects as lists
ecef_positions = [x.pos for x in orbits_no_noise]
eci_positions = self.batch_convert_to_eci(ecef_positions, times)
# monte_carlo_position_runs_eci = [self.batch_convert_to_eci(mc_pos_run, times) for mc_pos_run in monte_carlo_position_runs]
print(f'Spent {mc_finish_time - mc_start_time} seconds generating Monte Carlo data.')
# Post-process lists, calculate covariances to output ephemeris
start_covariance_time = time.time()
pan_times = [MonteCarlo.time_since_pan_epoch(t) for t in times]
monte_carlo_position_runs = np.array(monte_carlo_position_runs)
print(monte_carlo_position_runs.shape)
mc_runs_eci = ecef2eci_mc_runs(pan_times, monte_carlo_position_runs, GPSTime.EPOCH_WN) / 1000 # div by 1000 to convert to km
covariance_per_step = get_covariances(mc_runs_eci)
end_covariance_time = time.time()
print(f'Spent {end_covariance_time - start_covariance_time} seconds calculating covariance, and converting to ECI.')
km_positions = [[meter / 1000 for meter in position] for position in eci_positions] # convert to km
self.write_file(times, km_positions, covariance_per_step)
self.logger.put("EXITING MONTECARLO SIMS")
self.finish()
def propagate(t, r, v, steps):
"""Propagate and orbital state forward in time by the requested
number of steps.
"""
config["truth.t.ns"] = GPSTime(*t).to_pan_ns()
config["truth.leader.orbit.r"] = lin.Vector3(r)
config["truth.leader.orbit.v"] = lin.Vector3(v)
sim = Simulation(SingleOrbitGnc, config)
for _ in range(steps):
sim.step()
return GPSTime(sim["truth.t.ns"]).to_list(), \
list(sim["truth.leader.orbit.r"]), \
list(sim["truth.leader.orbit.v"]),
def test_constructor():
"""Tests constructs for the generic lin types. We ensure lin objects are
zero initialized and can be generated from arrays of one and two dimensions.
"""
m = lin.Matrix3x3()
for i in range(len(m)):
assert m[i] == 0.0
v = lin.Vector3()
for i in range(len(v)):
assert v[i] == 0.0
m = lin.Matrix3x3([
[0.0, 1.0, 2.0],
[3.0, 4.0, 5.0],
[6.0, 7.0, 8.0]
])
for i in range(len(m)):
assert m[i] == float(i)
m = lin.Matrix2x2([
0.0, 1.0,
2.0, 3.0
])
for i in range(len(m)):
assert m[i] == float(i)
def test_square():
"""Tests the square function.
"""
u = lin.Vector3([1.0, -3.0, 2.0])
v = lin.square(u)
assert type(v) is lin.Vector3
assert (v[0] == 1.0) and (v[1] == 9.0) and (v[2] == 4.0)
def test_sign():
"""Tests the sign function.
"""
u = lin.Vector3([-1.0, 0.0, 5.0])
v = lin.sign(u)
assert type(v) is lin.Vector3
assert (v[0] == -1.0) and (v[1] == 0.0) and (v[2] == 1.0)
def test_isfinite():
"""Tests the isfinite function.
"""
u = lin.Vector3([0.0, 0.0, float('nan')])
v = lin.Vector2()
assert not lin.isfinite(u)
assert lin.isfinite(v)
def test_dot():
"""Tests all overloads of the dot product function.
"""
u = lin.RowVector3([0.0, 2.0, 1.0])
v = lin.Vector3([1.0, -0.5, 2.0])
assert lin.dot(u, u) == 5.0
assert lin.dot(u, v) == 1.0
assert lin.dot(v, u) == 1.0
assert lin.dot(v, v) == 5.25
def test_multiply():
"""Tests all the overloads of the multiplication operator and multiply
function.
"""
a = lin.Matrix3x3([
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0
])
b = lin.Vector3([1.0, 2.0, 3.0])
c = a * b
assert type(c) is lin.Vector3
assert (c[0] == 1.0) and (c[1] == 2.0) and (c[2] == 3.0)
b = lin.Matrix3x2([
1.0, 2.0,
3.0, 4.0,
5.0, 6.0
])
c = a * b
assert type(c) is lin.Matrix3x2
for i in range(len(c)):
assert c[i] == b[i]
c = a * a
assert type(c) is lin.Matrix3x3
for i in range(len(c)):
assert c[i] == a[i]
a = lin.Vector2([1.0, 3.0])
b = lin.RowVector2([1.0, 2.0])
c = a * b
assert type(c) is lin.Matrix2x2
assert (c[0, 0] == 1.0) and (c[0, 1] == 2.0) and (c[1, 0] == 3.0) and \
(c[1, 1] == 6.0)
a *= 2.0
assert (a[0] == 2.0) and (a[1] == 6.0)
c = a * 0.5
assert type(c) is lin.Vector2
assert (c[0] == 1.0) and (c[1] == 3.0)
c = 2.0 * b
assert type(c) is lin.RowVector2
assert (c[0] == 2.0) and (c[1] == 4.0)
a = lin.RowVector2([2.0, 2.0])
c = lin.multiply(a, b)
assert type(c) is lin.RowVector2
assert (c[0] == 2.0) and (c[1] == 4.0)
def generate_monte_carlo_data(self, downlinked_leader, downlinked_follower, pos_sigma, vel_sigma):
"""Generate monte carlo data for the given downlink data
args:
downlinked_leader: OrbitData for the leader, at the start of the sim
downlinked_follower: OrbitData for the follower, at the start of the sim
pos_sigma: the sigma for the position noise
vel_sigma: the sigma for the velocity noise
returns:
a list of simulation runs, each as a list of OrbitData objects, describing the follower over the simulation"""
monte_carlo_position_runs = [] #list of positions for each trial
for i in range(1, NUM_MC_RUNS + 1):
list_of_orbit_data = self.run_psim(downlinked_leader,
downlinked_follower,
follower_r_noise=lin.Vector3(np.random.normal(scale=pos_sigma)),
follower_v_noise=lin.Vector3(np.random.normal(scale=vel_sigma)),
)
list_of_orbit_positions = [x.pos for x in list_of_orbit_data]
monte_carlo_position_runs.append(list_of_orbit_positions)
print(f"Finished {i} simulations")
return monte_carlo_position_runs
def to_lin_vector(var):
import lin
if type(var) == list:
_len = len(var)
if _len == 2:
var = lin.Vector2(var)
elif _len == 3:
var = lin.Vector3(var)
elif _len == 4:
var = lin.Vector4(var)
else:
raise RuntimeError(
"Unexpected List Length, can't change into lin Vector")
else:
raise RuntimeError("Expected list, can't change into lin Vector")
return var
def run_psim(self,
leader_orbit,
follower_orbit,
follower_r_noise = lin.Vector3([0,0,0]),
follower_v_noise = lin.Vector3([0,0,0]),
runtime=RUNTIME
) -> List[OrbitData]:
"""Run a simulation with the given parameters
args:
leader_orbit: OrbitData for the leader, at the start of the sim
follower_orbit: Orbitdata for the follower, at the start of the sim
follower_r_noise: noise to add to the follower position
follower_v_noise: noise to add to the follower velocity
runtime: the length of the simulation in nanoseconds
returns:
a list of OrbitData objects, describing the follower position over the simulation
times are GPSTime objects
"""
configs = ["sensors/base", "truth/base", "truth/vox_standby", "fc/base"]
configs = ["lib/common/psim/config/parameters/" + f + ".txt" for f in configs]
config = Configuration(configs)
# set psim random seed to random number
config["seed"] = np.random.randint(0, 2**32 - 1)
# update values to current
config["truth.leader.orbit.r"] = lin.Vector3(leader_orbit.pos)
config["truth.leader.orbit.v"] = lin.Vector3(leader_orbit.vel)
config["truth.follower.orbit.r"] = lin.Vector3(follower_orbit.pos) + follower_r_noise
config["truth.follower.orbit.v"] = lin.Vector3(follower_orbit.vel) + follower_v_noise
config["truth.t.ns"] = GPSTime(*(follower_orbit.time)).to_pan_ns() # takes in GPS epoch time -> pan epoch time in nano seconds
propagated_follower_orbits = []
# step sim to desired time
sim = Simulation(OrbitControllerTest, config)
steps = 0
while sim["truth.t.ns"] < config["truth.t.ns"] + runtime:
sim.step()
if steps % STEPS_PER_LOG_ENTRY == 0:
propagated_follower_orbits.append(OrbitData(
list(sim["truth.follower.orbit.r"]),
list(sim["truth.follower.orbit.v"]),
GPSTime(sim["truth.t.ns"]).to_list(),
))
steps += 1
return propagated_follower_orbits