def test_stationary_body(self, value):
v_w_IO_o = np.array([0., 0.00106178, 0.])
qBO = np.asarray(value)
w_BIB = np.array([0., 0., 0.])
result = fr.wBIb2wBOb(w_BIB, qBO, v_w_IO_o)
expected = qnv.quatRotate(qBO, v_w_IO_o)
self.assertTrue(np.allclose(result, expected))
def test_ideal_controlled(self):
v_w_IO_o = np.array([0., 0.00106178, 0.])
qBO = np.array([1., 0., 0., 0.])
w_BIB = -qnv.quatRotate(qBO, v_w_IO_o)
w_BOB = fr.wBIb2wBOb(w_BIB, qBO, v_w_IO_o)
expected = np.array([0., 0., 0.])
self.assertTrue(np.allclose(w_BOB, 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_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)
def test_dynamics(self):
t0 = 0.
h = 0.001
v_q_BO = np.array([0.4, 0.254, -0.508, 0.71931912])
v_q_BI = frames.qBO2qBI(v_q_BO, np.array([1e6, 53e5, 0.]),
np.array([5.60, -5.0, 0.0]))
mySat = satellite.Satellite(
np.hstack((v_q_BI, np.array([0.1, -0.05, -0.3]))), t0)
mySat.setPos(np.array([1e6, 53e5, 0.]))
mySat.setVel(np.array([5.60, -5.0, 0.0]))
mySat.setDisturbance_b(np.array([10e-10, -4e-6, -3e-5]))
mySat.setControl_b(np.array([1e-5, 1e-5, -8e-4]))
r = np.linalg.norm(mySat.getPos())
v_w_IO_o = np.array([
0., np.sqrt(G * M_EARTH / (r)**3), 0.
]) #angular velocity of orbit frame wrt inertial frame in orbit frame
v_w_BO_b = frames.wBIb2wBOb(np.array([0.1, -0.05, -0.3]), v_q_BO,
v_w_IO_o)
x1 = rk4Quaternion(mySat, x_dot_BO, h)
mySat.setState(x1.copy())
mySat.setTime(t0 + h)
k1 = h * x_dot_BO(mySat, t0 + 0.5 * h, np.hstack((v_q_BO, v_w_BO_b)))
k2 = h * x_dot_BO(mySat, t0 + 0.5 * h,
np.hstack((v_q_BO, v_w_BO_b)) + 0.5 * k1)
k3 = h * x_dot_BO(mySat, t0 + 0.5 * h,
np.hstack((v_q_BO, v_w_BO_b)) + 0.5 * k2)
k4 = h * x_dot_BO(mySat, t0 + h, np.hstack((v_q_BO, v_w_BO_b)) + k3)
error_state = np.hstack(
(v_q_BO, v_w_BO_b)) + (1. / 6.) * (k1 + 2. * k2 + 2. * k3 + k4)
expected = np.hstack((frames.qBO2qBI(error_state[0:4],
np.array([1e6, 53e5, 0.]),
np.array([5.60, -5.0, 0.0])),
frames.wBOb2wBIb(error_state[4:7],
error_state[0:4], v_w_IO_o)))
self.assertTrue(np.allclose(expected, mySat.getState()))
def test_zero_torque_rates_ideal_q(self): #zero torque, zero initial wBIB, aligned frames ''' For this test case set (from sixth model of Advitiy) and ALTITUDE = 700 km Ixx = 0.00152529 Iyy = 0.00145111 Izz = 0.001476 Ixy = 0.00000437 Iyz = - 0.00000408 Ixz = 0.00000118 ''' qBO = np.array([0.0,0.,0.0,1.]) vel = np.array([5.0,5.0,0.0]) v_w_BI_b = np.array([0.,0.,0.]) v_w_BO_b = frames.wBIb2wBOb(v_w_BI_b,qBO,v_w_IO_o) state = np.hstack((qBO,v_w_BO_b)) mySat = satellite.Satellite(state,12.0) mySat.setVel(vel) mySat.setDisturbance_b(np.array([0.,0.,0.])) mySat.setControl_b(np.array([0.,0.,0.])) result = x_dot_BO(mySat) self.assertTrue(np.allclose(result,[0.,0.00053089,0.,0.,0.,0.,0.]))
def test_dynamics(self,value): t0 = 0. h = value v_q_BO = np.array([0.4,0.254,-0.508,0.71931912]) v_w_BO_b = frames.wBIb2wBOb(np.array([0.1,-0.05,-0.3]),v_q_BO,v_w_IO_o) mySat1 = satellite.Satellite(np.hstack((v_q_BO, v_w_BO_b)),t0) mySat1.setPos(np.array([1e6,53e5,0.])) mySat1.setVel(np.array([5.60,-5.0,0.0])) mySat1.setDisturbance_b(np.array([10e-10,-4e-6,-3e-5])) mySat1.setControl_b(np.array([1e-5,1e-5,-8e-4])) updateStateTimeRK4(mySat1,x_dot_BO,h) x1 = mySat1.getState() mySat2 = satellite.Satellite(np.hstack((v_q_BO, v_w_BO_b)),t0) mySat2.setPos(np.array([1e6,53e5,0.])) mySat2.setVel(np.array([5.60,-5.0,0.0])) mySat2.setDisturbance_b(np.array([10e-10,-4e-6,-3e-5])) mySat2.setControl_b(np.array([1e-5,1e-5,-8e-4])) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))) k1 = h*x_dot_BO(mySat2) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))+0.5*k1) k2 = h*x_dot_BO(mySat2) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))+0.5*(k2)) k3 = h*x_dot_BO(mySat2) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))+k3) k4 = h*x_dot_BO(mySat2) error_state = np.hstack((v_q_BO,v_w_BO_b)) + (1./6.)*(k1 + 2.*k2 + 2.*k3 + k4) if (error_state[3]<0): error_state[0:4]=-1*error_state[0:4] error_state[0:4]=error_state[0:4]/np.linalg.norm(error_state[0:4]) self.assertTrue(np.allclose(error_state,x1))
def test_dynamics(self): t0 = 0. h = 0.001 v_q_BO = np.array([0.4,0.254,-0.508,0.71931912]) v_w_BO_b = frames.wBIb2wBOb(np.array([0.1,-0.05,-0.3]),v_q_BO,v_w_IO_o) mySat1 = satellite.Satellite(np.hstack((v_q_BO, v_w_BO_b)),t0) mySat1.setPos(np.array([1e6,53e5,0.])) mySat1.setVel(np.array([5.60,-5.0,0.0])) mySat1.setDisturbance_b(np.array([10e-10,-4e-6,-3e-5])) mySat1.setControl_b(np.array([1e-5,1e-5,-8e-4])) rk4Quaternion(mySat1,x_dot_BO,h) x1 = mySat1.getState() mySat2 = satellite.Satellite(np.hstack((v_q_BO, v_w_BO_b)),t0) mySat2.setPos(np.array([1e6,53e5,0.])) mySat2.setVel(np.array([5.60,-5.0,0.0])) mySat2.setDisturbance_b(np.array([10e-10,-4e-6,-3e-5])) mySat2.setControl_b(np.array([1e-5,1e-5,-8e-4])) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))) k1 = h*x_dot_BO(mySat2) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))+0.5*k1) k2 = h*x_dot_BO(mySat2) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))+0.5*(k2+k1)) k3 = h*x_dot_BO(mySat2) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))+k3+0.5*(k2+k1)) k4 = h*x_dot_BO(mySat2) error_state = np.hstack((v_q_BO,v_w_BO_b)) + (1./6.)*(k1 + 2.*k2 + 2.*k3 + k4) print(error_state) self.assertTrue(np.allclose(error_state,x1))
v_w_IO_o = np.array([0., np.sqrt(G * M_EARTH / (r)**3), 0.])
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])
#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
# =============================================================================
