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 # =============================================================================