def cislunar1_timestep(visual_analysis, state_error, part_start, part_end, timestep, kickTime=None):
"""
[part_start, part_end): start (inclusive) and end (exclusive) indices of trajectory
"""
from tests.const import TEST_CISLUNAR1_meas, TEST_CISLUNAR1_moonEph, TEST_CISLUNAR1_sunEph, TEST_CISLUNAR1_traj
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)
# trajTruthdf = pd.read_csv(TEST_6HOURS_traj)
# moonEphdf = pd.read_csv(TEST_6HOURS_moonEph)
# sunEphdf = pd.read_csv(TEST_6HOURS_sunEph)
# measEphdf = pd.read_csv(TEST_6HOURS_meas)
P = np.diag(np.array([100, 100, 100, 1e-5, 1e-6, 1e-5], dtype=np.float)) # Initial Covariance Estimate of 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=np.float)).reshape(6,1)
R = np.diag(np.array(state_error, dtype=np.float))
error = np.random.multivariate_normal(np.zeros((6,)),R).reshape(6,1)
state = state + error
liveTraj = None
if visual_analysis == "True":
liveTraj = LiveTrajectoryPlot()
# print(int(trajTruthdf.shape[0]*1 - 1))
for t in tqdm(range( len(d_traj['x']) ), desc ='Trajectory Completion'):
moonEph = (np.array([d_moonEph['x'][t], d_moonEph['y'][t], d_moonEph['z'][t], d_moonEph['vx'][t], d_moonEph['vy'][t], d_moonEph['vz'][t]], dtype=np.float)).reshape(1,6)
sunEph = (np.array([d_sunEph['x'][t], d_sunEph['y'][t], d_sunEph['z'][t], d_sunEph['vx'][t], d_sunEph['vy'][t], d_sunEph['vz'][t]], dtype=np.float)).reshape(1,6)
meas = (np.array([d_camMeas['z1'][t], d_camMeas['z2'][t], d_camMeas['z3'][t], d_camMeas['z4'][t], d_camMeas['z5'][t], d_camMeas['z6'][t]], dtype=np.float)).reshape(6,1)
orientation = None
if kickTime is not None and t > kickTime:
orientation = [0, 0, 0, 1]
state, P, K = runTrajUKF(moonEph, sunEph, meas, state, 60, P, MatlabTestCameraParameters, dynamicsOnly=False) # used to have this, not sure why it's gone orientation=orientation
# Per iteration error
traj = (np.array([d_traj['x'][t], d_traj['y'][t], d_traj['z'][t], d_traj['vx'][t], d_traj['vy'][t], d_traj['vz'][t]], dtype=np.float)).reshape(6,1)
traj = traj.flatten()
fstate = state.flatten()
posError = math.sqrt( np.sum((traj[:3] - fstate[:3])**2) )
velError = math.sqrt( np.sum((traj[3:6] - fstate[3:6])**2) )
# plot
if liveTraj:
liveTraj.updateEstimatedTraj(fstate[0], fstate[1], fstate[2])
liveTraj.updateTrueTraj(traj[0], traj[1], traj[2])
liveTraj.renderUKF(text="Iteration {} - Pos Error {} - Vel Error {}".format(t, round(posError), round(velError)))
if liveTraj:
liveTraj.close()
t = part_end - 1
traj = (np.array([d_traj['x'][t], d_traj['y'][t], d_traj['z'][t], d_traj['vx'][t], d_traj['vy'][t], d_traj['vz'][t]], dtype=np.float)).reshape(6,1)
traj = traj.flatten()
state = state.flatten()
posError = math.sqrt( np.sum((traj[:3] - state[:3])**2) )
velError = math.sqrt( np.sum((traj[3:6] - state[3:6])**2) )
print(f'Position error: {posError}\nVelocity error: {velError}')
assert posError <= POS_ERROR, 'Position error is too large'
assert velError <= VEL_ERROR, 'Velocity error is too large'
def sixhours_direct_test(visual_analysis, state_error, part_start, part_end, timestep):
"""
Calls ukf directly without database overhead
[part_start, part_end): start (inclusive) and end (exclusive) indices of trajectory
[timestep]: interval in minutes
"""
from tests.const import TEST_6HOURS_meas, TEST_6HOURS_moonEph, TEST_6HOURS_sunEph, TEST_6HOURS_traj
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)
P = np.diag(np.array([100, 100, 100, 1e-5, 1e-6, 1e-5], dtype=float)) # Initial Covariance Estimate of 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]
R = np.diag(np.array(state_error, dtype=float))
error = np.random.multivariate_normal(np.zeros((6,)), R).reshape(6, 1)
state = state + error
liveTraj = None
if visual_analysis == "True":
print("Visual Analysis on")
liveTraj = LiveTrajectoryPlot()
first = true
dt = last_estimate_dt_seconds
# print(int(trajTruthdf.shape[0]*1 - 1))
for t in tqdm(range( len(d_traj['x']) ), desc ='Trajectory Completion'):
# if t == 0: continue
moonEph = (np.array(
[d_moonEph['x'][t], d_moonEph['y'][t], d_moonEph['z'][t], d_moonEph['vx'][t], d_moonEph['vy'][t],
d_moonEph['vz'][t]], dtype=float)).reshape(1, 6)
sunEph = (np.array([d_sunEph['x'][t], d_sunEph['y'][t], d_sunEph['z'][t], d_sunEph['vx'][t], d_sunEph['vy'][t],
d_sunEph['vz'][t]], dtype=float)).reshape(1, 6)
meas = (np.array(
[d_camMeas['z1'][t], d_camMeas['z2'][t], d_camMeas['z3'][t], d_camMeas['z4'][t], d_camMeas['z5'][t],
d_camMeas['z6'][t]], dtype=float)).reshape(6, 1)
traj_out = runTrajUKF(
EphemerisVector(moonEph[0, 0], moonEph[0, 1], moonEph[0, 2], moonEph[0, 3], moonEph[0, 4], moonEph[0, 5]),
EphemerisVector(sunEph[0, 0], sunEph[0, 1], sunEph[0, 2], sunEph[0, 3], sunEph[0, 4], sunEph[0, 5]),
CameraMeasurementVector(meas[0, 0], meas[1, 0], meas[2, 0], meas[3, 0], meas[4, 0], meas[5, 0]),
TrajectoryStateVector.from_numpy_array(state),
dt,
CovarianceMatrix(matrix=P),
MatlabTestCameraParameters,
dynamicsOnly=False) # used to have this, not sure why it's gone orientation=orientation
state = traj_out.new_state.data
P = traj_out.new_P.data
K = traj_out.K
# Per iteration error
traj = (np.array(
[d_traj['x'][t], d_traj['y'][t], d_traj['z'][t], d_traj['vx'][t], d_traj['vy'][t], d_traj['vz'][t]],
dtype=float)).reshape(6, 1)
traj = traj.flatten()
fstate = state.flatten()
posError = math.sqrt( np.sum((traj[:3] - fstate[:3])**2) )
velError = math.sqrt( np.sum((traj[3:6] - fstate[3:6])**2) )
# plot
if liveTraj:
liveTraj.updateEstimatedTraj(fstate[0], fstate[1], fstate[2])
liveTraj.updateTrueTraj(traj[0], traj[1], traj[2])
liveTraj.renderUKF(text="Iteration {} - Pos Error {} - Vel Error {}".format(t, round(posError), round(velError)))
if first:
dt=timestep*60
if liveTraj:
liveTraj.close()
t = int((part_end - part_start)/timestep - 1)
traj = (np.array([d_traj['x'][t],
d_traj['y'][t],
d_traj['z'][t],
d_traj['vx'][t],
d_traj['vy'][t],
d_traj['vz'][t]], dtype=float)).reshape(6, 1)
traj = traj.flatten()
state = state.flatten()
print(f'true_state: {traj}')
print(f'est_state: {state}')
posError = math.sqrt( np.sum((traj[:3] - state[:3])**2) )
velError = math.sqrt( np.sum((traj[3:6] - state[3:6])**2) )
print(f'Position error: {posError}\nVelocity error: {velError}')
assert posError <= POS_ERROR_6HOURS, 'Position error is too large'
assert velError <= VEL_ERROR_6HOURS, 'Velocity error is too large'
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()
def test_attitude_6_hours():
secPerMin = 60
minPerHour = 60
cameraDt = 1
omegaInit = [0., 0.001, 6., 0., 0., 0.]
biasInit = [0., 0., 0.]
quat = np.array([[
np.random.randn(),
np.random.randn(),
np.random.randn(),
np.random.randn()
]]).T
gyroSampleCount = 4
coldGasKickTime = 200
gyro_noise_sigma = 1.e-7
gyro_sigma = 1.e-10
gyro_t = 1.0 / gyroSampleCount
meas_sigma = 8.7e-4
# timeNoiseSigma=2
startTime = 0
# numberOfMeasForTest = 100
endTime = 700
q1, q2, q3, q4, omegax, omegay, omegaz, biasx, biasy, biasz, d_camMeas, d_moonEph, d_sunEph, d_traj, earthVectors, moonVectors, sunVectors, totalIntegrationTime = \
generator.get6HoursBatch(startTime, endTime, coldGasKickTime, cameraDt, omegaInit, biasInit, quat, gyroSampleCount, gyro_sigma, gyro_noise_sigma, att_meas=True)
numberOfMeasForTest = len(d_camMeas['z1'])
P0 = np.array([[1.e-1, 0., 0., 0., 0., 0.], [0., 1.e-1, 0., 0., 0., 0.],
[0., 0., 1.e-1, 0., 0., 0.], [0., 0., 0., 9.7e-10, 0., 0.],
[0., 0., 0., 0., 9.7e-10, 0.],
[0., 0., 0., 0., 0., 9.7e-10]]) * 10.
Q = np.array([[
gyro_noise_sigma**2. -
(1. / 6.) * gyro_sigma**2. * gyro_t**2., 0., 0., 0., 0., 0.
],
[
0., gyro_noise_sigma**2. -
(1. / 6.) * gyro_sigma**2. * gyro_t**2., 0., 0., 0., 0.
],
[
0., 0., gyro_noise_sigma**2. -
(1. / 6.) * gyro_sigma**2. * gyro_t**2., 0., 0., 0.
], [0., 0., 0., gyro_sigma**2., 0., 0.],
[0., 0., 0., 0., gyro_sigma**2., 0.],
[0., 0., 0., 0., 0., gyro_sigma**2.]]) * .5 * gyro_t
R = np.eye(9) * meas_sigma**2.
x0 = np.array([[0., 0., 0., 0., 0., 0.]]).T
gyroVars = GyroVars()
gyroVars.gyro_noise_sigma = gyro_noise_sigma
gyroVars.gyro_sample_rate = gyro_t
gyroVars.gyro_sigma = gyro_sigma
gyroVars.meas_sigma = meas_sigma
# Create dummy position UKF outputs
estimatedSatStates = np.zeros(
(len(d_camMeas['time'] * gyroSampleCount), 3))
moonEph = np.zeros((len(d_camMeas['time'] * gyroSampleCount), 3))
sunEph = np.zeros((len(d_camMeas['time'] * gyroSampleCount), 3))
omegaMeasurements = np.zeros((len(d_camMeas['time'] * gyroSampleCount), 3))
# assuming biases can be measured.
# biasMeasurements = np.zeros((len(d_camMeas['time']*gyroSampleCount), 3))
count = 0
results = []
resultsTimeline = []
# For each camera measurement
for i in tqdm(range(numberOfMeasForTest), desc='Trajectory Estimates'):
# Sat
pos = np.array([d_traj['x'][i], d_traj['y'][i], d_traj['z'][i]
]) + np.random.normal(scale=0, size=3)
vel = np.array([d_traj['vx'][i], d_traj['vy'][i], d_traj['vz'][i]])
start_t = d_traj['time'][i]
times = start_t + gyro_t * np.arange(0, gyroSampleCount)
resultsTimeline.append(times[-1]) # time corresponding to new estimate
tempCount = count
for i_, t in enumerate(times):
pos = pos + t * vel # propogate position estimate to gyro measurement time
estimatedSatStates[tempCount, :] = pos
tempCount += 1
# Moon
pos = np.array([
d_moonEph['x'][i], d_moonEph['y'][i], d_moonEph['z'][i]
]) + np.random.normal(scale=100, size=3)
vel = np.array(
[d_moonEph['vx'][i], d_moonEph['vy'][i], d_moonEph['vz'][i]])
tempCount = count
for i_, t in enumerate(times):
pos = pos + t * vel # propogate position estimate to gyro measurement time
moonEph[tempCount, :] = pos
tempCount += 1
# Sun
pos = np.array([d_sunEph['x'][i], d_sunEph['y'][i], d_sunEph['z'][i]
]) + np.random.normal(scale=100, size=3)
vel = np.array(
[d_sunEph['vx'][i], d_sunEph['vy'][i], d_sunEph['vz'][i]])
tempCount = count
for i_, t in enumerate(times):
pos = pos + t * vel # propogate position estimate to gyro measurement time
sunEph[tempCount, :] = pos
tempCount += 1
# omegas and biases
tempCount = count
omegasMeasurementVectors: List[GyroMeasurementVector] = []
for i_, t in enumerate(times):
omegaMeasurements[tempCount, :] = np.array(
[float(omegax(t)),
float(omegay(t)),
float(omegaz(t))])
# biasMeasurements[tempCount, :] = np.array([float(x0[3]), float(x0[4]), float(x0[5])])
omegasMeasurementVectors.append(
GyroMeasurementVector(omega_x=float(omegax(t)),
omega_y=float(omegay(t)),
omega_z=float(omegaz(t))))
tempCount += 1
count = tempCount
timeline = []
for i, t in enumerate(times):
if i == 0:
timeline.append(0)
else:
timeline.append(t - timeline[-1])
timeline[0] = sum(timeline) / (gyroSampleCount - 1.)
# run ukf
gyroVarsTup = (gyroVars.gyro_sigma, gyroVars.gyro_sample_rate,
gyroVars.get_Q_matrix(), gyroVars.get_R_matrix())
estimate: AttitudeEstimateOutput = runAttitudeUKF(
cameraDt, gyroVarsTup, CovarianceMatrix(matrix=P0),
AttitudeStateVector.from_numpy_array(state=x0),
QuaternionVector.from_numpy_array(quat=quat),
omegasMeasurementVectors, estimatedSatStates, moonEph, sunEph,
timeline)
results.append(estimate)
P0 = estimate.new_P.data
x0 = estimate.new_state.data
quat = estimate.new_quat.data
print(resultsTimeline)
print(len(results), len(resultsTimeline))
generator.plotResults(results, q1, q2, q3, q4, biasx, biasy, biasz,
resultsTimeline)
def test_attitude_6_hours(visual_analysis):
secPerMin = 60
minPerHour = 60
cameraDt = 1
omegaInit = [0., 0.001, 2., 0., 0., 0.]
biasInit = [0., 0., 0.]
quat = np.array([[
np.random.randn(),
np.random.randn(),
np.random.randn(),
np.random.randn()
]]).T
gyroSampleCount = 5
coldGasKickTime = 100
gyro_noise_sigma = 1.e-7
gyro_sigma = 1.e-10
gyro_t = 1.0 / gyroSampleCount
meas_sigma = 8.7e-4
# timeNoiseSigma=2
startTime = 0
# numberOfMeasForTest = 100
endTime = 700
q1, q2, q3, q4, omegax, omegay, omegaz, biasx, biasy, biasz, d_camMeas, d_moonEph, d_sunEph, d_traj, earthVectors, moonVectors, sunVectors, totalIntegrationTime = \
get6HoursBatch(startTime, endTime, coldGasKickTime, cameraDt, omegaInit, biasInit, quat, gyroSampleCount, gyro_sigma, gyro_noise_sigma, att_meas=True)
numberOfMeasForTest = len(d_camMeas['z1'])
P0 = np.array([[1.e-1, 0., 0., 0., 0., 0.], [0., 1.e-1, 0., 0., 0., 0.],
[0., 0., 1.e-1, 0., 0., 0.], [0., 0., 0., 9.7e-10, 0., 0.],
[0., 0., 0., 0., 9.7e-10, 0.],
[0., 0., 0., 0., 0., 9.7e-10]]) * 10.
Q = np.array([[
gyro_noise_sigma**2. -
(1. / 6.) * gyro_sigma**2. * gyro_t**2., 0., 0., 0., 0., 0.
],
[
0., gyro_noise_sigma**2. -
(1. / 6.) * gyro_sigma**2. * gyro_t**2., 0., 0., 0., 0.
],
[
0., 0., gyro_noise_sigma**2. -
(1. / 6.) * gyro_sigma**2. * gyro_t**2., 0., 0., 0.
], [0., 0., 0., gyro_sigma**2., 0., 0.],
[0., 0., 0., 0., gyro_sigma**2., 0.],
[0., 0., 0., 0., 0., gyro_sigma**2.]]) * .5 * gyro_t
R = np.eye(9) * meas_sigma**2.
x0 = np.array([[0., 0., 0., 0., 0., 0.]]).T
# Create dummy position UKF outputs
estimatedSatStates = np.zeros(
(len(d_camMeas['time'] * gyroSampleCount), 3))
moonEph = np.zeros((len(d_camMeas['time'] * gyroSampleCount), 3))
sunEph = np.zeros((len(d_camMeas['time'] * gyroSampleCount), 3))
omegaMeasurements = np.zeros((len(d_camMeas['time'] * gyroSampleCount), 3))
# assuming biases can be measured.
biasMeasurements = np.zeros((len(d_camMeas['time'] * gyroSampleCount), 3))
timeline = [0.] * (len(d_camMeas['time'] * gyroSampleCount))
count = 0
for i in tqdm(range(numberOfMeasForTest), desc='Trajectory Estimates'):
# Sat
pos = np.array([d_traj['x'][i], d_traj['y'][i], d_traj['z'][i]
]) + np.random.normal(scale=100, size=3)
vel = np.array([d_traj['vx'][i], d_traj['vy'][i], d_traj['vz'][i]])
start_t = d_traj['time'][i]
times = start_t + gyro_t * np.arange(0, gyroSampleCount)
tempCount = count
for i_, t in enumerate(times):
pos = pos + t * vel # propogate position estimate to gyro measurement time
estimatedSatStates[tempCount, :] = pos
tempCount += 1
# Moon
pos = np.array([
d_moonEph['x'][i], d_moonEph['y'][i], d_moonEph['z'][i]
]) + np.random.normal(scale=100, size=3)
vel = np.array(
[d_moonEph['vx'][i], d_moonEph['vy'][i], d_moonEph['vz'][i]])
tempCount = count
for i_, t in enumerate(times):
pos = pos + t * vel # propogate position estimate to gyro measurement time
moonEph[tempCount, :] = pos
tempCount += 1
# Sun
pos = np.array([d_sunEph['x'][i], d_sunEph['y'][i], d_sunEph['z'][i]
]) + np.random.normal(scale=100, size=3)
vel = np.array(
[d_sunEph['vx'][i], d_sunEph['vy'][i], d_sunEph['vz'][i]])
tempCount = count
for i_, t in enumerate(times):
pos = pos + t * vel # propogate position estimate to gyro measurement time
sunEph[tempCount, :] = pos
tempCount += 1
# omegas and biases
tempCount = count
for i_, t in enumerate(times):
omegaMeasurements[tempCount, :] = np.array(
[float(omegax(t)),
float(omegay(t)),
float(omegaz(t))])
biasMeasurements[tempCount, :] = np.array(
[float(biasx(t)),
float(biasy(t)),
float(biasz(t))])
timeline[tempCount] = t
tempCount += 1
count = tempCount
print("START")
results = runAttitudeUKF(cameraDt, (gyro_sigma, gyro_t, Q, R), P0, x0,
quat, omegaMeasurements, biasMeasurements,
estimatedSatStates, moonEph, sunEph, timeline)
print("END")
plotResults(results, q1, q2, q3, q4, biasx, biasy, biasz, timeline)