def open_loop(self, current_att_state, current_jd):
# Current state
self.mpc_current_quaternion_i2b = current_att_state[0]
self.mpc_current_omega_b = current_att_state[1]
self.mpc_current_torque_b = current_att_state[2]
self.mpc_start_jd = current_jd
self.mpc_main_count_time = 0
last_omega_b = self.mpc_current_omega_b
last_quaternion_q_i2b = self.mpc_current_quaternion_i2b
last_torque_b = self.mpc_current_torque_b
last_pos_i, last_vel_i = self.mpc_dynamics_orb.update_state_orbit(current_jd)
last_sideral = gstime(current_jd)
last_tar_pos_eci = rotationZ(self.tar_pos_ecef, last_sideral)
last_rel_vector = last_tar_pos_eci - last_pos_i
for i in range(self.N_pred_horiz):
self.mpc_main_count_time += self.mpc_sim_step_prop
self.mpc_future_jd = self.mpc_start_jd + self.mpc_main_count_time * inv_sec_day
mpc_decyear = JdToDecyear(self.mpc_future_jd)
# Dynamics update
current_q_i2b, current_omega_b = self.mpc_update_attitude(last_omega_b,
last_quaternion_q_i2b,
last_torque_b)
current_position_i, current_velocity_i = self.mpc_dynamics_orb.update_state_orbit(self.mpc_future_jd)
current_sideral = gstime(self.mpc_future_jd)
current_tar_pos_eci = rotationZ(self.tar_pos_ecef, last_sideral)
# ECI to Geodetic state
alt, long, lat = eci_to_geodetic(current_position_i, current_sideral)
# Get Earth magnetic field
mag_b = self.get_mag_earth_b(mpc_decyear, alt, long, lat, current_q_i2b, current_sideral)
#print(mag_b, ', Paso:', i + 1)
# Control
current_tar_pos_b = current_q_i2b.frame_conv(current_tar_pos_eci)
current_torque_b = np.zeros(3)
# Save last data
last_omega_b = current_omega_b
last_quaternion_q_i2b = current_q_i2b
last_torque_b = current_torque_b
last_tar_pos_eci = current_tar_pos_eci
return
def get_omega_tar(self, current_jd, current_position_i, current_velocity_i,
current_q_i2b):
# omega_target_b error
sideral = gstime(current_jd)
tar_pos_i = rotationZ(self.tar_pos_ecef, -sideral)
vel_gs_i = np.cross(earth_rot_i, tar_pos_i)
vel_gs_sc = vel_gs_i - current_velocity_i
pos_gs_sc = tar_pos_i - current_position_i
mag_omega_gs_sc = np.linalg.norm(vel_gs_sc) / np.linalg.norm(pos_gs_sc)
unit_vec_pos = pos_gs_sc / np.linalg.norm(pos_gs_sc)
unit_vec_vel = vel_gs_sc / np.linalg.norm(vel_gs_sc)
unit_vec_omega_gs_sc = np.cross(unit_vec_pos, unit_vec_vel)
omega_gs_from_sc_i = mag_omega_gs_sc * unit_vec_omega_gs_sc
return current_q_i2b.frame_conv(omega_gs_from_sc_i)
def open_loop(self, current_att_state, current_jd):
# Current state
self.mpc_current_quaternion_i2b = current_att_state[0]
self.mpc_current_omega_b = current_att_state[1]
self.mpc_current_torque_b = current_att_state[2]
self.mpc_start_jd = current_jd
self.mpc_main_count_time = 0
# Arrays of independent variables of control
self.mpc_array_jd = self.mpc_start_jd + np.arange(
0, self.N_pred_horiz) * self.mpc_sim_step_prop * inv_sec_day
temp = [
self.mpc_dynamics_orb.update_state_orbit(elem)
for elem in self.mpc_array_jd
]
self.mpc_array_sc_pos_i = [
temp[i][0] for i in range(self.N_pred_horiz)
]
self.mpc_array_sc_vel_i = [
temp[i][1] for i in range(self.N_pred_horiz)
]
self.mpc_array_sideral = [gstime(elem) for elem in self.mpc_array_jd]
self.mpc_array_tar_pos_i = [
rotationZ(self.tar_pos_ecef, -k_sideral)
for k_sideral in self.mpc_array_sideral
]
self.mpc_array_tar_pos_sc_i = [
self.mpc_array_tar_pos_i[i] - self.mpc_array_sc_pos_i[i]
for i in range(self.N_pred_horiz)
]
temp = [
eci_to_geodetic(self.mpc_array_sc_pos_i[i],
self.mpc_array_sideral[i])
for i in range(self.N_pred_horiz)
]
self.mpc_array_alt = [temp[i][0] for i in range(self.N_pred_horiz)]
self.mpc_array_lon = [temp[i][1] for i in range(self.N_pred_horiz)]
self.mpc_array_lat = [temp[i][2] for i in range(self.N_pred_horiz)]
# Get Earth magnetic field
self.mpc_array_decyear = [
JdToDecyear(elem) for elem in self.mpc_array_jd
]
self.mpc_array_mag_i = [
self.get_mag_earth_ned(self.mpc_array_decyear[i],
self.mpc_array_alt[i],
self.mpc_array_lon[i],
self.mpc_array_lat[i],
self.mpc_array_sideral[i])
for i in range(self.N_pred_horiz)
]
"""
x0 = np.zeros(self.N_pred_horiz-1)
xl = np.zeros(3*(self.N_pred_horiz-1))
xu = 1e4*np.ones(3*(self.N_pred_horiz-1))
bounds = Bounds(xl, xu)
res = minimize(self.objetive_function, x0, method='trust-constr', options={'verbose': 1}, bounds=bounds)
#res = minimize(self.objetive_function, x0, method='SLSQP', options={'ftol': 1e-9, 'disp': True}, bounds=bounds)
"""
bounds1 = [(-1e-4, 1e-4)] * (3 * (self.N_pred_horiz - 1))
res = optimize.differential_evolution(self.objective_func,
bounds1,
maxiter=20,
popsize=10,
tol=1e-4)
return res.x[0:3], res.fun
def mag_ned_to_eci(mag_0, theta, lonrad, gmst):
mag_local_0y = rotationY(mag_0, np.pi - theta)
mag_local_yz = rotationZ(mag_local_0y, -lonrad)
return rotationZ(mag_local_yz, -gmst)
def _mag_NED_to_ECI(self, mag_0, theta, lonrad, gmst):
mag_local_0y = rotationY(mag_0, np.pi - theta)
mag_local_yz = rotationZ(mag_local_0y, -lonrad)
self.Mag_i = rotationZ(mag_local_yz, -gmst)
def check_mode(self):
if self.adcs_mode == DETUMBLING:
self.omega_b_tar = np.array([0.0, 0.0, 0.0])
# self.controller.set_gain(self.P_omega, self.I_quat, np.diag([0.0, 0.0, 0.0]))
elif self.adcs_mode == NAD_POINT:
print('Nadir pointing mode...')
elif self.adcs_mode == REF_POINT:
# Vector direction of the Body frame to point to another vector
self.b_dir = np.array([0, 0, 1])
# Vector target from Inertial frame
i_tar = np.array([1, -1, -1])
i_tar = i_tar / np.linalg.norm(i_tar)
self.b_tar_i = i_tar
self.controller.set_ref_vector_i(i_tar)
# Vector target from body frame
self.b_tar_b = self.q_i2b_est.frame_conv(i_tar)
self.b_tar_b /= np.linalg.norm(self.b_tar_b)
self.current_theta_e = np.arccos(np.dot(self.b_dir, self.b_tar_b))
self.vec_u_e = np.cross(self.b_dir, self.b_tar_b)
self.vec_u_e /= np.linalg.norm(self.vec_u_e)
self.q_b2b_now2tar.setquaternion(
[self.vec_u_e, self.current_theta_e])
self.q_b2b_now2tar.normalize()
self.q_i2b_tar = self.q_i2b_est * self.q_b2b_now2tar
elif self.adcs_mode == GS_POINT:
# omega_target_b error
sideral = gstime(self.dynamics.simtime.current_jd)
tar_pos_i = rotationZ(self.tar_pos_ecef, -sideral)
self.tar_pos_eci = tar_pos_i
vel_gs_i = np.cross(earth_rot_i, tar_pos_i)
vel_gs_sc = vel_gs_i - self.dynamics.orbit.current_velocity_i
pos_sc2tar_i = tar_pos_i - self.dynamics.orbit.current_position_i
mag_omega_gs_sc = np.linalg.norm(vel_gs_sc) / np.linalg.norm(
pos_sc2tar_i)
unit_vec_pos = pos_sc2tar_i / np.linalg.norm(pos_sc2tar_i)
unit_vec_vel = vel_gs_sc / np.linalg.norm(vel_gs_sc)
unit_vec_omega_gs_sc = np.cross(unit_vec_pos, unit_vec_vel)
omega_gs_from_sc_i = mag_omega_gs_sc * unit_vec_omega_gs_sc
self.omega_b_tar = self.q_i2b_est.frame_conv(omega_gs_from_sc_i)
# Vector direction of the Body frame to point to another vector
self.b_dir = np.array([0, 0, 1])
# Error
self.b_tar_i = pos_sc2tar_i / np.linalg.norm(pos_sc2tar_i)
current_tar_pos_b = self.q_i2b_est.frame_conv(pos_sc2tar_i)
self.b_tar_b = current_tar_pos_b / np.linalg.norm(
current_tar_pos_b)
self.current_theta_e = np.arccos(np.dot(self.b_dir, self.b_tar_b))
self.vec_u_e = np.cross(self.b_dir, self.b_tar_b)
self.vec_u_e /= np.linalg.norm(self.vec_u_e)
self.q_b2b_now2tar.setquaternion(
[self.vec_u_e, self.current_theta_e])
self.q_b2b_now2tar.normalize()
self.q_i2b_tar = self.q_i2b_est * self.q_b2b_now2tar
else:
print('No mode selected')