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):
