def test_data2(self): qOB = (0.5/np.sqrt(2.))*np.array([np.sqrt(3.),np.sqrt(3.),1.,1.]) r = np.array([0.,0.,7.07e6]) v = np.array([0.,7.0e3,0.]) qIB = 0.5 * np.array([0,-1,0,np.sqrt(3)]) result = fr.qBO2qBI(qnv.quatInv(qOB),r,v) self.assertTrue(np.allclose(qnv.quatInv(qIB) ,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.]) qBO = np.array([1.,0.,0.,0.]) result = fr.qBO2qBI(qBO,r,v) self.assertTrue(np.allclose(qBI ,result))
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_data3(self): r = 1/3 * np.array([7.07e6,7.07e6,7.07e6]) v = np.array([0.,1/2**(1/2),1/2**(1/2)]) qOB = (0.5/np.sqrt(2.))*np.array([np.sqrt(3.),np.sqrt(3.),1.,1.]) qBI = np.array([ -0.32227667, 0.74698487, 0.43819357, 0.38227967]) #m_OI = -1 * np.array(((2**(1/2)/9, -1/(9*2**(1/2)), -1/(9*2**(1/2))),(0, -1/(3*2**(1/2)), 1/(3*2**(1/2))),(1/3**(1/2), 1/3**(1/2), 1/3**(1/2)))) #q_OI = qnv.quatMultiplyNorm(qnv.quatInv(qOB),qnv.rotm2quat(m_A)) = [-0.1159169 -0.88047624 0.3647052 0.27984814] result = fr.qBO2qBI(qnv.quatInv(qOB),r,v) print(result) self.assertTrue(np.allclose(qBI ,result))
def test_data2(self):
expected = 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.])
v_q_BO = (0.5 / np.sqrt(2.)) * np.array(
[np.sqrt(3.), np.sqrt(3.), 1., 1.])
result = fr.qBO2qBI(v_q_BO, r, v)
self.assertTrue(np.allclose(expected, result))
def getQ_BI(self): #get exact quaternion from inertial frame to body frame v_q_BI = fs.qBO2qBI(self.v_state[0:4],self.v_pos_i,self.v_vel_i) return v_q_BI
m_light_output = m_light_output_temp[init:(init + N), :].copy()
print N, 'Simulation for ', MODEL_STEP * (N - 1), 'seconds'
#Initial conditions
v_q0_BO = np.array([1., 0., 0., 0.])
r = np.linalg.norm(m_sgp_output_i[0, 1:4])
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: