def setState(self,state): self.state = state.copy() v_pos_sat_i = state[0:3].copy() v_v_sat_i = state[3:6].copy() q = state[6:10],copy() self.qi = qnv.quatInv(q) self.v_pos_sat_b = qnv.quatRotate(qi, v_pos_sat_i) v_L_i = qnv.quatRotate(q,v_L_b) self.v_dL_cap_i = v_L_i/np.linalg.norm(v_L_i) self.v_dL_cap_e = fs.ecif2ecef(v_dL_cap_i,self.time) self.v_v_sat_e = fs.ecif2ecef(v_v_sat_i,self.time)
def test_magnetometer(self): qBO = np.array([.0,0.,0.,1.]) v_w_BI_b = np.array([-3.9999, 4.8575, 0]) v_w_BO_b = v_w_BI_b + qnv.quatRotate(qBO,v_w_IO_o) state = np.hstack((qBO,v_w_BO_b)) sat = satellite.Satellite(state,12.05) v_pos_i = np.array([1e6,-2.03,-3.0]) v_vel_i = np.array([2.0e3,2.8,-73.2]) sat.setPos(v_pos_i) sat.setVel(v_vel_i) qOI = np.array([-0.70697614, -0.01353643, 0.70697824, 0.01353791]) Magi = qnv.quatRotate(qnv.quatInv(qOI),np.array((2.3e-6,3.4e-6,3.1e-6))) sat.setMag_i(Magi) self.assertTrue(np.allclose(sensor.magnetometer(sat),np.array((2.3e-6,3.4e-6,3.1e-6))))
def test_quatrotate(self, value):
v = np.asarray(value[0])
q = np.asarray(value[1])
vr = np.asarray(value[2])
A = qnv.quatRotate(q, v)
self.assertTrue(np.allclose(A, vr))
def solar_torque(s):
q = s.getQ()
qi = qnv.quatInv(q)
r_com = constants.r_com
e = constants.e # coefficient of reflectivity of satellite
p = constants.p # solar radiation pressure at low earth orbit
v_sv_i = s.getSun_i(
) # unit sun vector in inertial frame obtained from satellite object
v_sv_b = qnv.quatRotate(qi, v_sv_i) / np.linalg.norm(v_sv_i)
cosw1 = np.dot(v_sv_b, [1, 0, 0])
cosw2 = np.dot(v_sv_b, [0, 1, 0])
cosw3 = np.dot(v_sv_b, [0, 0, 1])
l1 = constants.l1 # length of cuboid satellite in x direction in body frame
l2 = constants.l2 # length of cuboid satellite in y direction in body frame
l3 = constants.l3 # length of cuboid satellite in z direction in body frame
area = l2 * l3 * abs(cosw1) + l1 * l3 * abs(cosw2) + l1 * l2 * abs(cosw3)
# area of satellite perpendicular to sun vector
v_t_sd1_b = np.cross(
r_com, v_sv_b) * p * (1 - e) * area # torque due to absorption
v_t_sd2_b = np.cross(r_com, [
l2 * l3 * abs(cosw1) * cosw1, l1 * l3 * abs(cosw2) * cosw2,
l1 * l2 * abs(cosw3) * cosw3
]) * 2 * e * p
# reflection torque
v_t_sd_b = v_t_sd1_b + v_t_sd2_b
return v_t_sd_b
def test_appr_actTypeB(self, value):
torque_req = np.asarray(value[0])
magfield_b = np.asarray(value[1])
app_torque_exp = np.asarray(value[2])
vel = np.array([5, 6, 7])
pos = np.array([3, 4, 5])
q_BI = np.array([0.5, -0.5, 0.5, -0.5])
q_IB = quatInv(q_BI)
q_BO = qBI2qBO(q_BI, pos, vel)
magfield_i = quatRotate(q_IB, magfield_b)
w_BO_b = np.array([8, 6, 7])
state = np.hstack((q_BO, w_BO_b))
t0 = 0
s = sat.Satellite(state, t0)
s.setVel(vel)
s.setPos(pos)
s.setMag_i(magfield_i)
s.setControl_b(torque_req)
app_torque = act.actuatorTypeB(s)
self.assertTrue(np.allclose(app_torque, app_torque_exp))
def solarTorqueb(s):
'''
this function takes input:
Satellite Object from which it access
sun vector in eci frame
quarternion to convert a vector in eci frame to body frame
gives output:
torque due to solar drag about COM in body frame
'''
if s.getLight() == 0: #in eclipse: zero solar torque
v_t_sd_b = np.array([0., 0., 0.])
else: #light region
v_q_BI = s.getQ()
v_sv_i = s.getSun_i(
) # unit sun vector in inertial frame obtained from satellite object
v_sv_b_u = qnv.quatRotate(v_q_BI, v_sv_i) / np.linalg.norm(v_sv_i)
# area of satellite perpendicular to sun vector
area = Lx * Lx * (abs(v_sv_b_u[0]) + abs(v_sv_b_u[1]) +
abs(v_sv_b_u[2]))
# torque due to absorption
v_t_sd1_b = np.cross(r_COG_2_COM_b, v_sv_b_u) * SOLAR_PRESSURE * (
1 - REFLECTIVITY) * area
# reflection torque
v_t_sd2_b = np.cross(r_COG_2_COM_b, [abs(v_sv_b_u[0]) * v_sv_b_u[0], abs(v_sv_b_u[1]) * v_sv_b_u[1], \
abs(v_sv_b_u[2]) * v_sv_b_u[2]]) * 2.0* REFLECTIVITY * SOLAR_PRESSURE * Lx * Lx
#total solar-pressure torque
v_t_sd_b = v_t_sd1_b + v_t_sd2_b
return v_t_sd_b
def test_ideal_controlled(self):
v_w_IO_o = np.array([0., 0.00106178, 0.])
qBO = np.array([1., 0., 0., 0.])
w_BIB = -qnv.quatRotate(qBO, v_w_IO_o)
w_BOB = fr.wBIb2wBOb(w_BIB, qBO, v_w_IO_o)
expected = np.array([0., 0., 0.])
self.assertTrue(np.allclose(w_BOB, expected))
def aeroTorqueb(s):
'''
this function takes input:
Satellite Object from which it access
velocity of COM of satellite in eci frame
quarternion to convert a vector in eci frame to body frame
gives output:
torque due to air drag about COM in body frame
'''
v_q_BI = s.getQ()
v_vel_sat_i = s.getVel()
v_pos_i = s.getPos()
v_vel_atm_i = np.cross(np.array([0., 0., W_EARTH]),
np.array([v_pos_i[0], v_pos_i[1],
0.])) #velocity of atmosphere in eci
v_vel_i = v_vel_sat_i - v_vel_atm_i #velocity of satellite wrt atmosphere in eci
v_vel_b = qnv.quatRotate(v_q_BI, v_vel_i)
area = Lx * Lx * (abs(v_vel_b[0]) + abs(v_vel_b[1]) + abs(v_vel_b[2])
) # area of satellite perpendicular to velocity
v_t_ad_b = np.cross(r_COG_2_COM_b, v_vel_b) * RHO * AERO_DRAG * area / 2.
return v_t_ad_b
def test_stationary_body(self, value):
v_w_IO_o = np.array([0., 0.00106178, 0.])
qBO = np.asarray(value)
w_BIB = np.array([0., 0., 0.])
result = fr.wBIb2wBOb(w_BIB, qBO, v_w_IO_o)
expected = qnv.quatRotate(qBO, v_w_IO_o)
self.assertTrue(np.allclose(result, expected))
def test_sunsensor(self): qBO = np.array([.0,0.,0.,1.]) v_w_BI_b = np.array([-3.9999, 4.8575, 0]) v_w_BO_b = v_w_BI_b + qnv.quatRotate(qBO,v_w_IO_o) state = np.hstack((qBO,v_w_BO_b)) sat = satellite.Satellite(state,12.05) v_pos_i = np.array([1e6,-2.03,-3.0]) v_vel_i = np.array([2.0e3,2.8,-73.2]) sat.setPos(v_pos_i) sat.setVel(v_vel_i) qOI = np.array([-0.70697614, -0.01353643, 0.70697824, 0.01353791]) Suni = qnv.quatRotate(qnv.quatInv(qOI),np.array((3,4,5))) sat.setSun_i(Suni) result = sensor.sunsensor(sat) expected = np.array((3/(50**(0.5)),4/(50**(0.5)),5/(50**(0.5)))) self.assertTrue(np.allclose(result,expected))
def test_gyroscope(self): qBO = np.array([.0,0.,0.,1.]) v_w_BI_b = np.array([-3.9999, 4.8575, 0]) v_w_BO_b = v_w_BI_b + qnv.quatRotate(qBO,v_w_IO_o) state = np.hstack((qBO,v_w_BO_b)) sat = satellite.Satellite(state,12.05) sat.setGyroVarBias(np.array((-0.1, 0.1, 0.1))) self.assertTrue(np.allclose(sensor.gyroscope(sat),np.array((-4.0999,4.9575,0.1))))
def sunsensor(sat):
v_q_BO = sat.getQ_BO()
v_s_o = sat.getSun_o() #obtain sunvector in orbit frame
v_s_b = qnv.quatRotate(v_q_BO, v_s_o) #obtain sunvector in body frame
ss_read = ADC(v_s_b) #find reading per sunsensor
v_sun_b_m = calc_SV(ss_read) #calculate sunvector from sunsensor readings
return v_sun_b_m
def magnetometer(sat):
#model the magnetometer
#input : satellite class object
#output : measured magnetic field
v_q_BO = sat.getQ_BO()
v_B_o = sat.getMag_o() #obtain magnetic field in orbit frame
v_B_b = qnv.quatRotate(v_q_BO, v_B_o) #obtain magnetic field in body frame
v_B_m = v_B_b + MAG_BIAS #add errors
return v_B_m
def currentToTorque(current_list,sat):
'''
This function calculates the torques acting on satellite to a corresponding current in the torquer.
Input: array of currents with first row as time and next three as currents, satellite
Output: The torque acting on the satellite due to current in torquer(an array).
'''
v_mu_app = No_Turns*np.multiply(v_A_Torquer,current_list[:,1:])
v_magnetic_field_i=sat.getMag_i()
v_magnetic_field_b=quatRotate(sat.getQ(),v_magnetic_field_i) #get mag field in boyd frame
v_torque_app_b = np.cross(v_mu_app,v_magnetic_field_b)
return v_torque_app_b
def magnetometer(sat):
#model the magnetometer
#input : satellite class object
#output : measured magnetic field
v_q_BI = sat.getQ_BI()
v_B_i = sat.getMag_i() #obtain magnetic field in orbit frame
v_B_b = qnv.quatRotate(v_q_BI, v_B_i) #obtain magnetic field in body frame
v_B_m = v_B_b + np.random.multivariate_normal(MAG_BIAS,
MAG_COV) #add errors
return v_B_m
def sunsensor(sat):
v_q_BI = sat.getQ_BI()
v_s_i = sat.getSun_i() #obtain sunvector in orbit frame
v_s_b = qnv.quatRotate(v_q_BI, v_s_i) #obtain sunvector in body frame
ss_read = ADC(
v_s_b
) #find reading per sunsensor, error already incorporated in ADC code
v_sun_b_m = calc_SV(ss_read) #calculate sunvector from sunsensor readings
return v_sun_b_m
def test_GPS(self): qBO = np.array([.0,0.,0.,1.]) v_w_BI_b = np.array([-3.9999, 4.8575, 0]) v_w_BO_b = v_w_BI_b + qnv.quatRotate(qBO,v_w_IO_o) state = np.hstack((qBO,v_w_BO_b)) sat = satellite.Satellite(state,3.132) v_pos_i = np.array([5000,5000,6000]) v_vel_i = np.array([7e3,0,0]) sat.setPos(v_pos_i) sat.setVel(v_vel_i) result=sensor.GPS(sat) self.assertTrue(np.allclose(result,np.array((5000,5000,6000,7e3,0,0,3.132))))
def m_dFdT(v_pos_dL_b, v_dL_cap_i, v_v_sat_i, q, t, dL):
qi = qnv.quatInv(q)
v_pos_dL_i = qnv.quatRotate(q, v_pos_dL_b)
height = np.linalg.norm(v_pos_dL_i) - R
height = height / 1e3
v_pos_dL_e = fs.ecif2ecef(v_pos_dL_i, t)
lat, lon = fs.latlon(v_pos_dL_e.reshape((3, )))
B = runigrf12(day, 0, 1, height, lat, lon)
v_B_n = np.vstack((B[0], B[1], B[2]))
v_B_n = v_B_n * 1e-9 #convert from nT to T
v_B_e = fs.ned2ecef(v_B_n.reshape((3, )), lat, lon)
v_dL_cap_e = fs.ecif2ecef(v_dL_cap_i, t)
v_v_sat_e = fs.ecif2ecef(v_v_sat_i, t)
e = qnv.dot1(qnv.cross1(v_v_sat_e, v_B_e), v_dL_cap_e)
i = e / mu_r
v_dF_e = dL * i * qnv.cross1(v_dL_cap_e, v_B_e)
v_dF_i = fs.ecef2ecif(v_dF_e, t)
v_dF_b = qnv.quatRotate(qi, v_dF_i)
v_dL_cap_b = v_L_b / np.linalg.norm(v_L_b)
v_dT_b = qnv.cross1(v_dL_cap_b, v_dF_b)
return v_dF_i, dL * v_dT_b
def GG_torque(satellite):
q = satellite.getQ() #body frame to eci frame??
qi = qnv.quatInv(q)
v_pos_sat_i = satellite.getPos() #pos vector in eci frame
v_pos_sat_b = qnv.quatRotate(qi, v_pos_sat_i) #pos vector in body frame
pos_norm = np.linalg.norm(v_pos_sat_b)
m_INERTIA = constants.m_INERTIA #how is this formed? can this be used directly?
Mass_earth = constants.M
G = constants.G
v_T_gg_i = 3 * Mass_earth * G * (np.linalg.cross(
v_pos_sat_b, m_INERTIA.dot(v_pos_sat_b))) / (pos_norm**5)
return v_T_gg_b
def actuatorTypeA(sat):
#input: magnetic moment required in Control Step
#output: Torque applied in body frame about COM for duration of model step
v_magnetic_moment_b = sat.getMagmomentRequired_b()
v_magnetic_field_i = sat.getMag_i()
v_magnetic_field_b = quatRotate(
sat.getQ_BI(), v_magnetic_field_i) #get mag field in body frame
v_torque_app_b = np.cross(v_magnetic_moment_b, v_magnetic_field_b)
sat.setAppTorque_b(v_torque_app_b)
def gg_torque(s):
q = s.getQ()
v_pos_sat_i = s.getPos()
v_pos_sat_b = qnv.quatRotate(q, v_pos_sat_i)
pos_norm = np.linalg.norm(v_pos_sat_b)
m_inertia = constants.m_INERTIA # moment of inertia matrix in body frame
m = constants.M_EARTH
g = constants.G
v_t_gg_b = 3 * m * g * (np.cross(v_pos_sat_b, np.dot(m_inertia, v_pos_sat_b))) / pos_norm ** 5
return v_t_gg_b
def Aero_torque(satellite):
q = satellite.getQ()
qi = qnv.quatInv(q)
r_com_b = [1, 1, 1] # yet to include
density = 1 # yet to include
Cd = 1 # yet to include
l = 0.1 #length of cube in metres
v_vel_i = satellite.getVel() #velocity of COM in eci frame
v_vel_b = qnv.quatRotate(qi, v_vel_i) #velocity of COM in body frame
vel_norm = np.linalg.norm(v_vel_b)
area = l * l * (abs(np.dot([1., 0, 0], v_vel_b)) + abs(
np.dot([0, 1., 0], v_vel_b)) + abs(np.dot([0, 0, 1.], v_vel_b)))
v_T_ad_b = density * Cd / 2 * area * vel_norm * np.cross(r_com_b, v_vel_b)
return v_T_ad_b
def magneticForceTorque(state, t):
q = state[6:10].copy()
qi = qnv.quatInv(q)
v_pos_sat_i = state[0:3].copy()
v_pos_sat_b = qnv.quatRotate(qi, v_pos_sat_i)
v_v_sat_i = state[3:6].copy()
v_L_i = qnv.quatRotate(q, v_L_b)
v_dL_i = v_L_i / nLb
v_dL_cap_i = v_dL_i / np.linalg.norm(v_dL_i)
dL = np.linalg.norm(v_dL_i)
v_dL_b = v_L_b / nLb
v_F_i = np.array([[0.], [0.], [0.]])
v_T_b = np.array([[0.], [0.], [0.]])
if nLb > 1:
for i in range(1, nLb / 2 + 1):
v_F_i, v_T_b = simpsonM(v_F_i, v_T_b, 4, m_dFdT, (2 * i - 1),
v_pos_sat_b, v_dL_b, v_dL_cap_i, v_v_sat_i,
q, t, dL)
for i in range(2, nLb / 2 + 1):
v_F_i, v_T_b = simpsonM(v_F_i, v_T_b, 2, m_dFdT, (2 * i - 2),
v_pos_sat_b, v_dL_b, v_dL_cap_i, v_v_sat_i,
q, t, dL)
for i in [1, nLg]:
v_F_i, v_T_b = simpsonM(v_F_i, v_T_b, 1, m_dFdT, i, v_pos_sat_b,
v_dL_b, v_dL_cap_i, v_v_sat_i, q, t, dL)
v_F_i = v_F_i / 3
v_T_b = v_T_b / 3
else:
v_F_i, v_T_b = simpsonM(v_F_i, v_T_b, 1, m_dFdT, nLb / 2, v_pos_sat_b,
v_dL_b, v_dL_cap_i, v_v_sat_i, q, t, dL)
return v_F_i, v_T_b
def currentToTorque(current_list, sat):
'''
This function calculates the torques acting on satellite to a corresponding current in the torquer.
Input: array of currents, satellite
Output: The torque acting on the satellite due to current in torquer at an instant.
'''
v_mu_app = No_Turns * np.multiply(
v_A_Torquer, current_list[:, :]
) # since current_list is array with [time, I1,I2,I3]
v_magnetic_field_i = sat.getMag_i()
v_magnetic_field_b = quatRotate(
sat.getQ_BI(), v_magnetic_field_i) #get mag field in body frame
v_torque_app_b = np.cross(v_mu_app, v_magnetic_field_b)
return v_torque_app_b
def actuatorTypeB(sat):
#input: Torque required in body frame about COM
#output: Torque applied in body frame about COM for duration of model step
v_magnetic_field_i = sat.getMag_i()
v_magnetic_field_b = quatRotate(
sat.getQ_BI(), v_magnetic_field_i) #get mag field in body frame
v_torque_control_b = sat.getControl_b()
#below is formulae for calculating required magnetic moment from the control torque
v_magnetic_moment_b = (1 /
(np.linalg.norm(v_magnetic_field_b))**2) * np.cross(
v_magnetic_field_b, v_torque_control_b)
v_torque_app_b = np.cross(v_magnetic_moment_b, v_magnetic_field_b)
sat.setAppTorque_b(v_torque_app_b)
def aero_torque(s):
q = s.getQ()
qi = qnv.quatInv(q)
v_vel_i = s.getVel()
v_vel_b = qnv.quatRotate(qi, v_vel_i)
r_com = constants.r_com # vector containing coordinates of COM of satellite in body frame
cd = constants.cd # aerodynamic drag coefficient of satellite
rho = constants.rho # density of atmosphere at low earth orbit
vel_norm = np.linalg.norm(v_vel_b)
cosw1 = np.dot(v_vel_b, [1, 0, 0]) / x
cosw2 = np.dot(v_vel_b, [0, 1, 0]) / x
cosw3 = np.dot(v_vel_b, [0, 0, 1]) / x
l = constants.length # length of cube in body frame
area = l * l * (abs(cosw1) + abs(cosw2) + abs(cosw3)
) # area of satellite perpendicular to velocity
v_t_ad_b = np.cross(r_com, v_vel_b) * rho * cd * vel_norm * area / 2
return v_t_ad_b
def ctrlTorqueToVoltage(sat):
'''
This function takes control torque from control law(this case b_dot law) and convert to magnetic moment,
which in turn converted to current and then voltage to be applied
Input: satellite
Output: Voltage to be applied to Torquer to achieve the required control torque
'''
v_magnetic_field_i=sat.getMag_i()
v_magnetic_field_b=quatRotate(sat.getQ(),v_magnetic_field_i) #get mag field in body frame
v_torque_control_b = sat.getControl_b()
#below is formulae for calculating required magnetic moment from the control torque
v_magnetic_moment_b=(1/(np.linalg.norm(v_magnetic_field_b))**2)*np.cross(v_magnetic_field_b,v_torque_control_b)
v_current=(1.0/No_Turns)*np.divide(v_magnetic_moment_b,v_A_Torquer)
voltage=v_current*RESISTANCE #simple I*R is used, since there's no other way as of now
return voltage
def ggTorqueb(s):
'''
this function takes input:
Satellite Object from which it access
position of centre of mass of satellite in eci frame
quarternion to convert vector in eci frame to body frame
gives output:
torque due to gravity gradient about centre of mass in body frame
'''
v_q_BI = s.getQ() #unit quaternion
v_pos_i = s.getPos()
v_pos_b = qnv.quatRotate(v_q_BI, v_pos_i)
pos_norm = np.linalg.norm(v_pos_b)
v_t_gg_b = 3. * M_EARTH * G * (np.cross(v_pos_b, np.dot(
m_INERTIA, v_pos_b))) / (pos_norm**5.)
return v_t_gg_b
def solar_torque(s):
q = s.getQ()
r_com = constants.r_COM
e = constants.REFLECTIVITY # coefficient of reflectivity of satellite
p = constants.SOLAR_PRESSURE # solar radiation pressure at low earth orbit
v_sv_i = s.getSun_i() # unit sun vector in inertial frame obtained from satellite object
v_sv_b_u = qnv.quatRotate(q, v_sv_i) / np.linalg.norm(v_sv_i)
l = constants.Lx
area = l * l * (abs(v_sv_b_u[0]) + abs(v_sv_b_u[1]) + abs(v_sv_b_u[2]))
# area of satellite perpendicular to sun vector
v_t_sd1_b = np.cross(r_com, v_sv_b_u) * p * (1 - e) * area # torque due to absorption
v_t_sd2_b = np.cross(r_com, [abs(v_sv_b_u[0]) * v_sv_b_u[0], abs(v_sv_b_u[1]) * v_sv_b_u[1], abs(v_sv_b_u[2]) * v_sv_b_u[2]]) * 2 * e * p * l * l
# reflection torque
v_t_sd_b = v_t_sd1_b + v_t_sd2_b
return v_t_sd_b
def aero_torque(s):
q = s.getQ()
v_vel_sat_i = s.getVel()
v_pos_i = s.getPos()
v_vel_atm_i = np.cross(np.array([0.,0.,constants.W_EARTH]),np.array([v_pos_i[0],v_pos_i[1],0.])) #velocity of atmosphere in eci
v_vel_i = v_vel_sat_i - v_vel_atm_i #velocity of satellite wrt atmosphere in eci
v_vel_b = qnv.quatRotate(q, v_vel_i)
r_com = constants.r_COM # vector containing coordinates of COM of satellite in body frame
cd = constants.AERO_DRAG # aerodynamic drag coefficient of satellite
rho = constants.RHO # density of atmosphere at low earth orbit
vel_norm = np.linalg.norm(v_vel_b)
l = constants.Lx # length of cube
area = l * l * (abs(v_vel_b[0]) + abs(v_vel_b[1]) + abs(v_vel_b[2])) # area of satellite perpendicular to velocity
v_t_ad_b = np.cross(r_com, v_vel_b) * rho * cd * area / 2.
return v_t_ad_b
