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
class Attitude(object):
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 update_attitude(self, current_simtime):
if self.attitude_update_flag:
while np.abs(current_simtime - self.attitudecountTime -
self.attitudestep) > 1e-6:
self.rungeonestep(self.attitudecountTime, self.attitudestep)
self.attitudecountTime += self.attitudestep
self.rungeonestep(self.attitudecountTime, self.attitudestep)
self.attitudecountTime = current_simtime
else:
self.attitude_update_flag = True
self.calangmom()
self.reset_var_b()
def save_attitude_data(self):
self.historical_quaternion_i2b.append(self.current_quaternion_i2b())
self.historical_omega_b.append(self.current_omega_b)
self.historical_torque_t_b.append(self.total_torque_b())
self.historical_h_total_i.append(self.h_total_i_norm)
self.historical_mass.append(self.current_mass)
def add_ext_torque_b(self, ext_torque_b):
self.ext_torque_b += ext_torque_b
def add_ext_force_b(self, ext_force_b):
self.ext_force_b += ext_force_b
def get_ext_force_b(self):
return self.ext_force_b
def add_int_torque_b(self, torque_b):
self.int_torque_b += torque_b
def add_int_force_b(self, force_b):
self.int_force_b += force_b
def get_current_q_i2b(self):
return self.current_quaternion_i2b()
def get_class_q_i2b(self):
return self.current_quaternion_i2b
def get_class_q_b2i(self):
return Quaternions(self.current_quaternion_i2b.conjugate())
def total_torque_b(self):
return self.ext_torque_b + self.int_torque_b
def reset_var_b(self):
self.ext_torque_b = np.zeros(3)
self.int_torque_b = np.zeros(3)
self.ext_force_b = np.zeros(3)
self.int_force_b = np.zeros(3)
def dynamics_and_kinematics(self, x, t):
x_omega_b = x[0:3]
x_quaternion_i2b = x[3:]
self.skewsymmetricmatrix(x_omega_b)
self.omega4kinematics(x_omega_b)
h_total_b = self.current_h_rw_b + self.Inertia.dot(x_omega_b)
w_dot = -self.inv_Inertia.dot(
self.S_omega.dot(h_total_b) - self.total_torque_b())
q_dot = 0.5 * self.Omega.dot(x_quaternion_i2b)
f_x = np.concatenate((w_dot, q_dot))
return f_x
def rungeonestep(self, t, dt):
x = np.concatenate(
(self.current_omega_b, self.current_quaternion_i2b()))
k1 = self.dynamics_and_kinematics(x, t)
xk2 = x + (dt / 2.0) * k1
k2 = self.dynamics_and_kinematics(xk2, (t + dt / 2.0))
xk3 = x + (dt / 2.0) * k2
k3 = self.dynamics_and_kinematics(xk3, (t + dt / 2.0))
xk4 = x + dt * k3
k4 = self.dynamics_and_kinematics(xk4, (t + dt))
next_x = x + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4)
self.current_omega_b = next_x[0:3]
self.current_quaternion_i2b.setquaternion(next_x[3:])
self.current_quaternion_i2b.normalize()
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)
def skewsymmetricmatrix(self, x_omega_b):
self.S_omega[1, 0] = x_omega_b[2]
self.S_omega[2, 0] = -x_omega_b[1]
self.S_omega[0, 1] = -x_omega_b[2]
self.S_omega[0, 2] = x_omega_b[1]
self.S_omega[2, 1] = x_omega_b[0]
self.S_omega[1, 2] = -x_omega_b[0]
def omega4kinematics(self, x_omega_b):
self.Omega[1, 0] = -x_omega_b[2]
self.Omega[2, 0] = x_omega_b[1]
self.Omega[3, 0] = -x_omega_b[0]
self.Omega[0, 1] = x_omega_b[2]
self.Omega[0, 2] = -x_omega_b[1]
self.Omega[0, 3] = x_omega_b[0]
self.Omega[1, 2] = x_omega_b[0]
self.Omega[1, 3] = x_omega_b[1]
self.Omega[2, 1] = -x_omega_b[0]
self.Omega[2, 3] = x_omega_b[2]
self.Omega[3, 1] = -x_omega_b[1]
self.Omega[3, 2] = -x_omega_b[2]
def get_log_values(self):
report_attitude = {
'omega_t_b(X)[rad/s]': np.array(self.historical_omega_b)[:, 0],
'omega_t_b(Y)[rad/s]': np.array(self.historical_omega_b)[:, 1],
'omega_t_b(Z)[rad/s]': np.array(self.historical_omega_b)[:, 2],
'q_t_i2b(0)[-]': np.array(self.historical_quaternion_i2b)[:, 0],
'q_t_i2b(1)[-]': np.array(self.historical_quaternion_i2b)[:, 1],
'q_t_i2b(2)[-]': np.array(self.historical_quaternion_i2b)[:, 2],
'q_t_i2b(3)[-]': np.array(self.historical_quaternion_i2b)[:, 3],
'torque_t_b(X)[Nm]': np.array(self.historical_torque_t_b)[:, 0],
'torque_t_b(Y)[Nm]': np.array(self.historical_torque_t_b)[:, 1],
'torque_t_b(Z)[Nm]': np.array(self.historical_torque_t_b)[:, 2],
'h_total[Nms]': np.array(self.historical_h_total_i),
'Mass [kg]': np.array(self.historical_mass)
}
return report_attitude
class ODCS(ComponentBase):
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)
#thr.set_max_thrust(300)
#thr.set_burn_time(10)
def main_routine(self, count):
self.read_sensors()
self.determine_attitude()
self.determine_orbit()
self.check_mode()
self.calculate_control_force_torque()
self.calc_thruster_force_i()
return
def read_sensors(self):
self.current_omega_c_gyro = self.components.gyro.measure(
self.dynamics.attitude.current_omega_b)
self.current_position_i = self.dynamics.trajectory.current_position
self.current_velocity_i = self.dynamics.trajectory.current_velocity
def determine_attitude(self):
att = self.dynamics.attitude.get_current_q_i2b()
self.q_i2b_est.setquaternion(att)
return
def determine_orbit(self):
return
def check_mode(self):
# Vector direction of the Body frame to point to another vector
b_dir = np.array([0, 0, 1])
# Vector target from Inertial frame
i_tar = np.array([0, 0, 1])
i_tar = i_tar / np.linalg.norm(i_tar)
# Vector target from body frame
b_tar = self.q_i2b_est.frame_conv(i_tar)
b_lambda = np.cross(b_dir, b_tar)
rot = np.arcsin(
np.linalg.norm(b_lambda) /
(np.linalg.norm(b_tar) * np.linalg.norm(b_dir)))
self.q_b2b_now2tar.setquaternion([b_lambda, rot])
self.q_b2b_now2tar.normalize()
self.q_i2b_tar = self.q_i2b_est * self.q_b2b_now2tar
return
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 calc_thruster_force_b(self):
for thr in self.components.thruster:
thr.set_thrust_i(self.control_force_i /
len(self.components.thruster))
thr.calc_thrust_and_torque_b(self.q_i2b_est)
self.torque_thruster_b = self.control_force_i
def calc_thruster_force_i(self):
thruster_force_i = 0
for thr in self.components.thruster:
thr.set_thrust_i(self.control_force_i /
len(self.components.thruster))
thr.calc_thrust_and_torque_i(self.ctrl_cycle)
thruster_force_i += thr.get_force_i()
self.force_thruster_i = thruster_force_i
def get_force_b(self):
return self.force_thruster_b
def get_force_i(self):
return self.control_force_i
def get_torque_b(self):
return self.torque_thruster_b
def log_value(self):
self.historical_control_torque_b.append(self.control_torque_b)
self.historical_control_force_i.append(self.control_force_i)
self.historical_thruster_i.append(self.force_thruster_i)
return
def get_log_values(self):
report = {
'Control_X_i [N]': np.array(self.historical_control_force_i)[:, 0],
'Control_Y_i [N]': np.array(self.historical_control_force_i)[:, 1],
'Control_Z_i [N]': np.array(self.historical_control_force_i)[:, 2],
'Thruster_X_i [N]': np.array(self.historical_thruster_i)[:, 0],
'Thruster_Y_i [N]': np.array(self.historical_thruster_i)[:, 1],
'Thruster_Z_i [N]': np.array(self.historical_thruster_i)[:, 2]
}
return report
class ADCS(ComponentBase):
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 main_routine(self, count):
self.read_sensors()
self.check_mode()
self.determine_attitude()
self.calculate_control_torque()
self.calc_rw_torque()
return
def read_sensors(self):
self.current_omega_c_gyro = self.components.gyro.measure(
self.dynamics.attitude.current_omega_b)
def determine_attitude(self):
self.omega_b_est = self.current_omega_c_gyro
att = self.dynamics.attitude.get_current_q_i2b()
self.q_i2b_est.setquaternion(att)
return
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
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)
# Vector target from body frame
b_tar = self.q_i2b_est.frame_conv(i_tar)
b_tar /= np.linalg.norm(b_tar)
b_lambda = np.cross(b_dir, b_tar)
b_lambda /= np.linalg.norm(b_lambda)
rot = np.arccos(np.dot(b_dir, b_tar))
self.q_b2b_now2tar.setquaternion([b_lambda, rot])
self.q_b2b_now2tar.normalize()
self.q_i2b_tar = self.q_i2b_est * self.q_b2b_now2tar
else:
print('No mode selected')
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 calc_rw_torque(self):
f = 0
for rw in self.components.rwmodel:
rw.control_power()
rw.set_torque(self.control_torque[f], self.ctrl_cycle)
self.rw_torque_b += rw.calc_torque(self.ctrl_cycle)
f += 1
return
def get_rwtorque(self):
return self.rw_torque_b
def save_data(self):
self.historical_control.append(self.control_torque)
def get_log_values(self, subsys):
report = {
'RWModel_' + subsys + '_b(X)[Nm]': 0,
'RWModel_' + subsys + '_b(Y)[Nm]': 0,
'RWModel_' + subsys + '_b(Z)[Nm]': 0
}
if hasattr(self.components, 'gyro'):
gyro = self.components.gyro
report['gyro_omega_' + subsys + '_c(X)[rad/s]'] = np.array(
gyro.historical_omega_c)[:, 0]
report['gyro_omega_' + subsys + '_c(Y)[rad/s]'] = np.array(
gyro.historical_omega_c)[:, 1]
report['gyro_omega_' + subsys + '_c(Z)[rad/s]'] = np.array(
gyro.historical_omega_c)[:, 2]
if hasattr(self.components, 'rwmodel'):
for rw in self.components.rwmodel:
report['RWModel_' + subsys + '_b(X)[Nm]'] += np.array(
rw.historical_rw_torque_b)[:, 0]
report['RWModel_' + subsys + '_b(Y)[Nm]'] += np.array(
rw.historical_rw_torque_b)[:, 1]
report['RWModel_' + subsys + '_b(Z)[Nm]'] += np.array(
rw.historical_rw_torque_b)[:, 2]
report_control = {
'Control_(X)[Nm]': np.array(self.historical_control)[:, 0],
'Control_(Y)[Nm]': np.array(self.historical_control)[:, 1],
'Control_(Z)[Nm]': np.array(self.historical_control)[:, 2]
}
report = {**report, **report_control}
return report
class ADCS(ComponentBase):
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 main_routine(self, count):
self.read_sensors()
self.determine_attitude()
self.check_mode()
self.calculate_control_torque()
return
def read_sensors(self):
self.current_omega_c_gyro = self.components.gyro.measure(
self.dynamics.attitude.current_omega_b)
def determine_attitude(self):
self.omega_b_est = self.current_omega_c_gyro
att = self.dynamics.attitude.get_current_q_i2b()
self.q_i2b_est.setquaternion(att)
return
def check_mode(self):
# Vector direction of the Body frame to point to another vector
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)
# Vector target from body frame
b_tar = self.q_i2b_est.frame_conv(i_tar)
b_lambda = np.cross(b_dir, b_tar)
rot = np.arcsin(
np.linalg.norm(b_lambda) /
(np.linalg.norm(b_tar) * np.linalg.norm(b_dir)))
self.q_b2b_now2tar.setquaternion([b_lambda, rot])
self.q_b2b_now2tar.normalize()
self.q_i2b_tar = self.q_i2b_est * self.q_b2b_now2tar
return
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 get_torque(self):
return self.rw_torque_b
def log_value(self):
return
def get_log_values(self):
return
class ADCS(ComponentBase):
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 main_routine(self, count, sc_isDark):
self.read_sensors(sc_isDark)
self.check_mode()
self.determine_attitude()
self.calculate_control_torque()
# self.calc_rw_torque()
# self.calc_mtt_torque()
return
def read_sensors(self, sc_isDark):
self.current_omega_c_gyro = self.components.gyro.measure(
self.dynamics.attitude.current_omega_b)
self.current_magVect_c_magSensor = self.components.mag.measure(
self.magVect_b)
for i in range(self.number_ss):
self.components.sunsensors[i].measure(sc_isDark)
self.current_I_sunsensors[i] = self.components.sunsensors[
i].get_I_b()
for i in range(self.number_fss):
self.components.fss[i].measure(sc_isDark)
self.V_ratios_fss[i] = self.components.fss[i].get_V_ratio_b()
self.params_fss[i] = self.components.fss[i].calib_params
def determine_attitude(self):
# To do: Omega Estimate
self.omega_b_est = self.current_omega_c_gyro
# True/Simulated quaternion
att = self.dynamics.attitude.get_current_q_i2b()
self.q_i2b_est.setquaternion(att)
# Simulated Sun Vector preprocessing
self.sunPos_i = self.dynamics.ephemeris.selected_body[
'SUN'].get_pos_from_center_i()
self.sunDir_b = unitVector(
self.dynamics.ephemeris.selected_body['SUN'].get_pos_from_sc_b())
# Coarse Sun Sensor preprocessing (vector extraction from currents)
self.sunPos_est_b = self.eskf.get_ss_vect(
self.current_I_sunsensors, self.components.sunsensors[0].I_max)
# Fine Sun Sensor preprocessing (vector extraction from quadrature voltage)
for i in range(self.number_fss):
self.rsfss_b[i] = self.eskf.get_fss_vect(self.V_ratios_fss[i],
self.params_fss[i])
self.qdrsfss_b[i] = self.eskf.get_fss_qdvect(
self.V_ratios_fss[i], self.params_fss[i])
# Fine Sun Sensor average preprocessing (vector extraction from quadrature voltage)
self.rsfss_mean_b = self.eskf.get_fss_mean_vect(self.rsfss_b)
# Estimate Spacecraft Attitude quaternion
# self.eskf_process(self.ctrl_cycle/1000)
return
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
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)
# Vector target from body frame
b_tar = self.q_i2b_est.frame_conv(i_tar)
b_tar /= np.linalg.norm(b_tar)
b_lambda = np.cross(b_dir, b_tar)
b_lambda /= np.linalg.norm(b_lambda)
rot = np.arccos(np.dot(b_dir, b_tar))
self.q_b2b_now2tar.setquaternion([b_lambda, rot])
self.q_b2b_now2tar.normalize()
self.q_i2b_tar = self.q_i2b_est * self.q_b2b_now2tar
else:
print('No mode selected')
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)
#control = self.controller.calc_control(error_, error_omega_, self.adcs_mode)
def calc_mtt_torque(self):
self.components.mtt.calc_torque(self.control_torque,
self.current_magVect_c_magSensor)
return
def calc_rw_torque(self):
f = 0
for rw in self.components.rwmodel:
rw.control_power()
rw.set_torque(self.control_torque[f], self.ctrl_cycle)
self.rw_torque_b += rw.calc_torque(self.ctrl_cycle)
f += 1
return
def get_rwtorque(self):
return self.rw_torque_b
def set_magVector(self, mag_i, mag_b):
self.magVect_i = mag_i
self.magVect_b = mag_b
def save_data(self):
self.historical_control.append(self.control_torque)
self.historical_magVect_i.append(self.magVect_i)
def get_log_values(self, subsys):
report = {
'MTT_' + subsys + '_b(X)[Am]': 0,
'MTT_' + subsys + '_b(Y)[Am]': 0,
'MTT_' + subsys + '_b(Z)[Am]': 0
}
report_control = {
'Control_' + subsys + '_b(X)[Nm]':
np.array(self.historical_control)[:, 0],
'Control_' + subsys + '_b(Y)[Nm]':
np.array(self.historical_control)[:, 1],
'Control_' + subsys + '_b(Z)[Nm]':
np.array(self.historical_control)[:, 2]
}
report = {**report, **report_control}
return report
class ADCS(ComponentBase):
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.omega_b_tar = np.zeros(3)
self.b_dir = np.zeros(3)
self.b_tar_b = np.zeros(3)
self.b_tar_i = np.zeros(3)
self.control_torque = np.zeros(3)
self.tar_pos_eci = 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.historical_theta_e = []
self.historical_b_tar_b = []
self.historical_b_tar_i = []
self.historical_b_dir_b = []
self.historical_omega_b_tar = []
self.historical_vec_dir_tar_b = []
self.historical_calc_time = []
self.historical_cost_function = []
self.historical_tar_pos_eci = []
self.current_calc_time = 0
self.current_theta_e = 0
self.current_cost = 0
self.vec_u_e = 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_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 = GS_POINT
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': 10}
# Geodetic to ECEF
self.tar_pos_ecef = geodetic_to_ecef(
tar_alt * 1e-3, tar_long * deg2rad, tar_lat * deg2rad) * 1e3
self.controller = Controller().mpc(self.adcs_mode, self.dynamics,
control_parameters, self.ctrl_cycle)
def main_routine(self, count, sc_isDark):
# self.read_sensors(sc_isDark)
self.determine_attitude()
self.check_mode()
self.calculate_control_torque()
# self.calc_rw_torque()
# self.calc_mtt_torque()
return
def read_sensors(self, sc_isDark):
self.current_omega_c_gyro = self.components.gyro.measure(
self.dynamics.attitude.current_omega_b)
self.current_magVect_c_magSensor = self.components.mag.measure(
self.magVect_b)
for i in range(self.number_ss):
self.components.sunsensors[i].measure(sc_isDark)
self.current_I_sunsensors[i] = self.components.sunsensors[
i].get_I_b()
for i in range(self.number_fss):
self.components.fss[i].measure(sc_isDark)
self.V_ratios_fss[i] = self.components.fss[i].get_V_ratio_b()
self.params_fss[i] = self.components.fss[i].calib_params
def determine_attitude(self):
# To do: Omega Estimate
self.omega_b_est = self.dynamics.attitude.current_omega_b
# True/Simulated quaternion
att = self.dynamics.attitude.get_current_q_i2b()
self.q_i2b_est.setquaternion(att)
# # Simulated Sun Vector preprocessing
# self.sunPos_i = self.dynamics.ephemeris.selected_body['SUN'].get_pos_from_center_i()
# self.sunDir_b = unitVector(self.dynamics.ephemeris.selected_body['SUN'].get_pos_from_sc_b())
# # Coarse Sun Sensor preprocessing (vector extraction from currents)
# self.sunPos_est_b = self.eskf.get_ss_vect(self.current_I_sunsensors, self.components.sunsensors[0].I_max)
# # Fine Sun Sensor preprocessing (vector extraction from quadrature voltage)
# for i in range(self.number_fss):
# self.rsfss_b[i] = self.eskf.get_fss_vect(self.V_ratios_fss[i], self.params_fss[i])
# self.qdrsfss_b[i] = self.eskf.get_fss_qdvect(self.V_ratios_fss[i], self.params_fss[i])
# # Fine Sun Sensor average preprocessing (vector extraction from quadrature voltage)
# self.rsfss_mean_b = self.eskf.get_fss_mean_vect(self.rsfss_b)
# Estimate Spacecraft Attitude quaternion
# self.eskf_process(self.ctrl_cycle/1000)
return
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')
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
start_time = time.time()
# Control MPC
# control_mag_torque, self.current_cost = 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.current_calc_time = time.time() - start_time
# self.control_torque = control_mag_torque
# Control PID
# self.control_torque = 2e-5 * angle_rotation * self.vec_u_e - self.omega_b_est * 5e-4
# self.control_torque = 2e-5 * angle_rotation * self.vec_u_e + (self.omega_b_tar - self.omega_b_est) * 5e-4
def calc_mtt_torque(self):
self.components.mtt.calc_torque(self.control_torque,
self.current_magVect_c_magSensor)
return
def calc_rw_torque(self):
f = 0
for rw in self.components.rwmodel:
rw.control_power()
rw.set_torque(self.control_torque[f], self.ctrl_cycle)
self.rw_torque_b += rw.calc_torque(self.ctrl_cycle)
f += 1
return
def get_rwtorque(self):
return self.rw_torque_b
def set_magVector(self, mag_i, mag_b):
self.magVect_i = mag_i
self.magVect_b = mag_b
def save_data(self):
self.historical_control.append(self.control_torque)
self.historical_magVect_i.append(self.magVect_i)
self.historical_theta_e.append(self.current_theta_e)
self.historical_omega_b_tar.append(self.omega_b_tar)
self.historical_b_tar_b.append(self.b_tar_b)
self.historical_b_tar_i.append(self.b_tar_i)
self.historical_b_dir_b.append(self.b_dir)
self.historical_vec_dir_tar_b.append(self.vec_u_e)
self.historical_calc_time.append(self.current_calc_time)
self.historical_cost_function.append(self.current_cost)
self.historical_tar_pos_eci.append(self.tar_pos_eci)
def get_log_values(self, subsys):
report = {
'MTT_' + subsys + '_b(X)[Am]': 0,
'MTT_' + subsys + '_b(Y)[Am]': 0,
'MTT_' + subsys + '_b(Z)[Am]': 0
}
report_control = {
'Control_' + subsys + '_b(X)[Nm]':
np.array(self.historical_control)[:, 0],
'Control_' + subsys + '_b(Y)[Nm]':
np.array(self.historical_control)[:, 1],
'Control_' + subsys + '_b(Z)[Nm]':
np.array(self.historical_control)[:, 2]
}
report_target_state = {
'Theta_error [rad]':
np.array(self.historical_theta_e),
'Vector_dir_b(X) [-]':
np.array(self.historical_b_dir_b)[:, 0],
'Vector_dir_b(Y) [-]':
np.array(self.historical_b_dir_b)[:, 1],
'Vector_dir_b(Z) [-]':
np.array(self.historical_b_dir_b)[:, 2],
'Vector_tar_b(X) [-]':
np.array(self.historical_b_tar_b)[:, 0],
'Vector_tar_b(Y) [-]':
np.array(self.historical_b_tar_b)[:, 1],
'Vector_tar_b(Z) [-]':
np.array(self.historical_b_tar_b)[:, 2],
'Vector_tar_i(X) [-]':
np.array(self.historical_b_tar_i)[:, 0],
'Vector_tar_i(Y) [-]':
np.array(self.historical_b_tar_i)[:, 1],
'Vector_tar_i(Z) [-]':
np.array(self.historical_b_tar_i)[:, 2],
'Vector_dir_tar_b(X) [-]':
np.array(self.historical_vec_dir_tar_b)[:, 0],
'Vector_dir_tar_b(Y) [-]':
np.array(self.historical_vec_dir_tar_b)[:, 1],
'Vector_dir_tar_b(Z) [-]':
np.array(self.historical_vec_dir_tar_b)[:, 2],
'omega_b_tar(X) [rad/s]':
np.array(self.historical_omega_b_tar)[:, 0],
'omega_b_tar(Y) [rad/s]':
np.array(self.historical_omega_b_tar)[:, 1],
'omega_b_tar(Z) [rad/s]':
np.array(self.historical_omega_b_tar)[:, 2],
'Calculation_time [sec]':
np.array(self.historical_calc_time),
'Cost_function [-]':
np.array(self.historical_cost_function),
'tar_pos_eci(X) [m]':
np.array(self.historical_tar_pos_eci)[:, 0],
'tar_pos_eci(Y) [m]':
np.array(self.historical_tar_pos_eci)[:, 1],
'tar_pos_eci(Z) [m]':
np.array(self.historical_tar_pos_eci)[:, 2]
}
report = {**report, **report_control, **report_target_state}
return report