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)