def test_appr_actTypeB(self, value):
torque_req = np.asarray(value[0])
magfield_b = np.asarray(value[1])
app_torque_exp = np.asarray(value[2])
vel = np.array([5, 6, 7])
pos = np.array([3, 4, 5])
q_BI = np.array([0.5, -0.5, 0.5, -0.5])
q_IB = quatInv(q_BI)
q_BO = qBI2qBO(q_BI, pos, vel)
magfield_i = quatRotate(q_IB, magfield_b)
w_BO_b = np.array([8, 6, 7])
state = np.hstack((q_BO, w_BO_b))
t0 = 0
s = sat.Satellite(state, t0)
s.setVel(vel)
s.setPos(pos)
s.setMag_i(magfield_i)
s.setControl_b(torque_req)
app_torque = act.actuatorTypeB(s)
self.assertTrue(np.allclose(app_torque, app_torque_exp))
def test_data(self): qIB = 0.5 * np.array([0,-1,0,np.sqrt(3)]) r = np.array([0.,0.,7.07e6]) v = np.array([0.,7.0e3,0.]) expectedInv = (0.5/np.sqrt(2.))*np.array([np.sqrt(3.),np.sqrt(3.),1.,1.]) result = fr.qBI2qBO(qnv.quatInv(qIB),r,v) self.assertTrue(np.allclose(qnv.quatInv(expectedInv) ,result))
def test_south_pole_ideal(self):
#above south pole, body frame, orbit frame and eci frame have same attitude wrt each other
r = np.array([0., 0., -7.07e6])
v = np.array([7.0e3, 0., 0.])
qBI = np.array([1., 0., 0., 0.])
expected = np.array([1., 0., 0., 0.])
result = fr.qBI2qBO(qBI, r, v)
self.assertTrue(np.allclose(expected, result))
def test_magmeter(self, value):
qBI = np.asarray(value[0])
v_sv_i = np.asarray(value[1])
v_pos_i = np.array([1e6, -2.03, -3.0])
v_vel_i = np.array([2.0e3, 2.8, -73.2])
qBO = frames.qBI2qBO(qBI, v_pos_i, v_vel_i)
state = np.hstack((qBO, np.zeros(3)))
mySat = satellite.Satellite(state, 12.0)
mySat.setQ_BI(qBI)
result = defblock.magmeter(mySat, v_sv_i)
v_expected = value[2], value[3]
self.assertTrue(np.allclose(result, v_expected))
def test_gg_data_type(self): qBI = np.array([0.,0.,0.,1.]) wBIb = np.array([0.,0.,0.]) v_pos_i = np.array([7070e3,0.,0.]) v_vel_i = np.array([2.0e3,2.8,-73.2]) 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([7070e3,0.,0.])) sat.setQ_BI(qBI) dist.ggTorqueb(sat) result = sat.getggDisturbance_b() self.assertEqual(type(result),np.ndarray)
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_inertia_eigenvec(self, value):
qBI = np.array([0., 0., 0., 1.])
wBIb = np.array([0.1, -0.02, -0.2])
v_pos_i = value
v_vel_i = np.array([5.60, -5.0, 0.0])
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(value)
sat.setVel(v_vel_i)
dist.ggTorqueb(sat)
result = sat.getggDisturbance_b()
self.assertTrue(np.allclose(result, [0., 0., 0.]))
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]))
def test_magmeter(self,value): qBI = np.asarray(value[0]) v_B_i=np.asarray(value[1]) v_pos_i = np.array([1e6,-2.03,-3.0]) v_vel_i = np.array([2.0e3,2.8,-73.2]) qBO = frames.qBI2qBO(qBI,v_pos_i,v_vel_i) state = np.hstack((qBO,np.zeros(3))) mySat = satellite.Satellite(state,12.0) mySat.setMag_i(v_B_i) mySat.setPos(v_pos_i) mySat.setVel(v_vel_i) qOI = qnv.quatMultiplyNorm(qnv.quatInv(qBO),qBI) result = defblock.magnetometer(mySat) v_expected = value[2]; self.assertTrue(np.allclose(result,v_expected))
def test_data(self):
qBI = np.array([
0.5 * np.sqrt(1 + 0.5 * np.sqrt(3.)),
-0.5 * np.sqrt(1 + 0.5 * np.sqrt(3.)),
-0.25 * np.sqrt(2 / (2 + np.sqrt(3))),
0.25 * np.sqrt(2 / (2 + np.sqrt(3)))
])
r = np.array([0., 0., 7.07e6])
v = np.array([0., 7.0e3, 0.])
expected = (0.5 / np.sqrt(2.)) * np.array(
[np.sqrt(3.), np.sqrt(3.), 1., 1.])
result = fr.qBI2qBO(qBI, r, v)
self.assertTrue(np.allclose(expected, result))
def test_solar_torque_value(self): qBI = np.array([0.,0.,0.,1.]) wBIb = np.array([0.1,-0.02,-0.2]) v_pos_i = np.array([7070e3,0.,0.]) 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) v_sv_i=np.array([1.0,0.0,0.0]) #sun vector in eci frame sat.setSun_i(v_sv_i) sat.setLight(1) dist.solarTorqueb(sat) result = sat.getsolarDisturbance_b() self.assertTrue(np.allclose(result,[ 0.00000000e+00,-3.66624000e-11,3.17376000e-10]))
def test_solar_torque_type(self): qBI = np.array([0.,0.,0.,1.]) wBIb = np.array([0.,0.,0.]) v_pos_i = np.array([7070e3,0.,0.]) 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([7070e3,0.,0.])) v_sv_i=np.array([1.0,0.0,0.0]) sat.setSun_i(v_sv_i) sat.setLight(1) dist.solarTorqueb(sat) result = sat.getsolarDisturbance_b() self.assertEqual(type(result),np.ndarray)
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()
#Calculate observable quantities
m_q_BO[i + 1, :] = fs.qBI2qBO(v_state_next[0:4], m_sgp_output_i[i + 1,
1:4],
m_sgp_output_i[i + 1, 4:7])
m_w_BO_b[i + 1, :] = fs.wBIb2wBOb(v_state_next[4:7], m_q_BO[i + 1, :],
v_w_IO_o)
m_euler_BO[i + 1, :] = qnv.quat2euler(m_q_BO[i + 1, :])
#save the data files
os.chdir('Logs/')
os.mkdir('polar-identity-no-dist')
os.chdir('polar-identity-no-dist')
np.savetxt('position.csv', m_sgp_output_i[:, 1:4], delimiter=",")
np.savetxt('velocity.csv', m_sgp_output_i[:, 4:7], delimiter=",")
np.savetxt('time.csv', m_sgp_output_i[:, 0] - t0, delimiter=",")
np.savetxt('w_BOB.csv', m_w_BO_b, delimiter=",")
np.savetxt('q_BO.csv', m_q_BO, delimiter=",")
