def test_constant(self): t0 = 0.0 mySat = satellite.Satellite(np.array([1.0,0.,0.,0.,0.,0.,0.]),t0) t = 10 mySat.setPos(np.array([1e6,0.,0.])) mySat.setVel(np.array([5.0,5.0,0.0])) h = 0.1 for i in range(0,int(t/h)): rk4Quaternion(mySat,f_constant,h) mySat.setTime(t0+(i+1)*h) self.assertTrue(np.allclose([10.0,10.0,10.0],mySat.getW_BO_b()))
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))
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()))
sat.setMag_i(Mag_i) sat.setPos(np.array([1, 1, 1])) sat.setVel(np.array([1, 1, 2])) voltageRequired = TorqueApplied.ctrlTorqueToVoltage(sat) duty_cycle = voltageRequired / 3.3 n = 15 min_duty = np.abs(np.amin(duty_cycle)) max_duty = np.abs(np.amax(duty_cycle)) mid_duty = duty_cycle[0] + duty_cycle[1] + duty_cycle[3] - min_duty - max_duty denom_t = 1.0 / min_duty + 1.0 / max_duty + 1.0 / mid_duty + 10 time_arr_a = np.linspace(min_duty / PWM_FREQUENCY) current_applied = np.zeros((3, 3)) edgeCurrentList = aac.getEdgeCurrent(duty_cycle, np.zeros(3)) state_array = np.zeros((100001, 7)) start_1 = timer.time() for i in range(0, CONTROL_STEP * PWM_FREQUENCY * 15): current_applied = aac.getCurrentList( duty_cycle, np.linspace(time[i], time[i + 1], 3, endpoint=True), 3, np.zeros(3)) torque_applied = TorqueApplied.currentToTorque(current_applied, sat) solver.rk4Quaternion(sat, dynamics_actuator.x_dot_BO, time[i + 1] - time[i], torque_applied) state_array[i, :] = sat.getState() if (i % 100 == 0): print(i / 100, "%") end_1 = timer.time() print(end_1 - start_1) np.savetxt("aac_test.csv", state_array[:, :], delimiter=",") end = timer.time() print(start - end)
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() #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
#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(CONTROL_STEP/MODEL_STEP)+k,:] = Advitiy.getggDisturbance_b() torque_dist_aero[i*int(CONTROL_STEP/MODEL_STEP)+k,:] = Advitiy.getaeroDisturbance_b() torque_dist_solar[i*int(CONTROL_STEP/MODEL_STEP)+k,:] = Advitiy.getsolarDisturbance_b() torque_dist_total[i*int(CONTROL_STEP/MODEL_STEP)+k,:] = torque_dist_gg[i*int(CONTROL_STEP/MODEL_STEP)+k,:] + torque_dist_aero[i*int(CONTROL_STEP/MODEL_STEP)+k,:] + torque_dist_solar[i*int(CONTROL_STEP/MODEL_STEP)+k,:] Advitiy.setDisturbance_b(torque_dist_total[i*int(CONTROL_STEP/MODEL_STEP)+k,:].copy()) #Use rk4 solver to calculate the state for next step sol.rk4Quaternion(Advitiy,x_dot_BO,h) #storing data in matrices v_state[i*int(CONTROL_STEP/MODEL_STEP)+k+1,:] = Advitiy.getState() euler[i*int(CONTROL_STEP/MODEL_STEP)+k+1,:] = qnv.quat2euler(Advitiy.getQ_BO()) #sensor reading if (sensbool == 0): #getting default sensor reading (zero noise in our case) Advitiy.setSun_b_m(defblock.sunsensor(Advitiy)) Advitiy.setMag_b_m_p(Advitiy.getMag_b_m_c()) Advitiy.setMag_b_m_c(defblock.magnetometer(Advitiy)) Advitiy.setgpsData(defblock.gps(Advitiy)) Advitiy.setOmega_m(defblock.gyroscope(Advitiy)) Advitiy.setJ2Data(defblock.J2_propagator(Advitiy)) if (sensbool == 1):