######## Observation model
# Get observation model
if select_obs == RA_DEC:
observationModel = rightAscensionDeclinationObsModel.getObserverModel(theta_0, theta_dot, R_E, e_earth, GS_coord)
obs_used = '_ra_dec'
else:
observationModel = rangeRangeRateObsModel.getObserverModel(theta_0, theta_dot, R_E, e_earth, GS_coord)
obs_used = ''
######## Orbit simulation
J_sim = np.array([0, 0, J_2, J_3]) # Vector with the J params to simulate
orbModelGenerator = zonalHarmonicsModel.getDynamicModel(mu, R_E, J_sim)
orbModel_params = ()
obsGen = observationSimulator.getObservationSimulator(orbModelGenerator, observationModel)
obsGen.addNoise(noise_flag, mean_noise_obs, R_noise_obs)
obsGen.simulate(X_0, orbModel_params, t0, tf, dt, rtol, atol)
(observers, observations) = obsGen.getObservations() # observers: contains the number of observer. observations: contains the observations
obs_time_vec = obsGen.getTimeVector()
obs_states_vec = obsGen.getObservedStates() # Vector with the observed states
dynGen = obsGen.getDynamicSimulator() # all the states.
nmbrObs = obs_time_vec.size
state_list = satellite.stateList.getFromVectors(mu, dynGen.getStatesVector(), dynGen.getTimeVector())
state_time_vec = state_list.getTimeVector()
# J3 acceleration
J3_model = zonalHarmonicsModel.getDynamicModel(mu, R_E, np.array([0,0,0,J_3]), False)
initial_gyro_bias = np.array([0.001, 0.002, -0.001])
Q = np.diag([sigma_ARW, sigma_ARW, sigma_ARW, sigma_RRW, sigma_RRW,
sigma_RRW])**2
####### Observation simulator: MRP Observations
MRPobsModelGenerator = mrpObs.getObserverModel(mrp_obs_rate)
MRPobsModelGenerator.setNoise(True, mean_noise_mrp_obs, R_noise_mrp_obs)
gyroObsModelGenerator = rateGyroObs.getObserverModel(gyro_rate)
gyroObsModelGenerator.setNoise(True, None, None, [
sigma_ARW * np.array([1.0, 1.0, 1.0]),
sigma_RRW * np.array([1.0, 1.0, 1.0]), initial_gyro_bias
])
obsGen = observationSimulator.getObservationSimulator(
attitudeModelGenerator, [MRPobsModelGenerator, gyroObsModelGenerator],
Integrator.RK4, attitudeKinematics.switchMRPrepresentation)
obsGen.simulate(np.concatenate((mrp_BN_0, w_BN_0_B)), (), t0, tf, dt)
(observers, observations) = obsGen.getObservations(
) # observers: contains the number of observer. observations: contains the observations
obs_time = obsGen.getTimeVector()
obs_states = obsGen.getObservedStates() # Vector with the observed states
#dynGen = gyroGen.getDynamicSimulator() # all the states.
obs_time_vec_mrp_obs = obs_time[0]
observations_mrp_obs = observations[0]
obs_true_states_mrp_obs = obs_states[0]
observers_mrp_obs = observers[0]
nmbr_obs_mrp_obs = len(observations_mrp_obs)
if select_obs == RA_DEC:
observationModel = rightAscensionDeclinationObsModel.getObserverModel(
theta_0, theta_dot, R_E, e_earth, GS_coord)
obs_used = '_ra_dec'
else:
observationModel = rangeRangeRateObsModel.getObserverModel(
theta_0, theta_dot, R_E, e_earth, GS_coord)
obs_used = ''
######## Orbit simulation
J_sim = np.array([0, 0, J_2, J_3]) # Vector with the J params to simulate
orbModelGenerator = zonalHarmonicsModel.getDynamicModel(mu, R_E, J_sim)
orbModel_params = ()
obsGen = observationSimulator.getObservationSimulator(orbModelGenerator,
observationModel)
obsGen.addNoise(noise_flag, mean_noise_obs, R_noise_obs)
obsGen.simulate(X_0, orbModel_params, t0, tf, dt, rtol, atol)
(observers, observations) = obsGen.getObservations(
) # observers: contains the number of observer. observations: contains the observations
obs_time_vec = obsGen.getTimeVector()
obs_states_vec = obsGen.getObservedStates() # Vector with the observed states
dynGen = obsGen.getDynamicSimulator() # all the states.
nmbrObs = obs_time_vec.size
state_list = satellite.stateList.getFromVectors(mu, dynGen.getStatesVector(),
dynGen.getTimeVector())
state_time_vec = state_list.getTimeVector()
N_ARW = 0.55 # [deg/sqrt(hour)] K_RRW = 7.0 # [deg/sqrt(hour^3)] sigma_ARW = np.deg2rad(N_ARW/np.sqrt(3600)) # ARW: Angular Random Walk (rate white noise) sigma_RRW = np.deg2rad(K_RRW/np.sqrt(3600**3)) # RRW: Rate Random Walk initial_gyro_bias = np.array([0.001,0.002,-0.001]) Q = np.diag([sigma_ARW, sigma_ARW, sigma_ARW, sigma_RRW, sigma_RRW, sigma_RRW])**2 ####### Observation simulator: MRP Observations MRPobsModelGenerator = mrpObs.getObserverModel(mrp_obs_rate) MRPobsModelGenerator.setNoise(True, mean_noise_mrp_obs, R_noise_mrp_obs) gyroObsModelGenerator = rateGyroObs.getObserverModel(gyro_rate) gyroObsModelGenerator.setNoise(True, None, None, [sigma_ARW*np.array([1.0,1.0,1.0]), sigma_RRW*np.array([1.0,1.0,1.0]), initial_gyro_bias]) obsGen = observationSimulator.getObservationSimulator(attitudeModelGenerator, [MRPobsModelGenerator,gyroObsModelGenerator], Integrator.RK4, attitudeKinematics.switchMRPrepresentation) obsGen.simulate(np.concatenate((mrp_BN_0, w_BN_0_B)), (), t0, tf, dt) (observers, observations) = obsGen.getObservations() # observers: contains the number of observer. observations: contains the observations obs_time = obsGen.getTimeVector() obs_states = obsGen.getObservedStates() # Vector with the observed states #dynGen = gyroGen.getDynamicSimulator() # all the states. obs_time_vec_mrp_obs = obs_time[0] observations_mrp_obs = observations[0] obs_true_states_mrp_obs = obs_states[0] observers_mrp_obs = observers[0] nmbr_obs_mrp_obs = len(observations_mrp_obs) obs_time_vec_GYRO = obs_time[1]
