def beaconStates(input):
# pull out the inputs for the generation of observation data
beaconList = input[0]
observationTimes = input[1]
extras = input[-1]
# compute and save total number of observations
nObservationss = len( beaconList )
# Load Ephemeris Files from extras dictionary
pyswice.furnsh_c(bskSpicePath + extras['basic_bsp'])
pyswice.furnsh_c(dinoSpicePath + extras['mission_bsp'])
pyswice.furnsh_c(dinoSpicePath + extras['tls'])
# instantiate beaconStates
beaconStates = []
# for each observation time, compute the state of the dictated beacon
for ii in xrange( nObservationss ):
stateArray = np.zeros(6)
state = pyswice.new_doubleArray(6)
lt = pyswice.new_doubleArray(1)
pyswice.spkezr_c( beaconList[ii], observationTimes[ii], extras['ref_frame'],\
'None', 'SUN', state, lt)
for i in range(6):
stateArray[i] = pyswice.doubleArray_getitem(state, i)
beaconStates.append(stateArray)
# convert final data set into numpy array
beaconStates = np.array(beaconStates)
return beaconStates
def EOM(state, et, primaryIndex, secondaryIndices, nSecondaries, muPrimary, muSecondaries,
kSRP, cR, abcorr, refFrame, bodies, stateDimension):
# pull out the STM
phi = np.array(state[stateDimension:], copy=True)
phi = np.reshape(phi, (stateDimension, stateDimension) )
# gravitational force from primary body
fPrimary = -muPrimary * state[0:3] / np.linalg.norm(state[0:3]) ** 3
# gravitational force from secondary bodies
f3rdBodies = 0
# set the size of the spacecraftPositionitionSecondaries between
# secondary bodies and primary body
position_secondaries_primary = np.zeros((3, nSecondaries))
# loop through the secondary bodies
for ii in range(nSecondaries):
# determine distance from secondary to primary body
positionArray = np.zeros(3)
stateSpice = pyswice.new_doubleArray(6)
lt = pyswice.new_doubleArray(1)
pyswice.spkezr_c(bodies[secondaryIndices[ii]], et, refFrame,
abcorr, bodies[primaryIndex], stateSpice, lt)
for i in range(3):
positionArray[i] = pyswice.doubleArray_getitem(stateSpice, i)
position_secondaries_primary[:, ii] = positionArray
# calculate the "third body" force
f3rdBodies += -muSecondaries[ii] * \
( ( state[0:3] - position_secondaries_primary[:, ii] ) / \
np.linalg.norm(state[0:3] - position_secondaries_primary[:, ii] ) ** 3 + \
position_secondaries_primary[:, ii] / \
np.linalg.norm( position_secondaries_primary[:, ii] )**3 )
# SRP force
fSRP = cR * kSRP * state[0:3] / \
np.linalg.norm(state[0:3]) ** 3
# total force (acceleration) vector
f = fPrimary + f3rdBodies + fSRP
# args for the A matrix function
args = (state[0:stateDimension], nSecondaries, muPrimary, muSecondaries, kSRP, cR,\
position_secondaries_primary)
# A matrix calculation
A = matrixA(args)
# compute the derivative of the STM
dPhi = np.dot(A, phi)
dPhi = np.reshape(dPhi,stateDimension * stateDimension)
# acceleration vector to be returned to the integrator
dState = [state[3], state[4], state[5], f[0], f[1], f[2]]
dState += list(dPhi)
return dState
def beaconStates(input):
# pull out the inputs for the generation of observation data
beaconList = input[0]
observationTimes = input[1]
extras = input[-1]
# compute and save total number of observations
nObservationss = len(beaconList)
# Load Ephemeris Files from extras dictionary
pyswice.furnsh_c(bskSpicePath + extras['basic_bsp'])
pyswice.furnsh_c(dinoSpicePath + extras['mission_bsp'])
pyswice.furnsh_c(dinoSpicePath + extras['tls'])
# instantiate beaconStates
beaconStates = []
# for each observation time, compute the state of the dictated beacon
for ii in xrange(nObservationss):
stateArray = np.zeros(6)
state = pyswice.new_doubleArray(6)
lt = pyswice.new_doubleArray(1)
pyswice.spkezr_c( beaconList[ii], observationTimes[ii], extras['ref_frame'],\
'None', 'SUN', state, lt)
for i in range(6):
stateArray[i] = pyswice.doubleArray_getitem(state, i)
beaconStates.append(stateArray)
# convert final data set into numpy array
beaconStates = np.array(beaconStates)
return beaconStates
def EOM(state, et, primary_index, secondary_indices, nSecondaries, muPrimary,
muSecondaries, kSRP, cR, abcorr, ref_frame, bodies, stateDimension):
# pull out the STM
phi = np.array(state[stateDimension:], copy=True)
phi = np.reshape(phi, (stateDimension, stateDimension))
# gravitational force from primary body
fPrimary = -muPrimary * state[0:3] / np.linalg.norm(state[0:3])**3
# gravitational force from secondary bodies
f3rdBodies = 0
# set the size of the spacecraftPositionitionSecondaries between
# secondary bodies and primary body
position_secondaries_primary = np.zeros((3, nSecondaries))
# loop through the secondary bodies
for ii in range(nSecondaries):
# determine distance from secondary to primary body
positionArray = np.zeros(3)
stateSpice = pyswice.new_doubleArray(6)
lt = pyswice.new_doubleArray(1)
pyswice.spkezr_c(bodies[secondary_indices[ii]], et, ref_frame, abcorr,
bodies[primary_index], stateSpice, lt)
for i in range(3):
positionArray[i] = pyswice.doubleArray_getitem(stateSpice, i)
position_secondaries_primary[:, ii] = positionArray
# calculate the "third body" force
f3rdBodies += -muSecondaries[ii] * \
( ( state[0:3] - position_secondaries_primary[:, ii] ) / \
np.linalg.norm(state[0:3] - position_secondaries_primary[:, ii] ) ** 3 + \
position_secondaries_primary[:, ii] / np.linalg.norm( position_secondaries_primary[:, ii] )**3 )
# SRP force
fSRP = cR * kSRP * state[0:3] / np.linalg.norm(state[0:3])**3
# total force (acceleration) vector
f = fPrimary + f3rdBodies + fSRP + state[6:9]
# args for the A matrix function
args = (state[0:stateDimension], nSecondaries, muPrimary, muSecondaries,
kSRP, cR, position_secondaries_primary)
# A matrix calculation
A = matrixA(args)
# calculate the derivative of the STM
dPhi = np.dot(A, phi)
dPhi = np.reshape(dPhi, stateDimension * stateDimension)
# acceleration vector to be returned to the integrator
dState = [0] * stateDimension
dState[0:6] = [state[3], state[4], state[5], f[0], f[1], f[2]]
dState += list(dPhi)
return dState
def main():
# extras dictionary for importing to functions
extras = {}
###########################################
#
# S P I C E C O D E
#
##########################################
# basic .bsp filename (generic, such as de430, etc)
extras['basic_bsp'] = 'de430.bsp'
# .bsp filename for mission
extras['mission_bsp'] = 'DINO_kernel.bsp'
# .tls filename
extras['tls'] = 'naif0011.tls'
# prep pyswice for the extraction of initial data
# is the only reason that we do this is for lines 165 and 166?
pyswice.furnsh_c(bskSpicePath + 'de430.bsp')
pyswice.furnsh_c(dinoSpicePath + 'naif0011.tls')
pyswice.furnsh_c(dinoSpicePath + 'DINO_kernel.bsp')
DINO_kernel = dinoSpicePath + 'DINO_kernel.bsp'
body_int = -100 #SP.spkobj(DINO_kernel)
body_id_str = str(body_int)
# search_window = pyswice.new_doubleArray(2)
# pyswice.spkcov_c(DINO_kernel, body_int, search_window)
# list_of_events = pyswice.wnfetd_c(search_window, 0)
# tBSP_Start = list_of_events[0]
# tBSP_End = list_of_events[1]
###########################################
# Initial condition for spacecraft
# data = io.loadmat('saves/obsData.mat')
# trueEphemeris = {}
# reference of sun to sc
# trueEphemeris['spacecraft'] = np.copy(data['stateS'])
# # reference of sun to Earth
# trueEphemeris['S2E'] = np.copy(data['stateE'])
# # reference of sun to Mars
# trueEphemeris['S2M'] = np.copy(data['stateM'])
# time span
# timeSpan = data['etT'].flatten()
#Filtering End Epochs
start_et = pyswice.new_doubleArray(1)
end_et = pyswice.new_doubleArray(1)
pyswice.utc2et_c('23 JUL 2020 17:00:00', start_et)
pyswice.utc2et_c('30 JUL 2020 17:00:00', end_et)
start_et = pyswice.doubleArray_getitem(start_et, 0)
end_et = pyswice.doubleArray_getitem(end_et, 0)
# body vector for SUN, EARTH, MARS
# CODE RELIES ON SUN BEING INDEXED AS 0
extras['bodies'] = ['SUN', '3', '399']
# specify primary and secondary
extras['primary'] = 0
extras['secondary'] = [1, 2]
# respective GP vector
extras['mu'] = [1.32712428 * 10**11, 3.986004415 * 10**5, 4.305 * 10**4]
# abcorr for spkzer
extras['abcorr'] = 'NONE'
# reference frame
extras['ref_frame'] = 'J2000'
# SRP parameter
# A/M ratio multiplied by solar pressure constant at 1 AU with adjustments
extras[
'SRP'] = 0.3**2 / 14. * 149597870.**2 * 1358. / 299792458. / 1000. # turboprop document Eq (64)
# coefficient of reflectivity
extras['cR'] = 1.
# number of observations per beacon until moving to the next
extras['repeat_obs'] = 1
# SNC coefficient
extras['SNC'] = (2 * 10**(-4))**3
# Number of batch iterations
extras['iterations'] = 3
# Initializing the error
extras['x_hat_0'] = 0
# rng seed for debugging purposes
extras['seed'] = 5
##################################################################################
#
# Camera/P&L Parameters
#
##################################################################################
# Focal Length (mm)
extras['FoL'] = 100.
angles = []
extras['DCM_BI'] = np.eye(3)
extras['DCM_TVB'] = np.eye(3)
# Camera resolution (pixels)
extras['resolution'] = [1024., 1024.]
# width and height of pixels in camera
extras['pixel_width'] = 5.
extras['pixel_height'] = 5.
# direction coefficient of pixel and line axes
extras['pixel_direction'] = 1.
extras['line_direction'] = 1.
# Are we using the real dynamics for the ref or the trueData
extras['realData'] = 'OFF'
# Add anomaly detection parameters
extras['anomaly'] = False
extras['anomaly_num'] = 0
extras['anomaly_threshold'] = 4
##################################################################################
# Get Observation Times and Ephemerides. This outputs a full data set that is not
# parsed in any way. Ephemerides for all objects at all times are given.
trueEphemeris, timeSpan = dg.generate_data(
sc_ephem_file=DINO_kernel,
planet_beacons=['earth', 'mars barycenter'],
beaconIDs=[],
n_observations=24,
start_et=start_et,
end_et=end_et,
extras=extras,
realData=extras['realData'])
tt_switch = 5
print '------------------'
print 'Filter Image Span : ', (timeSpan[-1] - timeSpan[0]) / (60 * 60 *
24), 'days'
print '------------------'
# number and keys of beacons. note that the true ephem is going to have one spot for the
# sun, which in NOT a beacon. These are used in beaconBinSPICE.
beacon_names = trueEphemeris.keys()
beacon_names.remove('spacecraft')
extras['unique_beacon_IDs'] = beacon_names
extras['n_unique_beacons'] = len(beacon_names)
##################################################################################
#
# BLOCK A page 196
#
##################################################################################
# copy the initial conditions as the first sun to SC referenceStates from the SPICE file
IC = np.copy(trueEphemeris['spacecraft'][:, 0])
print 'IC', IC
# spice_derived_state is only referenced here. Should these be axed?
spice_derived_state = pyswice.new_doubleArray(6)
lt = pyswice.new_doubleArray(1)
pyswice.spkezr_c(body_id_str, timeSpan[0], 'J2000', 'None', 'Sun',
spice_derived_state, lt)
# a priori uncertainty for the referenceStates
covBar = np.zeros((IC.shape[0], IC.shape[0]))
covBar[0, 0] = 10000**2
covBar[1, 1] = 10000**2
covBar[2, 2] = 10000**2
covBar[3, 3] = .1**2
covBar[4, 4] = .1**2
covBar[5, 5] = .1**2
# add uncertainty to the IC
initialPositionError = 1000 * np.divide(IC[0:3], np.linalg.norm(IC[0:3]))
initialVelocityError = 0.01 * np.divide(IC[3:6], np.linalg.norm(IC[3:6]))
IC[0:6] += np.append(initialPositionError, initialVelocityError)
# uncertainty to be added in the form of noise to the measurables.
# Takes the form of variance. Currently, the same value is used in both
# the creation of the measurements as well as the weighting of the filter (W)
observationUncertainty = np.identity(2)
observationUncertainty[0, 0] = 0.2**2
observationUncertainty[1, 1] = 0.2**2
# the initial STM is an identity matrix
phi0 = np.identity(IC.shape[0])
# initiate a priori deviation
stateDevBar = np.zeros(IC.shape)
# initiate a filter output dictionary
filterOutputs = {}
##################################################################################
#
# Get the noisy observations
#
##################################################################################
# observation inputs
observationInputs = (trueEphemeris, observationUncertainty, angles, extras)
# Get the observation data (dataObservations). This dictionary contains the SPICE data
# from which values are calculated (key = 'SPICE'), the true observations before
# uncertainty is added (key = 'truth') and the measured observations (key = 'measurements').
# These are the 'measurements' values that are now simulating an actual observation,
# and they are to be processed by the filter.
# The dictionary also contains the list of beacons by name and order of processing.
# This list of strings (key = 'beacons') is needed for
# the filter's own beacon position generator
dataObservations = getObs(observationInputs)
# create dictionary for observation data to be inputs in filter. This is a more limited
# dictionary than dataObservations and serves as the most "real" input
filterObservations = {}
filterObservations['measurements'] = dataObservations['measurements']
filterObservations['beaconIDs'] = dataObservations['beacons']
##################################################################################
#
# Run the Filter
#
##################################################################################
# alter to coefficient of reflectivity to be zero. This negates any contribution of
# modeling SRP
extras['cR'] = 0.0
# run the filter and output the referenceStates (including STMs), est states and extra data
for itr in xrange(extras['iterations']):
if itr > 0:
IC = estimatedState[0, :]
stateDevBar -= extraData['stateDevHatArray'][0, :]
if itr == 0:
extras['oldPost'] = np.zeros([len(timeSpan), 2])
# the arguments for the filter: the IC, the first STM, the time span, the observables
# data dictionary, a priori uncertainty, and the measurables' uncertainty,
# as well as any extras
filterInputs = (IC, phi0, timeSpan, filterObservations,\
covBar, observationUncertainty, stateDevBar, angles, extras)
# run filter function
referenceState, estimatedState, extraData = run_batch(filterInputs)
extras['oldPost'] = extraData['postfit residuals']
# Check for anomaly:
[anomaly_bool, anomaly_num] = extraData['anomaly_detected']
if anomaly_bool == True:
print '**********************************************************'
print 'Anomaly Detected - Estimates are not to be trusted'
print '**********************************************************'
print anomaly_num, 'Residuals out of bounds'
return
# save all outputs into the dictionary with a name associated with the iteration
filterOutputs[str(itr)] = {}
filterOutputs[str(itr)]['referenceState'] = referenceState
filterOutputs[str(itr)]['estimatedState'] = estimatedState
filterOutputs[str(itr)]['extraData'] = extraData
##################################################################################
#
# \ BLOCK A page 196
#
##################################################################################
# Iteration Directory
dirIt = 'Batch_Iteration' + str(itr + 1)
# Make directory for the iterations
if not os.path.exists(dirIt):
os.makedirs(dirIt)
# File to write data
writingText( itr+1, referenceState, estimatedState, trueEphemeris, extraData,\
initialPositionError , initialVelocityError)
# calculate the difference between the perturbed reference and
# true trajectories: reference state errors
stateError = referenceState[:, 0:6] - trueEphemeris['spacecraft'].T
# compare the estimated and true trajectories: estimated state errors
stateErrorHat = estimatedState[:, 0:6] - trueEphemeris['spacecraft'].T
plotData = extraData
plotData['postfit delta'] = extraData['postfit changes']
plotData['states'] = estimatedState
plotData['truth'] = dataObservations['truth']
plotData['beacon_list'] = dataObservations['beacons']
plotData['timeSpan'] = timeSpan
plotData['dirIt'] = dirIt
plotData['err'] = stateError
plotData['stateErrorHat'] = stateErrorHat
plotData['obs_uncertainty'] = observationUncertainty
plotData['referenceState'] = referenceState
plotData['trueEphemeris'] = trueEphemeris
plotData['extras'] = extras
plotData['acc_est'] = 'OFF'
PF(plotData)
# Write the output to the pickle file
fileTag = 'SRP_test'
file = dirIt + '/' + fileTag + '_data.pkl'
pklFile = open(file, 'wb')
pickle.dump(plotData, pklFile, -1)
pklFile.flush()
pklFile.close()
def main():
# extras dictionary for importing to functions
extras = {}
###########################################
#
# S P I C E C O D E
#
##########################################
# basic .bsp filename (generic, such as de430, etc)
extras['basic_bsp'] = 'de430.bsp'
# .bsp filename for mission
extras['mission_bsp'] = 'DINO_kernel.bsp'
# .tls filename
extras['tls'] = 'naif0011.tls'
# prep pyswice for the extraction of initial data
# is the only reason that we do this is for lines 165 and 166?
pyswice.furnsh_c(bskSpicePath + 'de430.bsp')
pyswice.furnsh_c(dinoSpicePath + 'naif0011.tls')
pyswice.furnsh_c(dinoSpicePath + 'DINO_kernel.bsp')
DINO_kernel = dinoSpicePath + 'DINO_kernel.bsp'
body_int = -100#SP.spkobj(DINO_kernel)
body_id_str = str(body_int)
# search_window = pyswice.new_doubleArray(2)
# pyswice.spkcov_c(DINO_kernel, body_int, search_window)
# list_of_events = pyswice.wnfetd_c(search_window, 0)
# tBSP_Start = list_of_events[0]
# tBSP_End = list_of_events[1]
###########################################
# Initial condition for spacecraft
# data = io.loadmat('saves/obsData.mat')
# trueEphemeris = {}
# reference of sun to sc
# trueEphemeris['spacecraft'] = np.copy(data['stateS'])
# # reference of sun to Earth
# trueEphemeris['S2E'] = np.copy(data['stateE'])
# # reference of sun to Mars
# trueEphemeris['S2M'] = np.copy(data['stateM'])
# time span
# timeSpan = data['etT'].flatten()
#Filtering End Epochs
start_et = pyswice.new_doubleArray(1)
end_et=pyswice.new_doubleArray(1)
pyswice.utc2et_c('23 JUL 2020 17:00:00', start_et)
pyswice.utc2et_c('30 JUL 2020 17:00:00', end_et)
start_et = pyswice.doubleArray_getitem(start_et, 0)
end_et = pyswice.doubleArray_getitem(end_et, 0)
# body vector for SUN, EARTH, MARS
# CODE RELIES ON SUN BEING INDEXED AS 0
extras['bodies'] = ['SUN', '3', '399']
# specify primary and secondary
extras['primary'] = 0
extras['secondary'] = [1, 2]
# respective GP vector
extras['mu'] = [1.32712428 * 10 ** 11, 3.986004415 * 10 ** 5, 4.305 * 10 ** 4]
# abcorr for spkzer
extras['abcorr'] = 'NONE'
# reference frame
extras['ref_frame'] = 'J2000'
# SRP parameter
# A/M ratio multiplied by solar pressure constant at 1 AU with adjustments
extras['SRP'] = 0.3**2/14. * 149597870.**2 * 1358. / 299792458. / 1000. # turboprop document Eq (64)
# coefficient of reflectivity
extras['cR'] = 1.
# number of observations per beacon until moving to the next
extras['repeat_obs'] = 1
# SNC coefficient
extras['SNC'] = (2 * 10 ** (-4)) ** 3
# Number of batch iterations
extras['iterations'] = 3
# Initializing the error
extras['x_hat_0'] = 0
# rng seed for debugging purposes
extras['seed'] = 5
##################################################################################
#
# Camera/P&L Parameters
#
##################################################################################
# Focal Length (mm)
extras['FoL'] = 100.
angles = []
extras['DCM_BI'] = np.eye(3)
extras['DCM_TVB'] = np.eye(3)
# Camera resolution (pixels)
extras['resolution'] = [1024., 1024.]
# width and height of pixels in camera
extras['pixel_width'] = 5.
extras['pixel_height'] = 5.
# direction coefficient of pixel and line axes
extras['pixel_direction'] = 1.
extras['line_direction'] = 1.
# Are we using the real dynamics for the ref or the trueData
extras['realData']= 'OFF'
# Add anomaly detection parameters
extras['anomaly']= False
extras['anomaly_num'] = 0
extras['anomaly_threshold'] = 4
##################################################################################
# Get Observation Times and Ephemerides. This outputs a full data set that is not
# parsed in any way. Ephemerides for all objects at all times are given.
trueEphemeris, timeSpan = dg.generate_data(sc_ephem_file=DINO_kernel,
planet_beacons = ['earth','mars barycenter'],
beaconIDs=[],
n_observations=24,
start_et=start_et,
end_et=end_et,
extras = extras,
realData = extras['realData'])
tt_switch = 5
print '------------------'
print 'Filter Image Span : ' , (timeSpan[-1] - timeSpan[0])/(60*60*24), 'days'
print '------------------'
# number and keys of beacons. note that the true ephem is going to have one spot for the
# sun, which in NOT a beacon. These are used in beaconBinSPICE.
beacon_names = trueEphemeris.keys()
beacon_names.remove('spacecraft')
extras['unique_beacon_IDs'] = beacon_names
extras['n_unique_beacons'] = len(beacon_names)
##################################################################################
#
# BLOCK A page 196
#
##################################################################################
# copy the initial conditions as the first sun to SC referenceStates from the SPICE file
IC = np.copy(trueEphemeris['spacecraft'][:, 0])
######################################
# UNMODELED ACCELERATION TERMS
# ALPHA IMPLEMENTATION
# CURRENTLY TOO MUCH HARD CODING
######################################
IC = np.append( IC, np.array( [0, 0, 0] ) )
print 'IC', IC
# spice_derived_state is only referenced here. Should these be axed?
spice_derived_state = pyswice.new_doubleArray(6)
lt = pyswice.new_doubleArray(1)
pyswice.spkezr_c(body_id_str, timeSpan[0], 'J2000', 'None', 'Sun', spice_derived_state, lt)
# a priori uncertainty for the referenceStates
covBar = np.zeros((IC.shape[0], IC.shape[0]))
covBar[0, 0] = 10000**2
covBar[1, 1] = 10000**2
covBar[2, 2] = 10000**2
covBar[3, 3] = .1**2
covBar[4, 4] = .1**2
covBar[5, 5] = .1**2
covBar[6, 6] = (10**(-8))**2
covBar[7, 7] = (10**(-8))**2
covBar[8, 8] = (10**(-8))**2
# add uncertainty to the IC
initialPositionError = 1000 * np.divide(IC[0:3], np.linalg.norm(IC[0:3]))
initialVelocityError = 0.01 * np.divide(IC[3:6], np.linalg.norm(IC[3:6]))
IC[0:6] += np.append(initialPositionError, initialVelocityError)
# uncertainty to be added in the form of noise to the measurables.
# Takes the form of variance. Currently, the same value is used in both
# the creation of the measurements as well as the weighting of the filter (W)
observationUncertainty = np.identity(2)
observationUncertainty[0, 0] = 0.2 ** 2
observationUncertainty[1, 1] = 0.2 ** 2
# the initial STM is an identity matrix
phi0 = np.identity(IC.shape[0])
# initiate a priori deviation
stateDevBar = np.zeros(IC.shape)
# initiate a filter output dictionary
filterOutputs = {}
##################################################################################
#
# Get the noisy observations
#
##################################################################################
# observation inputs
observationInputs = (trueEphemeris, observationUncertainty, angles, extras)
# Get the observation data (dataObservations). This dictionary contains the SPICE data
# from which values are calculated (key = 'SPICE'), the true observations before
# uncertainty is added (key = 'truth') and the measured observations (key = 'measurements').
# These are the 'measurements' values that are now simulating an actual observation,
# and they are to be processed by the filter.
# The dictionary also contains the list of beacons by name and order of processing.
# This list of strings (key = 'beacons') is needed for
# the filter's own beacon position generator
dataObservations = getObs(observationInputs)
# create dictionary for observation data to be inputs in filter. This is a more limited
# dictionary than dataObservations and serves as the most "real" input
filterObservations = {}
filterObservations['measurements'] = dataObservations['measurements']
filterObservations['beaconIDs'] = dataObservations['beacons']
##################################################################################
#
# Run the Filter
#
##################################################################################
# alter to coefficient of reflectivity to be zero. This negates any contribution of
# modeling SRP
extras['cR'] = 0.0
# run the filter and output the reference referenceStates (including STMs), est states and extra data
for itr in xrange(extras['iterations']):
if itr > 0:
# IC = est_state[0, :]
IC += extraData['stateDevHatArray'][0, :]
stateDevBar -= extraData['stateDevHatArray'][0, :]
# the arguments for the filter are the IC, the first STM, the time span, the observables
# data dictionary, a priori uncertainty, and the measurables' uncertainty,
# as well as any extras
if itr==0:
extras['oldPost'] = np.zeros([len(timeSpan), 2])
filterInputs = (IC, phi0, timeSpan, filterObservations,\
covBar, observationUncertainty, stateDevBar, angles, extras)
# run filter function
referenceState, estimatedState, extraData = run_batch(filterInputs)
extras['oldPost'] = extraData['postfit residuals']
# save all outputs into the dictionary with a name associated with the iteration
filterOutputs[str(itr)] = {}
filterOutputs[str(itr)]['referenceState'] = referenceState
filterOutputs[str(itr)]['estimatedState'] = estimatedState
filterOutputs[str(itr)]['extraData'] = extraData
##################################################################################
#
# \ BLOCK A page 196
#
##################################################################################
# Iteration Directory
dirIt = 'Batch_Iteration' + str(itr+1)
# Make directory for the iterations
if not os.path.exists(dirIt):
os.makedirs(dirIt)
# File to write data
writingText(itr+1, referenceState, estimatedState, trueEphemeris, extraData, initialPositionError , initialVelocityError)
# calculate the difference between the perturbed reference and true trajectories: reference state errors
stateError = referenceState[:, 0:6] - trueEphemeris['spacecraft'].T
# compare the estimated and true trajectories: estimated state errors
stateErrorHat = estimatedState[:, 0:6] - trueEphemeris['spacecraft'].T
plotData = extraData
plotData['postfit delta'] = extraData['postfit changes']
plotData['states'] = estimatedState
plotData['truth'] = dataObservations['truth']
plotData['beacon_list'] = dataObservations['beacons']
plotData['timeSpan'] = timeSpan
plotData['dirIt'] = dirIt
plotData['err'] = stateError
plotData['stateErrorHat'] = stateErrorHat
plotData['obs_uncertainty'] = observationUncertainty
plotData['referenceState'] = referenceState
plotData['trueEphemeris'] = trueEphemeris
plotData['extras'] = extras
plotData['acc_est'] = 'ON'
PF( plotData )
# Write the output to the pickle file
fileTag = 'SRP_test'
file = dirIt+'/'+fileTag+'_data.pkl'
pklFile = open( file, 'wb')
pickle.dump( plotData, pklFile, -1 )
pklFile.flush()
pklFile.close()
[anomaly_bool , anomaly_num] = extraData['anomaly_detected']
if anomaly_bool == True:
print '**********************************************************'
print 'Anomaly Detected - Estimates are not to be trusted'
print '**********************************************************'
print anomaly_num, 'Residuals out of bounds'
return
def generate_data(sc_ephem_file, planet_beacons,
beaconIDs, n_observations=10,
start_et=None, end_et=None, extras = {}, realData = 'OFF', ref='J2000'):
# Load Ephemeris Files
pyswice.furnsh_c(sc_ephem_file)
pyswice.furnsh_c(bskSpicePath + extras['basic_bsp'])
pyswice.furnsh_c(dinoSpicePath + extras['mission_bsp'])
pyswice.furnsh_c(dinoSpicePath + extras['tls'])
for beacon in planet_beacons:
found = pyswice.new_intArray(1)
pyswice.intArray_setitem(found, 0, 0)
beaconid = pyswice.new_intArray(1)
pyswice.bodn2c_c(beacon, beaconid, found)
beaconIDs.append(pyswice.intArray_getitem(beaconid, 0))
# Identify Spacecraft Body
bodyID = -100 # SP.spkobj(sc_ephem_file)
bodyIDStr = str(bodyID)
# Create Time Array
observationTimes = np.linspace(start_et, end_et, num=n_observations,
endpoint=True) # range(start_et, end_et, n_observations - 1)
# Create Beacon and Spacecraft states at Observation Times
# stateSpacecraft = []
# earth_states = []
# mars_states = []
# jupiter_states = []
# venus_states = []
# for t in observationTimes:
# earth_states.append(SP.spkezr(targ='3', et=t, ref=ref, abcorr='None', obs='SUN')[0])
# mars_states.append(SP.spkezr(targ='4', et=t, ref=ref, abcorr='None', obs='SUN')[0])
# jupiter_states.append(SP.spkezr(targ='5', et=t, ref=ref, abcorr='None', obs='SUN')[0])
# stateSpacecraft.append(SP.spkezr(targ=bodyIDStr, et=t, ref=ref, abcorr='None', obs='SUN')[0])
# venus_states.append(SP.spkezr(targ='2', et=t, ref=ref, abcorr='None', obs='SUN')[0])
#
# ephemerides = {'spacecraft': np.array(stateSpacecraft).T,
# 'earth': np.array(earth_states).T,
# 'mars': np.array(mars_states).T,
# 'jupiter': np.array(jupiter_states).T,
# 'venus': np.array(venus_states).T
# }
stateSpacecraft = []
for t in observationTimes:
state = pyswice.new_doubleArray(6)
lt = pyswice.new_doubleArray(1)
stateArray = np.zeros(6)
pyswice.spkezr_c(bodyIDStr, t, ref, 'None', 'SUN', state, lt)
for i in range(6):
stateArray[i] = pyswice.doubleArray_getitem(state, i)
stateSpacecraft.append(stateArray)
ephemerides = {'spacecraft': np.array(stateSpacecraft).T}
dyn_ref_state = []
if realData == 'OFF':
IC0 = stateSpacecraft[0]
stateDimension = IC0.shape[0]
phi0 = np.identity(stateDimension)
# input to the propagator takes the ref_state and STM at t0, as well as the list of times
propagationInputs = (IC0, phi0, observationTimes, extras)
# execute propagation
state = runRef(propagationInputs)
for jj in range(len(observationTimes)):
dyn_ref_state.append(state[jj][0:stateDimension])
ephemerides = {'spacecraft': np.array(dyn_ref_state).T}
# for planet in planet_beacons:
# planet = planet.lower()
#
# states = []
# for t in observationTimes:
# stateArray = np.zeros(6)
# state = pyswice.new_doubleArray(6)
# lt = pyswice.new_doubleArray(1)
# pyswice.spkezr_c(bodyIDStr, t, ref, 'None', 'SUN', state, lt)
# for i in range(6):
# stateArray[i] = pyswice.doubleArray_getitem(state, i)
# stateSpacecraft.append(stateArray)
#
# ephemerides[planet] = np.array(states).T
for beaconID in beaconIDs:
beaconState = []
for t in observationTimes:
stateArray = np.zeros(6)
state = pyswice.new_doubleArray(6)
lt = pyswice.new_doubleArray(1)
pyswice.spkezr_c(str(beaconID), t, ref, 'None', 'SUN', state, lt)
for i in range(6):
stateArray[i] = pyswice.doubleArray_getitem(state, i)
beaconState.append(stateArray)
beaconState = np.array(beaconState).T
ephemerides[str(beaconID)] = np.array(beaconState)
return ephemerides, observationTimes
def generate_data(sc_ephem_file,
planet_beacons,
beaconIDs,
n_observations=10,
start_et=None,
end_et=None,
extras={},
realData='OFF',
ref='J2000'):
# Load Ephemeris Files
pyswice.furnsh_c(sc_ephem_file)
pyswice.furnsh_c(bskSpicePath + extras['basic_bsp'])
pyswice.furnsh_c(dinoSpicePath + extras['mission_bsp'])
pyswice.furnsh_c(dinoSpicePath + extras['tls'])
for beacon in planet_beacons:
found = pyswice.new_intArray(1)
pyswice.intArray_setitem(found, 0, 0)
beaconid = pyswice.new_intArray(1)
pyswice.bodn2c_c(beacon, beaconid, found)
beaconIDs.append(pyswice.intArray_getitem(beaconid, 0))
# Identify Spacecraft Body
bodyID = -100 # SP.spkobj(sc_ephem_file)
bodyIDStr = str(bodyID)
# Create Time Array
observationTimes = np.linspace(
start_et, end_et, num=n_observations,
endpoint=True) # range(start_et, end_et, n_observations - 1)
# Create Beacon and Spacecraft states at Observation Times
# stateSpacecraft = []
# earth_states = []
# mars_states = []
# jupiter_states = []
# venus_states = []
# for t in observationTimes:
# earth_states.append(SP.spkezr(targ='3', et=t, ref=ref, abcorr='None', obs='SUN')[0])
# mars_states.append(SP.spkezr(targ='4', et=t, ref=ref, abcorr='None', obs='SUN')[0])
# jupiter_states.append(SP.spkezr(targ='5', et=t, ref=ref, abcorr='None', obs='SUN')[0])
# stateSpacecraft.append(SP.spkezr(targ=bodyIDStr, et=t, ref=ref, abcorr='None', obs='SUN')[0])
# venus_states.append(SP.spkezr(targ='2', et=t, ref=ref, abcorr='None', obs='SUN')[0])
#
# ephemerides = {'spacecraft': np.array(stateSpacecraft).T,
# 'earth': np.array(earth_states).T,
# 'mars': np.array(mars_states).T,
# 'jupiter': np.array(jupiter_states).T,
# 'venus': np.array(venus_states).T
# }
stateSpacecraft = []
for t in observationTimes:
state = pyswice.new_doubleArray(6)
lt = pyswice.new_doubleArray(1)
stateArray = np.zeros(6)
pyswice.spkezr_c(bodyIDStr, t, ref, 'None', 'SUN', state, lt)
for i in range(6):
stateArray[i] = pyswice.doubleArray_getitem(state, i)
stateSpacecraft.append(stateArray)
ephemerides = {'spacecraft': np.array(stateSpacecraft).T}
dyn_ref_state = []
if realData == 'OFF':
IC0 = stateSpacecraft[0]
stateDimension = IC0.shape[0]
phi0 = np.identity(stateDimension)
# input to the propagator takes the ref_state and STM at t0, as well as the list of times
propagationInputs = (IC0, phi0, observationTimes, extras)
# execute propagation
state = runRef(propagationInputs)
for jj in range(len(observationTimes)):
dyn_ref_state.append(state[jj][0:stateDimension])
ephemerides = {'spacecraft': np.array(dyn_ref_state).T}
# for planet in planet_beacons:
# planet = planet.lower()
#
# states = []
# for t in observationTimes:
# stateArray = np.zeros(6)
# state = pyswice.new_doubleArray(6)
# lt = pyswice.new_doubleArray(1)
# pyswice.spkezr_c(bodyIDStr, t, ref, 'None', 'SUN', state, lt)
# for i in range(6):
# stateArray[i] = pyswice.doubleArray_getitem(state, i)
# stateSpacecraft.append(stateArray)
#
# ephemerides[planet] = np.array(states).T
for beaconID in beaconIDs:
beaconState = []
for t in observationTimes:
stateArray = np.zeros(6)
state = pyswice.new_doubleArray(6)
lt = pyswice.new_doubleArray(1)
pyswice.spkezr_c(str(beaconID), t, ref, 'None', 'SUN', state, lt)
for i in range(6):
stateArray[i] = pyswice.doubleArray_getitem(state, i)
beaconState.append(stateArray)
beaconState = np.array(beaconState).T
ephemerides[str(beaconID)] = np.array(beaconState)
return ephemerides, observationTimes