def __init__(self, init_componenets, subsystem_setting, dynamics):
# prescalar_time in [ms]
ComponentBase.__init__(
self,
prescalar_time=(1000 / subsystem_setting['ADCS_com_frequency']))
# component quaternion
self.q_b2c = subsystem_setting['q_b2c']
# cycle in [ms]
self.ctrl_cycle = 1000 / subsystem_setting['ADCS_com_frequency']
self.port_id = subsystem_setting['port_id']
self.comp_number = subsystem_setting['ADCS_COMPONENT_NUMBER']
self.dynamics = dynamics
self.components = Components(init_componenets, self.dynamics,
self.port_id)
self.current_omega_c_gyro = np.zeros(3)
self.torque_rw_b = np.zeros(3)
self.error_int = 0
self.omega_b_est = np.zeros(3)
self.control_torque = np.zeros(3)
self.P_omega = subsystem_setting['P_omega']
self.I_quat = subsystem_setting['I_quat']
self.P_quat = subsystem_setting['P_quat']
self.rw_torque_b = np.zeros(3)
self.q_i2b_est = Quaternions([0, 0, 0, 1])
self.q_b2b_now2tar = Quaternions([0, 0, 0, 1])
self.q_i2b_tar = Quaternions([0, 0, 0, 1])
def __init__(self, init_componenets, subsystem_setting, dynamics):
# prescalar_time in [ms]
ComponentBase.__init__(
self,
prescalar_time=(1000 / subsystem_setting['ADCS_com_frequency']))
# component quaternion
self.q_b2c = subsystem_setting['q_b2c']
# cycle in [ms]
self.ctrl_cycle = 1000 / subsystem_setting['ADCS_com_frequency']
self.port_id = subsystem_setting['port_id']
self.comp_number = subsystem_setting['ADCS_COMPONENT_NUMBER']
self.dynamics = dynamics
self.components = Components(init_componenets, self.dynamics,
self.port_id)
self.current_omega_c_gyro = np.zeros(3)
self.torque_rw_b = np.zeros(3)
self.omega_b_est = np.zeros(3)
self.control_torque = np.zeros(3)
self.historical_control = []
self.P_omega = subsystem_setting['P_omega']
self.I_quat = subsystem_setting['I_quat']
self.P_quat = subsystem_setting['P_quat']
self.rw_torque_b = np.zeros(3)
self.q_i2b_est = Quaternions([0, 0, 0, 1])
self.q_b2b_now2tar = Quaternions([0, 0, 0, 1])
self.q_i2b_tar = Quaternions([0, 0, 0, 1])
self.adcs_mode = DETUMBLING
for rw in self.components.rwmodel:
rw.set_step_width(self.ctrl_cycle / 1000)
self.controller = Controller().pid(self.P_quat, self.I_quat,
self.P_omega,
self.ctrl_cycle / 1000)
def __init__(self, init_componenets, subsystem_setting, dynamics):
ComponentBase.__init__(
self, (1000 / subsystem_setting['ODCS_com_frequency']))
self.dynamics = dynamics
self.ctrl_cycle = 1000 / subsystem_setting['ODCS_com_frequency']
self.port_id = subsystem_setting['port_id']
self.components = Components(init_componenets, self.dynamics,
self.port_id)
self.current_omega_c_gyro = np.zeros(3)
self.current_position_i = np.zeros(3)
self.current_velocity_i = np.zeros(3)
self.force_thruster_i = np.array([0.0, 0.0, 0.0])
self.force_thruster_b = np.array([0.0, 0.0, 0.0])
self.torque_thruster_b = np.zeros(3)
self.target_position = np.zeros(3)
self.target_velocity = np.zeros(3)
self.q_i2b_est = Quaternions([0, 0, 0, 1])
self.q_b2b_now2tar = Quaternions([0, 0, 0, 1])
self.q_i2b_tar = Quaternions([0, 0, 0, 1])
self.ctrl_ang = np.pi / 4
self.mag_thrust = 0
self.control_torque_b = np.zeros(3)
self.control_force_i = np.zeros(3)
self.historical_control_torque_b = []
self.historical_control_force_i = []
self.historical_thruster_i = []
self.g = np.array([0, 0, -1.62])
self.sliding = SlidingControl(self.g, self.dynamics.attitude.Inertia,
self.dynamics.attitude.inv_Inertia,
self.dynamics.attitude.current_mass,
self.ctrl_cycle)
for thr in self.components.thruster:
thr.set_step_width(self.ctrl_cycle / 1000)
def calculate_control_torque(self):
q_b2i_est = Quaternions(self.q_i2b_est.conjugate())
# First it is necessary to pass the quaternion from attitude to inertial,
# then the target vector is rotated from the inertial to body frame
q_i2b_now2tar = q_b2i_est * self.q_i2b_tar
q_i2b_now2tar.normalize()
torque_direction = np.zeros(3)
torque_direction[0] = q_i2b_now2tar()[0]
torque_direction[1] = q_i2b_now2tar()[1]
torque_direction[2] = q_i2b_now2tar()[2]
angle_rotation = 2 * np.arccos(q_i2b_now2tar()[3])
error_omega_ = self.omega_b_tar - self.omega_b_est
error_ = angle_rotation * torque_direction
"""
self.controller.open_loop([self.dynamics.attitude.current_quaternion_i2b,
self.dynamics.attitude.current_omega_b,
self.control_torque],
self.dynamics.simtime.current_jd)
self.control_torque = np.zeros(3)
"""
self.control_torque = self.controller.closed_loop([
self.dynamics.attitude.current_quaternion_i2b,
self.dynamics.attitude.current_omega_b, self.control_torque
], self.dynamics.simtime.current_jd)
def __init__(self, attitude_spacecraft):
self.attitudestep = attitude_spacecraft['attitudestep']
self.attitudecountTime = 0
# First time is False to not update the init data
self.attitude_update_flag = False
# quaternion = [i, j, k, 1]
self.current_quaternion_i2b = Quaternions(
attitude_spacecraft['Quaternion_i2b'])
self.current_omega_b = np.array(attitude_spacecraft['Omega_b'])
self.Inertia = attitude_spacecraft['Inertia']
self.inv_Inertia = np.linalg.inv(self.Inertia)
self.current_mass = attitude_spacecraft['Mass']
self.current_h_rw_b = np.zeros(3)
self.int_torque_b = np.zeros(3)
self.ext_torque_b = np.zeros(3)
self.h_total_b = np.zeros(3)
self.h_total_i_norm = 0
self.new_omega_b = np.zeros(3)
self.new_quaternion_i2b = np.zeros(3)
self.h_total_i = np.zeros(3)
self.int_force_b = np.zeros(3)
self.ext_force_b = np.zeros(3)
self.S_omega = np.zeros((3, 3))
self.Omega = np.zeros((4, 4))
self.historical_quaternion_i2b = []
self.historical_omega_b = []
self.historical_torque_t_b = []
self.historical_h_total_i = []
self.historical_mass = []
def __init__(self, port_id, properties, dynamics):
ComponentBase.__init__(self, 10)
self.dynamics = dynamics
self.port_id = port_id
self.pos_vector = properties['pos_vector']
self.normal_vector = properties['normal_vector']
self.q_b2c = Quaternions(properties['q_b2c'])
self.q_c2b = Quaternions(self.q_b2c.conjugate())
self.calib_h = properties['h']
self.calib_x0 = properties['x0']
self.calib_y0 = properties['y0']
self.calib_delta = np.deg2rad(properties['delta'])
self.xyd_nr_std = properties['nr_stddev_c']
self.scidriver = SCIDriver()
self.xyd_nrs_c = NormalRandom([self.xyd_nr_std, 0, 0])
self.scidriver.connect_port(self.port_id, 0, 0)
self.current_sun_vector_b = np.zeros(3)
self.current_sun_vector_c = np.zeros(3)
self.historical_sun_vector_b = []
self.historical_sun_vector_c = []
self.historical_V_ratio_m = []
self.historical_rd_m = []
self.historical_theta_m = []
self.historical_phi_m = []
self.cos_theta = 0
self.theta_fss = 0
self.phi_fss = 0
self.T = np.array(
[[np.cos(self.calib_delta),
np.sin(self.calib_delta)],
[-np.sin(self.calib_delta),
np.cos(self.calib_delta)]])
self.Tinv = np.array(
[[np.cos(self.calib_delta), -np.sin(self.calib_delta)],
[np.sin(self.calib_delta),
np.cos(self.calib_delta)]])
self.calib_params = [
self.calib_h, self.calib_x0, self.calib_y0, self.T, self.q_c2b
]
self.V0_ratio = np.array([self.calib_x0, self.calib_y0])
self.V_ratio_m = np.zeros(2)
self.rfss_c = np.zeros(3)
self.rd_c = np.zeros(2)
self.condition_ = False
self.albeo_correction = False
def __init__(self, mtt_properties):
ComponentBase.__init__(self, 50)
self.q_b2c = Quaternions(mtt_properties['q_b2c'])
self.max_c_am2 = mtt_properties['max_c_am2']
self.min_c_am2 = mtt_properties['min_c_am2']
self.bias_c = mtt_properties['bias_c']
self.rw_stddev_c = mtt_properties['rw_stddev_c']
self.rw_limit_c = mtt_properties['rw_limit_c']
self.nr_stddev_c = mtt_properties['nr_stddev_c']
self.step_width = mtt_properties['prop_step']
self.historical_mtt_torque_b = []
self.mtt_torque_b = np.zeros(3)
def __init__(self, port_id, properties, dynamics):
ComponentBase.__init__(self, 10)
self.dynamics_in_gyro = dynamics
self.port_id = port_id
self.current = properties['current']
self.q_b2c = Quaternions(properties['q_b2c'])
self.scalefactor = properties['ScaleFactor']
self.bias_c = properties['Bias_c']
self.n_rw_c = RandomWalk(properties['rw_stepwidth'], properties['rw_stddev_c'], properties['rw_limit_c'])
self.range_to_const = properties['Range_to_const']
self.range_to_zero = properties['Range_to_zero']
self.current_omega_c = np.zeros(3)
self.historical_omega_c = []
self.scidriver = SCIDriver()
self.scidriver.connect_port(self.port_id, 0, 0)
def __init__(self, port_id, properties, dynamics):
ComponentBase.__init__(self, 2)
self.dynamics_in_mag = dynamics
self.port_id = port_id
self.current = properties['current']
self.q_b2c = Quaternions(properties['q_b2c'])
self.scalefactor = properties['ScaleFactor']
self.bias_c = properties['Bias_c']
self.nrs_c = NormalRandom(properties['nr_stddev_c'])
self.n_rw_c = RandomWalk(properties['rw_stepwidth'],
properties['rw_stddev_c'],
properties['rw_limit_c'])
self.current_magVect_c = np.zeros(3)
self.historical_magVect_c = []
self.scidriver = SCIDriver()
self.scidriver.connect_port(self.port_id, 0, 0)
def calculate_control_force_torque(self):
q_b2i_est = Quaternions(self.q_i2b_est.conjugate())
# First it is necessary to pass the quaternion from attitude to inertial,
# then the target vector is rotated from the inertial to body frame
q_i2b_now2tar = q_b2i_est * self.q_i2b_tar
q_i2b_now2tar.normalize()
omega_b_tar = np.zeros(3)
error_omega = omega_b_tar - self.dynamics.attitude.current_omega_b
error_vel = self.dynamics.trajectory.current_velocity - np.zeros(3)
error_pos = self.dynamics.trajectory.current_position - np.zeros(3)
self.sliding.set_current_state(error_pos, error_vel, error_omega,
q_i2b_now2tar)
self.sliding.calc_force_torque()
self.control_force_i = self.sliding.get_force_i()
self.torque_thruster_b = self.sliding.get_torque_b()
return
def __init__(self, control_mode, dynamics, controller_parameters,
ctrl_cycle):
self.mpc_control_mode = control_mode
if self.mpc_control_mode == REF_POINT:
self.objective_func = self.objective_func_for_constant_ref_point
elif self.mpc_control_mode == NAD_POINT or self.mpc_control_mode == GS_POINT:
self.objective_func = self.objective_func_for_variable_ref_point
elif self.mpc_control_mode == DETUMBLING:
self.objective_func = self.objective_func_for_detumbling
else:
print('Control mode not selected')
self.mpc_inertia = dynamics.attitude.Inertia
self.mpc_inv_inertia = dynamics.attitude.inv_Inertia
self.mpc_current_omega_b = np.zeros(3)
self.mpc_current_quaternion_i2b = np.zeros(4)
self.mpc_current_h_total_b = np.zeros(3)
self.mpc_current_torque_b = np.zeros(3)
self.mpc_dynamics_orb = deepcopy(dynamics.orbit.propagator_model)
self.N_pred_horiz = controller_parameters['N_pred_horiz']
self.mpc_main_count_time = 0
self.mpc_start_jd = dynamics.simtime.current_jd
self.mpc_att_step_prop = dynamics.simtime.attitudestep
self.mpc_orb_step_prop = dynamics.simtime.orbitstep
self.mpc_sim_step_prop = dynamics.simtime.stepsimTime
self.mpc_ctrl_cycle = ctrl_cycle
self.mpc_array_jd = []
self.mpc_array_sc_pos_i = []
self.mpc_array_sc_vel_i = []
self.mpc_array_sideral = []
self.mpc_array_tar_pos_i = []
self.mpc_array_tar_pos_sc_i = []
self.mpc_array_lat = []
self.mpc_array_lon = []
self.mpc_array_alt = []
self.mpc_array_mag_i = []
self.mpc_array_decyear = []
# Geodetic to ECEF
self.tar_pos_ecef = geodetic_to_ecef(
tar_alt * 1e-3, tar_long * deg2rad, tar_lat * deg2rad) * 1e3
# Vector direction of the Body frame to point to another vector
self.b_dir = np.array([0, 0, 1])
self.current_tar_sc_i = np.array([1, 1, 1])
self.q_b2b_now2tar = Quaternions([0, 0, 0, 1])
def calc_torque(self, control_torque_b, mag_earth_b):
# Body frame to components frame
mag_earth_c = self.q_b2c.frame_conv(mag_earth_b)
control_mag_mom_b = np.cross(mag_earth_b, control_torque_b)
control_mag_mom_c = self.q_b2c.frame_conv(control_mag_mom_b)
for i in range(3):
if control_mag_mom_c[i] > self.max_c_am2[i]:
control_mag_mom_c[i] = self.max_c_am2[i]
elif control_mag_mom_c[i] < self.min_c_am2[i]:
control_mag_mom_c[i] = self.min_c_am2[i]
mtt_torque_c = np.cross(control_mag_mom_c, nT2T * mag_earth_c)
q_c2b = Quaternions(self.q_b2c.conjugate())
self.mtt_torque_b = q_c2b.frame_conv(mtt_torque_c)
return self.mtt_torque_b
def calculate_control_torque(self):
q_b2i_est = Quaternions(self.q_i2b_est.conjugate())
# First it is necessary to pass the quaternion from attitude to inertial,
# then the target vector is rotated from the inertial to body frame
q_i2b_now2tar = q_b2i_est * self.q_i2b_tar
q_i2b_now2tar.normalize()
torque_direction = np.zeros(3)
torque_direction[0] = q_i2b_now2tar()[0]
torque_direction[1] = q_i2b_now2tar()[1]
torque_direction[2] = q_i2b_now2tar()[2]
angle_rotation = 2 * np.arccos(q_i2b_now2tar()[3])
error_omega_ = self.omega_b_tar - self.omega_b_est
error_ = angle_rotation * torque_direction
control = self.controller.calc_control(error_, error_omega_,
self.adcs_mode)
self.control_torque = control
def calculate_control_torque(self):
q_b2i_est = Quaternions(self.q_i2b_est.conjugate())
# First it is necessary to pass the quaternion from attitude to inertial,
# then the target vector is rotated from the inertial to body frame
q_i2b_now2tar = q_b2i_est * self.q_i2b_tar
q_i2b_now2tar.normalize()
torque_direction = np.zeros(3)
torque_direction[0] = q_i2b_now2tar()[0]
torque_direction[1] = q_i2b_now2tar()[1]
torque_direction[2] = q_i2b_now2tar()[2]
angle_rotation = 2*np.arccos(q_i2b_now2tar()[3])
error_integral = self.ctrl_cycle * angle_rotation * torque_direction
omega_b_tar = np.array([-0.0, -0.0, 0.0])
error_omega = omega_b_tar - self.omega_b_est
self.error_int += self.ctrl_cycle*error_omega
self.control_torque = self.P_quat.dot(angle_rotation*torque_direction) + self.I_quat.dot(error_integral)
def rungeonestep(self, last_omega_b, last_quaternion_q_i2b, last_torque_b):
dt = self.mpc_sim_step_prop
x = np.concatenate((last_omega_b, last_quaternion_q_i2b()))
k1 = self.dynamics_and_kinematics(x, last_torque_b)
xk2 = x + (dt / 2.0) * k1
k2 = self.dynamics_and_kinematics(xk2, last_torque_b)
xk3 = x + (dt / 2.0) * k2
k3 = self.dynamics_and_kinematics(xk3, last_torque_b)
xk4 = x + dt * k3
k4 = self.dynamics_and_kinematics(xk4, last_torque_b)
next_x = x + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4)
omega_b = next_x[0:3]
q_i2b = Quaternions(next_x[3:])
q_i2b.normalize()
return q_i2b, omega_b
def __init__(self, init_componenets, subsystem_setting, dynamics):
# prescalar_time in [ms]
ComponentBase.__init__(
self,
prescalar_time=(1000 / subsystem_setting['ADCS_com_frequency']))
# component quaternion
self.q_b2c = subsystem_setting['q_b2c']
# cycle in [ms]
self.ctrl_cycle = 1000 / subsystem_setting['ADCS_com_frequency']
self.port_id = subsystem_setting['port_id']
self.comp_number = subsystem_setting['ADCS_COMPONENT_NUMBER']
self.dynamics = dynamics
self.components = Components(init_componenets, self.dynamics,
self.port_id)
self.current_omega_c_gyro = np.zeros(3)
self.current_magVect_c_magSensor = np.zeros(3)
self.magVect_i = np.zeros(3)
self.magVect_b = np.zeros(3)
self.sunPos_i = np.zeros(3)
self.sunDir_b = np.zeros(3)
self.sunPos_est_b = np.zeros(3)
self.torque_rw_b = np.zeros(3)
self.omega_b_est = np.zeros(3)
self.control_torque = np.zeros(3)
self.historical_control = []
self.historical_estimation = []
self.historical_P = []
self.historical_P_det = []
self.historical_P_det = []
self.historical_magVect_i = []
self.historical_ssEst_b = []
self.historical_fssEst_b = []
self.historical_fssQdr_d = []
self.historical_ssDir_b = []
self.historical_eskf_res = []
self.historical_eskf_obserrmag = []
self.historical_eskf_obserrcss = []
self.historical_eskf_bias = []
self.P_omega = subsystem_setting['P_omega']
self.I_quat = subsystem_setting['I_quat']
self.P_quat = subsystem_setting['P_quat']
self.rw_torque_b = np.zeros(3)
self.q_i2b_est = Quaternions([0, 0, 0, 1])
self.q_i2b_est_eskf_temp = Quaternions([0, 0, 0, 1])
self.q_b2b_now2tar = Quaternions([0, 0, 0, 1])
self.q_i2b_tar = Quaternions([0, 0, 0, 1])
self.eskf = ErrorStateKalmanFilter(
dim_x=7,
dim_dx=6,
dim_u=3,
dim_z=3,
inertia=self.dynamics.attitude.Inertia,
invInertia=self.dynamics.attitude.inv_Inertia)
self.jacobians = Jacobians()
self.number_ss = len(self.components.sunsensors)
self.number_fss = len(self.components.fss)
self.current_I_sunsensors = np.zeros(self.number_ss)
self.V_ratios_fss = np.zeros((self.number_fss, 2))
self.params_fss = [None] * self.number_fss
self.rsfss_b = np.zeros((self.number_fss, 3))
self.qdrsfss_b = np.zeros((self.number_fss, 2))
self.tick_temp = 1
self.adcs_mode = DETUMBLING
self.components.mtt.set_step_width(self.ctrl_cycle / 1000)
for rw in self.components.rwmodel:
rw.set_step_width(self.ctrl_cycle / 1000)
# self.controller = Controller().pid(self.P_quat, self.I_quat, self.P_omega, self.ctrl_cycle/1000)
control_parameters = {'N_pred_horiz': 3}
self.controller = Controller().mpc(self.dynamics, control_parameters,
self.ctrl_cycle)
def get_class_q_b2i(self):
return Quaternions(self.current_quaternion_i2b.conjugate())
def calangmom(self):
h_spacecraft_b = self.Inertia.dot(self.current_omega_b)
self.h_total_b = self.current_h_rw_b + h_spacecraft_b
q_bi2 = Quaternions(self.current_quaternion_i2b.conjugate())
self.h_total_i = q_bi2.frame_conv(self.h_total_b)
self.h_total_i_norm = np.linalg.norm(self.h_total_i)