def teardown_method(self,function):
print(f'Running teardown for {self.__class__.__name__}::{function.__name__}')
# Reset databases
create_session = create_sensor_tables_from_path(sql_path)
session = create_session()
session.query(OpNavTrajectoryStateModel).delete()
assert len(session.query(OpNavTrajectoryStateModel).all()) == 0
session.query(OpNavAttitudeStateModel).delete()
assert len(session.query(OpNavAttitudeStateModel).all()) == 0
session.query(OpNavEphemerisModel).delete()
assert len(session.query(OpNavEphemerisModel).all()) == 0
session.query(OpNavCameraMeasurementModel).delete()
assert len(session.query(OpNavCameraMeasurementModel).all()) == 0
session.query(OpNavPropulsionModel).delete()
assert len(session.query(OpNavPropulsionModel).all()) == 0
session.query(OpNavGyroMeasurementModel).delete()
assert len(session.query(OpNavGyroMeasurementModel).all()) == 0
session.commit()
def sixhours_db_test(mocker, state_error, part_start, part_end, timestep):
"""
Calls ukf using the opnav pipeline, which uses databases
[timestep]: interval in minutes
Data:
- noise of magnitude [state_error] in starting estimate
- no noise in measurements
- no added noise in proceeding state estimates
- init state was taken seconds prior to current measurements
- ephemerides and measurements taken at same timesteps
"""
create_session = create_sensor_tables_from_path(sql_path)
session = create_session()
time = datetime.now()
d_camMeas, d_moonEph, d_sunEph, d_traj, totalIntegrationTime = get6HoursBatch(part_start, part_end, part_start,
timestep,
np.array([1, 1, 1, 1, 1, 1]),
np.array([1, 1, 1]),
np.array([1, 1, 1, 1]), 1, 1, 1,
att_meas=False)
assert len(d_camMeas['z1']) == len(d_traj['x'])
P = np.diag(np.array([100, 100, 100, 1e-5, 1e-6, 1e-5], dtype=float)) # Initial Covariance Estimate of State
R = np.diag(np.array(state_error, dtype=float))
# init state
state = (
np.array([d_traj['x'][0], d_traj['y'][0], d_traj['z'][0], d_traj['vx'][0], d_traj['vy'][0], d_traj['vz'][0]],
dtype=float)).reshape(6, 1)
last_estimate_dt_seconds = 200
state[0:3] = state[0:3] - last_estimate_dt_seconds * state[3:6]
error = np.random.multivariate_normal(np.zeros((6,)), R).reshape(6, 1)
state = state + error
state = state.flatten()
entries = [
OpNavTrajectoryStateModel.from_tuples(
position=(state[0], state[1], state[2]),
velocity=(state[3], state[4], state[5]),
P=TrajUKFConstants.P0.reshape(6,6),
time=time+timedelta(0,-last_estimate_dt_seconds)), # initial state used by propulsion step, output used by propagation step
OpNavAttitudeStateModel.from_tuples(
quat=(0., 0., 0., 0.),
rod_params=(0., 0., 0.),
biases=(0.,0.,0.,),
P=np.zeros((6,6)),
time=time), # dummy attitude states
]
measurements = []
timestamps = []
for t in tqdm(range( len(d_traj['x']) ), desc ='Preparing test dbs'):
new_time = time+timedelta(0,timestep*60*t)
timestamps.append(new_time)
measurements.append(
OpNavCameraMeasurementModel.from_tuples(
measurement=(d_camMeas['z1'][t], d_camMeas['z2'][t], d_camMeas['z3'][t], d_camMeas['z4'][t], d_camMeas['z5'][t], d_camMeas['z6'][t]),
time=new_time), # camera measurement taken now, uses state
)
entries.extend([
OpNavEphemerisModel.from_tuples(
sun_eph=(d_sunEph['x'][t], d_sunEph['y'][t], d_sunEph['z'][t], d_sunEph['vx'][t], d_sunEph['vy'][t], d_sunEph['vz'][t]),
moon_eph=(d_moonEph['x'][t], d_moonEph['y'][t], d_moonEph['z'][t], d_moonEph['vx'][t], d_moonEph['vy'][t], d_moonEph['vz'][t]),
time=new_time),
])
# save ephemerides
session.bulk_save_objects(entries)
session.commit()
progress_bar = tqdm(total = len(measurements), desc='Progress')
# setup mocks
def observe_mock(session, gyro_count):
curr_meas = measurements[0]
curr_time = curr_meas.time_retrieved
session.bulk_save_objects([
curr_meas,
OpNavGyroMeasurementModel.from_tuples(
measurement=(0., 0., 0.),
time=curr_time), # dummy gyro measurement 1
OpNavGyroMeasurementModel.from_tuples(
measurement=(0., 0., 0.),
time=curr_time+timedelta(0, 1)), # dummy gyro measurement 2
])
session.commit()
measurements.pop(0)
progress_bar.update(1)
return OPNAV_EXIT_STATUS.SUCCESS
mocker.patch('core.opnav.__observe', side_effect=observe_mock)
def process_propulsion_events_mock(*args, **kwargs):
return OPNAV_EXIT_STATUS.SUCCESS
mocker.patch('core.opnav.__process_propulsion_events', side_effect=process_propulsion_events_mock)
def run_attitude_ukf_mock(*args, **kwargs):
return AttitudeEstimateOutput(new_state=AttitudeStateVector(0,0,0,0,0,0),
new_P=CovarianceMatrix(matrix=np.zeros((6,6))),
new_quat=QuaternionVector(0, 0, 0, 0))
mocker.patch('core.attitude.runAttitudeUKF', side_effect=run_attitude_ukf_mock)
# def run_trajectory_ukf_mock(*args, **kwargs):
# print("----------TrajUKF Mock----------")
# return TrajectoryEstimateOutput(new_state=TrajectoryStateVector(0,0,0,0,0,0), new_P=CovarianceMatrix(matrix=np.zeros((6,6))), K=Matrix6x6(matrix=np.zeros((6,6))))
# mocker.patch('core.ukf.runTrajUKF', side_effect=run_trajectory_ukf_mock)
temp_db_len = len(session.query(OpNavTrajectoryStateModel).all())
assert temp_db_len == 1, f'Expected 1 trajectory state in db, got {temp_db_len}'
# start opnav system
status = opnav.start(sql_path=sql_path, num_runs=len(measurements), gyro_count=2, camera_params=MatlabTestCameraParameters)
# verification
entries = session.query(OpNavTrajectoryStateModel).all()
# print(entries)
expected_new_entries_len = len(d_traj['x'])
assert len(
entries) == expected_new_entries_len + 1, f'Expected {expected_new_entries_len + 1} entries in trajectory db, got {len(entries)}'
# start() should succeed
assert status == OPNAV_EXIT_STATUS.SUCCESS, f'Expected start() to succeed, got status {status}'
# position and velocity errors should be within bounds
# i = len(d_traj['x']) - 1
# entry = entries[-1]
# for i, timestamp in tqdm(enumerate(timestamps), desc=""):
i = len(timestamps) - 1
entry = session.query(OpNavTrajectoryStateModel).filter(OpNavTrajectoryStateModel.time_retrieved == timestamps[
-1]).first() # search for state estimate made at time of measurement
true_state = np.array(
[d_traj['x'][i], d_traj['y'][i], d_traj['z'][i], d_traj['vx'][i], d_traj['vy'][i], d_traj['vz'][i]],
dtype=float).flatten()
est_state = np.array(
[entry.position_x, entry.position_y, entry.position_z, entry.velocity_x, entry.velocity_y, entry.velocity_z],
dtype=float).flatten()
posError = calculate_position_error(true_state, est_state)
velError = calculate_velocity_error(true_state, est_state)
print(f'true_state: {true_state}')
print(f'est_state: {est_state}')
print(f'Position error: {posError}\nVelocity error: {velError}')
assert posError <= POS_ERROR_6HOURS, f'Position error is too large. Expected {POS_ERROR_6HOURS} Actual {posError}'
assert velError <= VEL_ERROR_6HOURS, f'Velocity error is too large. Expected {VEL_ERROR_6HOURS} Actual {velError}'
progress_bar.close()
