def test_quat2euler(self, value):
q = np.asarray(value[0])
expected = np.asarray(value[1])
result = qnv.quat2euler(q)
self.assertTrue(np.allclose(result, expected))
def test_zero_euler(self):
q = np.array([0., 0., 0., 1.0])
expected = np.array([0., 0., 0.])
result = qnv.quat2euler(q)
self.assertTrue(np.allclose(result, expected))
# initialize empty matrices which will be needed in this simulation
m_state = np.zeros((Nmodel + 1, 7))
m_euler = np.zeros((Nmodel + 1, 3))
m_w_BI_b = np.zeros((Nmodel, 3))
torque_dist_total = np.zeros((Nmodel, 3))
torque_dist_gg = np.zeros((Nmodel, 3))
torque_dist_aero = np.zeros((Nmodel, 3))
torque_dist_solar = np.zeros((Nmodel, 3))
torque_control = np.zeros((Nmodel, 3))
# defining initial conditions
# initial state based on initial qBO and wBOB
# perfectly aligned body frame and orbit frame (v_q0_BO is initial value defined in constants)
# Body frame is not rotating wrt orbit frame (v_w0_BOB is initial value defined in constants)
m_state[0, :] = np.hstack((v_q0_BO, v_w0_BOB))
m_euler[0, :] = qnv.quat2euler(v_q0_BO) # finding initial euler angles
# Make satellite object
Advitiy = satellite.Satellite(m_state[0, :],
t0) # t0 from line 42 of main_code
Advitiy.setPos(m_sgp_output_i[0, 1:4])
Advitiy.setVel(m_sgp_output_i[0, 4:7])
Advitiy.setLight(m_light_output[0, 1])
Advitiy.setTime(t0) # time at a cycle
Advitiy.setSun_i(m_si_output_b[0, 1:4])
Advitiy.setMag_i(m_magnetic_field_i[0, 1:4])
Advitiy.setMag_b_m_c(defblock.magnetometer(Advitiy))
time_gps = 10 # min This time must be an interal multiple of CONTROL_STEP
time_J2 = 10 # min This time must be an interal multiple of CONTROL_STEP
v_w0_BI_b = -v_w_IO_o.copy()
#initialize empty matrices
m_state = np.zeros((N, 7))
m_q_BO = np.zeros((N, 4))
m_w_BO_b = np.zeros((N, 3))
m_euler_BO = np.zeros((N, 3))
m_torque_dist = np.zeros((N, 3))
v_q0_BI = fs.qBO2qBI(v_q0_BO, m_sgp_output_i[0, 1:4], m_sgp_output_i[0, 4:7])
r = np.linalg.norm(m_sgp_output_i[0, 1:4])
m_state[0, :] = np.hstack((v_q0_BI, v_w0_BI_b))
m_q_BO[0, :] = v_q0_BO.copy()
m_w_BO_b[0, :] = fs.wBIb2wBOb(v_w0_BI_b, m_q_BO[0, :], v_w_IO_o)
m_euler_BO[0, :] = qnv.quat2euler(m_q_BO[0, :])
#Make satellite object
Advitiy = satellite.Satellite(m_state[0, :], t0)
Advitiy.setControl_b(np.array([0., 0., 0.])) #uncontrolled satellite
Advitiy.setDisturbance_b(np.array([0., 0., 0.]))
#-------------Main for loop---------------------
for i in range(0, N - 1):
if math.fmod(i, N / 100) == 0 and i > 5:
print 100 * i / N
#Set satellite parameters
Advitiy.setLight(m_light_output[i, 1])
Advitiy.setState(m_state[i, :])
#initialize empty matrices which will be needed in this simulation v_state = np.zeros((Nmodel,7)) euler = np.zeros((Nmodel,3)) torque_dist_total = np.zeros((Nmodel,3)) torque_dist_gg = np.zeros((Nmodel,3)) torque_dist_aero = np.zeros((Nmodel,3)) torque_dist_solar = np.zeros((Nmodel,3)) torque_control = np.zeros((Nmodel,3)) #defining initial conditions #initial state based on initial qBO and wBOB #perfectly aligned body frame and orbit frame (v_q0_BO is initial value defined in constants) #Body frame is not rotating wrt orbit frame (v_w0_BOB is initial value defined in constants) v_state[0,:] = np.hstack((v_q0_BO,v_w0_BOB)) euler[0,:] = qnv.quat2euler(v_q0_BO) #finding initial euler angles #Make satellite object Advitiy = satellite.Satellite(v_state[0,:],t0) #t0 from line 42 of main_code #initializing the controlTorque and measured magnetic field of satellite object as they are called in for loop before they can be set Advitiy.setControl_b(np.array([0.,0.,0.])) Advitiy.setMag_b_m_c(m_magnetic_field_i[0,:]) #-------------Main for loop--------------------- for i in range(0,N-1): #loop for control-cycle if math.fmod(i,int(Ncontrol/100)) == 0: #we are printing percentage of cycle completed to keep track of simulation print (int(100*i/Ncontrol)) for k in range (0,int(CONTROL_STEP/MODEL_STEP)+1): #loop for environment-cycle
#initialize empty matrices
v_state = np.zeros((N, 7))
v_q_BO = np.zeros((N, 4))
v_w_BOB = np.zeros((N, 3))
euler = np.zeros((N, 3))
torque_dist = np.zeros((N, 3))
v_current_p = np.zeros(3)
v_current_req = np.zeros(3)
v_q0_BI = fs.qBO_2_qBI(v_q0_BO, m_sgp_output_i[0, 1:4], m_sgp_output_i[0, 4:7])
r = np.linalg.norm(m_sgp_output_i[0, 1:4])
v_w0_BIB = -np.array([0., np.sqrt(G * M_EARTH / (r)**3), 0.])
v_state[0, :] = np.hstack((v_q0_BI, v_w0_BIB))
v_q_BO[0, :] = v_q0_BO
v_w_BOB[0, :] = fs.wBIb2wBOb(v_w0_BIB, v_q_BO[0, :], (-v_w0_BIB))
euler[0, :] = qnv.quat2euler(v_q_BO[0, :])
#Make satellite object
Advitiy = satellite.Satellite(v_state[0, :], t0)
Advitiy.setControl_b(np.array([0., 0., 0.]))
Advitiy.setDisturbance_i(np.array([0., 0., 0.]))
#-------------Main for loop---------------------
for i in range(0, N - 1):
print(i)
# =============================================================================
# if math.fmod(i,int(N/100)) == 0:
# print (int(100*i/N))
# if i==600 : break
# =============================================================================
#Set satellite parameters