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