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