def __newEstimate(xMean: np.ndarray,
zMean: np.ndarray,
Pxx: np.ndarray,
Pxz: np.ndarray,
Pzz: np.ndarray,
measurements: np.ndarray,
R: np.ndarray,
dynamicsOnly=False) -> TrajectoryEstimateOutput:
"""
Calculates new state given weighted sums of expected measurements and propogated sigmas (dynamics model)
Returns:
[xNew]: New state (6x1)
[pNew]: New covariance matrix (6x6)
[K]: Kalman gain
"""
# Moore-Penrose Pseudoinverse
if not dynamicsOnly:
K = Pxz.dot(np.linalg.pinv(Pzz))
else:
K = np.zeros((6, 6))
# To test dynamics Model
xNew = xMean + K.dot(measurements - zMean)
pNew = Pxx - K.dot(R.dot(K.T))
return TrajectoryEstimateOutput(
new_state=TrajectoryStateVector.from_numpy_array(state=xNew),
new_P=CovarianceMatrix(matrix=pNew),
K=Matrix6x6(matrix=K))
def __get_covariance_matrix_from_state(state_entry) -> CovarianceMatrix:
"""
Obtains 6x6 covariance matrix from state entry. Column names should be in the following format:
r#c# where 1 <= # <= 6
"""
a_P = [
[state_entry.r1c1, state_entry.r1c2, state_entry.r1c3, state_entry.r1c4, state_entry.r1c5, state_entry.r1c6],
[state_entry.r2c1, state_entry.r2c2, state_entry.r2c3, state_entry.r2c4, state_entry.r2c5, state_entry.r2c6],
[state_entry.r3c1, state_entry.r3c2, state_entry.r3c3, state_entry.r3c4, state_entry.r3c5, state_entry.r3c6],
[state_entry.r4c1, state_entry.r4c2, state_entry.r4c3, state_entry.r4c4, state_entry.r4c5, state_entry.r4c6],
[state_entry.r5c1, state_entry.r5c2, state_entry.r5c3, state_entry.r5c4, state_entry.r5c5, state_entry.r5c6],
[state_entry.r6c1, state_entry.r6c2, state_entry.r6c3, state_entry.r6c4, state_entry.r6c5, state_entry.r6c6],
]
return CovarianceMatrix(matrix=np.array(a_P, dtype=float).reshape(6, 6))
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 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))
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 runTrajUKF(moonEph: EphemerisVector,
sunEph: EphemerisVector,
measurements: CameraMeasurementVector,
initState: TrajectoryStateVector,
dt: np.float,
P: CovarianceMatrix,
cameraParams: CameraParameters,
main_thrust_info: MainThrustInfo = None,
dynamicsOnly: bool = False) -> TrajectoryEstimateOutput:
"""
Propogates dynamics model with instantaneous impulse from the main thruster.
Due to the processing delays of the system and the short impulse duration,
the thrust is modeled as an instantenous acceleration on the spacecraft.
[moonEph]: Moon ephemeris vector (1,6)
Format: [x pos (km), y pos (km), z pos (km), vx (km/s), vy (km/s), vz (km/s)] (J2000 ECI)
[sunEph]: Sun ephemeris vector (1,6)
Format: [x pos (km), y pos (km), z pos (km), vx (km/s), vy (km/s), vz (km/s)] (J2000 ECI)
[measurements]: measurement vector (6x1)
Format: [z1, z2, z3, z4, z5, z6]
z1 = pixel distance E-M
z2 = pixel distance E-S
z3 = pixel distance S-M
z4, z5, z6 = pixel widths of E, M, S
Can be None if running dynamics model only
[initEstimate]: state vector from previous execution (or start state) (6x1)
Format: [x pos (km), y pos (km), z pos (km), vx (km/s), vy (km/s), vz (km/s)] (J2000 ECI)
[dt]: time elapsed since last execution (seconds)
[P]: initial covariance matrix for state estimate
[main_thrust_info]: Dictionary representing main thrust data (4x1 quaternion 'kick_orientation', float 'acceleration_magnitude', float 'kick_duration')
[dynamicsOnly]: trust the dynamics model over measurements (helpful for testing)
Returns:
[xNew, pNew, K]: (6x1) new state vector, (6x6) new state covariance estimate, kalman gain
"""
#measurements[3] = 0
#measurements[4] = 0
#measurements[5] = 0
# print(f'moonEph: {moonEph.data}')
# print(f'sunEph: {sunEph.data}')
# print(f'measurements: {measurements.data}')
# print(f'initState: {initState.data}')
# print(f'dt: {dt}')
# print(f'P: {P.data}')
if measurements is None:
measurements = CameraMeasurementVector(0, 0, 0, 0, 0, 0)
moonEph.data = moonEph.data.reshape(1, 6)
sunEph.data = sunEph.data.reshape(1, 6)
measurements.data = measurements.data.reshape(6, 1)
initState.data = initState.data.reshape(6, 1)
P.data = P.data.reshape(6, 6)
if main_thrust_info is not None:
main_thrust_info.get_kick_orientation(
).data = main_thrust_info.get_kick_orientation().data.reshape(4, 1)
nx = __length(P.data)
nv = __length(Const.Q)
k = 3 - nx
lmbda = Const.alpha**2 * (nx + k) - nx # tunable
constant = np.sqrt(nx + nv + lmbda)
centerWeight = lmbda / (nx + nv + lmbda)
otherWeight = 1 / (2 * (nx + nv + lmbda))
const = cameraParams.hPix / (
cameraParams.hFov * np.pi / 180
) # camera constant: PixelWidth/FOV [pixels/radians]
# TODO: Remove test pixel to angle conversion
# measurements /= const
Sx = np.linalg.cholesky(P.data)
# Generate Sigma Points
sigmas, noise = __makeSigmas(initState.data, Sx, Const.Sv, nx, nv,
constant)
# Propogate Sigma Points
propSigmas = np.zeros_like(sigmas) # Initialize
# Proprogate Sigma Points by running through dynamics model/function
for j in range(__length(sigmas)):
propSigmas[:, j] = __dynamics_model(sigmas[:, j] + noise[:, j],
dt,
moonEph.data,
sunEph.data,
main_thrust_info=main_thrust_info)
sigmaMeasurements = np.zeros((6, __length(propSigmas)))
# Sigma measurements are the expected measurements calculated from
# running the propagated sigma points through measurement model
for j in range(__length(sigmas)):
sigmaMeasurements[:, j] = __measModel(
propSigmas[:, j] +
np.random.multivariate_normal(np.zeros(
(6, )), Const.R).T, moonEph.data, sunEph.data, const)
# a priori estimates
xMean, zMean = __getMeans(propSigmas, sigmaMeasurements, centerWeight,
otherWeight)
# Calculates the covariances as weighted sums
Pxx, Pxz, Pzz = __findCovariances(xMean, zMean, propSigmas,
sigmaMeasurements, centerWeight,
otherWeight, Const.alpha, Const.beta,
Const.R)
# a posteriori estimates
return __newEstimate(xMean,
zMean,
Pxx,
Pxz,
Pzz,
measurements.data.reshape(6, 1) +
np.random.multivariate_normal(np.zeros(
(6)), Const.R).reshape(6, 1),
Const.R,
dynamicsOnly=dynamicsOnly)
def UKFSingle(cameradt: float, gyroVars: GyroVars, P0: np.ndarray,
x0: np.ndarray, q0: np.ndarray, omegas, estimatedSatState,
moonEph: np.ndarray, sunEph: np.ndarray,
timeline: List[float]) -> AttitudeEstimateOutput:
"""
NOTE: Does not make any assumptions about gyroSampleCount
let n = # of camera measurements in batch
let gyroSampleCount = 1/gyro_sample_rate
[gyroVars]: (gyro_sigma, gyro_sample_rate, Q, R)
[omegas]: (x, y, z) components of measured angular velocity (n x gyroSampleCount, 3)
[estimatedSatState]: trajectory UKF outputs from t = 0 to t = totalIntegrationTime (n x gyroSampleCount, 3)
[moonEph]: moon position/vel from ephemeris table (n x gyroSampleCount, 3)
[sunEph]: sun position/vel from ephemeris table (n x gyroSampleCount, 3)
[timeline]: time deltas of gyro readings: [avg delta t, 2nd-1st, 3rd-2nd, ...]
See runAttitudeUKF() for remaining parameters and return type info.
"""
# assert(omegas[0].shape[0] == biases.shape[0] == estimatedSatState.shape[0] == moonEph.shape[0] == sunEph.shape[0])
n = moonEph.shape[0]
# (gyro_sigma, gyro_sample_rate, Q, R) = gyroVars
# Not sure if the following lines are what the above line meant to be:
gyro_sigma = gyroVars.gyro_sigma
gyro_sample_rate = gyroVars.gyro_sample_rate
Q = gyroVars.get_Q_matrix()
R = gyroVars.get_R_matrix()
# Phist = np.zeros((int(n/gyroSampleCount), 6, 6))
# qhist = np.zeros((int(n/gyroSampleCount), 4, 1))
# xhist = np.zeros((int(n/gyroSampleCount), 6, 1))
Phatkp = P0
xhatkp = x0
qhatkp = q0
# Calculate position vectors
# satPos, moonPos, sunPos = estimatedSatState[i:i+gyroSampleCount,:], moonEph[i:i+gyroSampleCount,:], sunEph[i:i+gyroSampleCount,:]
# earthVec, moonVec, sunVec = np.zeros((gyroSampleCount, 3)), np.zeros((gyroSampleCount, 3)), np.zeros((gyroSampleCount, 3))
# for g_sample in range(gyroSampleCount):
# earthVec[i+g_sample] = (-1*satPos[i+g_sample])/np.linalg.norm(satPos[i+g_sample])
# moonVec[i+g_sample] = (moonPos[i+g_sample] - satPos[i+g_sample])/np.linalg.norm(moonPos[i+g_sample] - satPos[i+g_sample])
# sunVec[i+g_sample] = (sunPos[i+g_sample] - satPos[i+g_sample])/np.linalg.norm(sunPos[i+g_sample] - satPos[i+g_sample])
satPos, moonPos, sunPos = estimatedSatState[0, :], moonEph[0, :], sunEph[
0, :]
earthVec = (-1 * satPos) / np.linalg.norm(satPos)
moonVec = (moonPos - satPos) / np.linalg.norm(moonPos - satPos)
sunVec = (sunPos - satPos) / np.linalg.norm(sunPos - satPos)
sigma_points = generateSigmas(xhatkp, Phatkp, Q)
err_quats = makeErrorQuaternion(sigma_points)
pert_quats = perturbQuaternionEstimate(err_quats, qhatkp)
prop_quats = propagateQuaternion(pert_quats, sigma_points, timeline,
gyro_sigma, gyro_sample_rate, omegas,
x0[3:], cameradt)
qhatkp1m = prop_quats[0]
prop_err = propagatedQuaternionError(prop_quats)
prop_sigmas = recoverPropSigma(prop_err, sigma_points)
x_mean = predictedMean(prop_sigmas)
p_mean = predictedCov(prop_sigmas, x_mean, Q)
h_list = hFromSigs(prop_quats, earthVec, moonVec, sunVec)
z_mean = meanMeasurement(h_list)
Pzz_kp1 = Pzz(h_list, z_mean, R)
Pxz_kp1 = Pxz(prop_sigmas, x_mean, h_list, z_mean)
K = getGain(Pxz_kp1, Pzz_kp1)
meas = np.concatenate((earthVec, moonVec, sunVec), axis=0).reshape(9, 1)
assert (z_mean.shape == meas.shape)
xhat_kp1 = updateXhat(x_mean, K, meas, z_mean)
Phat_kp1 = updatePhat(p_mean, K, Pzz_kp1)
qhat_kp1 = updateQuaternionEstimate(xhat_kp1, qhatkp1m)
# Phist[counter] = Phat_kp1
# qhist[counter] = qhat_kp1
# xhist[counter] = xhat_kp1
# Phatkp = Phat_kp1
# xhatkp = resetSigmaSeed(xhat_kp1)
# qhatkp = qhat_kp1
return AttitudeEstimateOutput(
new_state=AttitudeStateVector.from_numpy_array(state=xhat_kp1),
new_P=CovarianceMatrix(matrix=Phat_kp1),
new_quat=QuaternionVector.from_numpy_array(quat=qhat_kp1))
def cislunar1_timestep(visual_analysis, state_error, kickTime=None):
"""
[part_start, part_end): start (inclusive) and end (exclusive) indices of trajectory
"""
from tests.const import TEST_CISLUNAR_meas, TEST_CISLUNAR_moonEph, TEST_CISLUNAR_sunEph, \
TEST_CISLUNAR_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)
d_traj = pd.read_csv(TEST_CISLUNAR_traj).to_dict()
d_moonEph = pd.read_csv(TEST_CISLUNAR_moonEph).to_dict()
d_sunEph = pd.read_csv(TEST_CISLUNAR_sunEph).to_dict()
d_camMeas = pd.read_csv(TEST_CISLUNAR_meas).to_dict()
iterations = min(len(d_traj['x']), len(d_moonEph['x']), len(d_sunEph['x']),
len(d_camMeas['Z1']))
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)
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":
liveTraj = LiveTrajectoryPlot()
# print(int(trajTruthdf.shape[0]*1 - 1))
for t in tqdm(range(iterations), desc='Trajectory Completion'):
moonEph = EphemerisVector(x_pos=d_moonEph['x'][t],
y_pos=d_moonEph['y'][t],
z_pos=d_moonEph['z'][t],
x_vel=d_moonEph['vx'][t],
y_vel=d_moonEph['vy'][t],
z_vel=d_moonEph['vz'][t])
sunEph = EphemerisVector(x_pos=d_sunEph['x'][t],
y_pos=d_sunEph['y'][t],
z_pos=d_sunEph['z'][t],
x_vel=d_sunEph['vx'][t],
y_vel=d_sunEph['vy'][t],
z_vel=d_sunEph['vz'][t])
meas = CameraMeasurementVector(ang_em=d_camMeas['Z1'][t],
ang_es=d_camMeas['Z2'][t],
ang_ms=d_camMeas['Z3'][t],
e_dia=d_camMeas['Z4'][t],
m_dia=d_camMeas['Z5'][t],
s_dia=d_camMeas['Z6'][t])
orientation = None
if kickTime is not None and t > kickTime:
orientation = [0, 0, 0, 1]
stateEstimate: TrajectoryEstimateOutput = runTrajUKF(
moonEph,
sunEph,
meas,
TrajectoryStateVector.from_numpy_array(state=state),
60,
CovarianceMatrix(matrix=P),
CesiumTestCameraParameters,
dynamicsOnly=False
) # used to have this, not sure why it's gone orientation=orientation
state = stateEstimate.new_state.data
P = stateEstimate.new_P.data
K = stateEstimate.K.data
# 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 liveTraj:
liveTraj.close()
t = iterations - 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()
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'