def test_aero_type(self):
qBI = np.array([-np.sqrt(0.5), 0., 0., np.sqrt(0.5)])
wBIb = np.array([0.1, 0.23, 0.])
v_pos_i = np.array([0., 0., 7e6])
v_vel_i = np.array([0, 2e3, 6e3])
qBO = fs.qBI2qBO(qBI, v_pos_i, v_vel_i)
wBOB = fs.wBIb2wBOb(wBIb, qBO, v_w_IO_o)
sat = Satellite(np.hstack((qBO, wBOB)), 13.)
sat.setPos(np.array([0., 0., 7e6]))
sat.setVel(np.array([0, 2e3, 6e3]))
dist.aeroTorqueb(sat)
result = sat.getaeroDisturbance_b()
self.assertEqual(type(result), np.ndarray)
def test_aero_value(self): qBI = np.array([-np.sqrt(0.5),0.,0.,np.sqrt(0.5)]) wBIb = np.array([0.1,0.23,0.]) v_pos_i = np.array([0.,0.,7e6]) v_vel_i = np.array([0,2e3,6e3]) qBO = fs.qBI2qBO(qBI,v_pos_i,v_vel_i) wBOB = fs.wBIb2wBOb(wBIb,qBO,v_w_IO_o) sat = Satellite(np.hstack((qBO,wBOB)),13.) sat.setQ_BI(qBI) sat.setPos(np.array([0.,0.,7e6])) sat.setVel(np.array([0,2e3,6e3])) dist.aeroTorqueb(sat) result = sat.getaeroDisturbance_b() self.assertTrue(np.allclose(result, [2.99654080e-10,-2.57065600e-11,-7.71196800e-11]))
Advitiy.setVel(m_sgp_output_i[i * int(Nmodel / Ncontrol) + k, 4:7])
Advitiy.setLight(m_light_output[i * int(Nmodel / Ncontrol) + k, 1])
Advitiy.setSun_i(m_si_output_b[i * int(Nmodel / Ncontrol) + k,
1:4])
Advitiy.setMag_i(
m_magnetic_field_i[(i + 1) * int(Nmodel / Ncontrol) + k, 1:4])
# disturbance torque
if (distbool == 0):
# getting default disturbance torque (zero in our case)
Advitiy.setDisturbance_b(defblock.disturbance(Advitiy))
if (distbool == 1):
# getting disturbance torque by disturbance model
dist.ggTorqueb(Advitiy)
dist.aeroTorqueb(Advitiy)
dist.solarTorqueb(Advitiy)
torque_dist_gg[i * int(Nmodel / Ncontrol) +
k, :] = Advitiy.getggDisturbance_b()
torque_dist_aero[i * int(Nmodel / Ncontrol) +
k, :] = Advitiy.getaeroDisturbance_b()
torque_dist_solar[i * int(Nmodel / Ncontrol) +
k, :] = Advitiy.getsolarDisturbance_b()
torque_dist_total[
i * int(Nmodel / Ncontrol) +
k, :] = torque_dist_gg[i * int(Nmodel / Ncontrol) +
k, :] + torque_dist_aero[
i * int(Nmodel / Ncontrol) +
k, :] + torque_dist_solar[
i * int(Nmodel / Ncontrol) +
#-------------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, :])
Advitiy.setTime(t0 + i * MODEL_STEP)
Advitiy.setPos(m_sgp_output_i[i, 1:4])
Advitiy.setVel(m_sgp_output_i[i, 4:7])
Advitiy.setSun_i(m_si_output_i[i, 1:4])
#calculate the disturbance torques
v_torque_gg_b = dist.ggTorqueb(Advitiy).copy()
v_torque_aero_b = dist.aeroTorqueb(Advitiy).copy()
v_torque_solar_b = dist.solarTorqueb(Advitiy).copy()
v_torque_total_b = (v_torque_gg_b + v_torque_aero_b + v_torque_solar_b)
Advitiy.setDisturbance_b(v_torque_total_b)
m_torque_dist[i, :] = v_torque_total_b.copy()
v_state_next = np.zeros((1, 7))
#Use rk4 solver to calculate the state for next step
for j in range(0, int(MODEL_STEP / h)):
v_state_next = sol.rk4Quaternion(Advitiy, x_dot, h)
Advitiy.setState(v_state_next.copy())
Advitiy.setTime(t0 + i * MODEL_STEP + (j + 1) * h)
m_state[i + 1, :] = v_state_next.copy()
