def test_data2(self): qOB = (0.5/np.sqrt(2.))*np.array([np.sqrt(3.),np.sqrt(3.),1.,1.]) r = np.array([0.,0.,7.07e6]) v = np.array([0.,7.0e3,0.]) qIB = 0.5 * np.array([0,-1,0,np.sqrt(3)]) result = fr.qBO2qBI(qnv.quatInv(qOB),r,v) self.assertTrue(np.allclose(qnv.quatInv(qIB) ,result))
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 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_data3(self): r = 1/3 * np.array([7.07e6,7.07e6,7.07e6]) v = np.array([0.,1/2**(1/2),1/2**(1/2)]) qOB = (0.5/np.sqrt(2.))*np.array([np.sqrt(3.),np.sqrt(3.),1.,1.]) qBI = np.array([ -0.32227667, 0.74698487, 0.43819357, 0.38227967]) #m_OI = -1 * np.array(((2**(1/2)/9, -1/(9*2**(1/2)), -1/(9*2**(1/2))),(0, -1/(3*2**(1/2)), 1/(3*2**(1/2))),(1/3**(1/2), 1/3**(1/2), 1/3**(1/2)))) #q_OI = qnv.quatMultiplyNorm(qnv.quatInv(qOB),qnv.rotm2quat(m_A)) = [-0.1159169 -0.88047624 0.3647052 0.27984814] result = fr.qBO2qBI(qnv.quatInv(qOB),r,v) print(result) self.assertTrue(np.allclose(qBI ,result))
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 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 gg_torque(s):
q = s.getQ()
qi = qnv.quatInv(q)
v_pos_sat_i = s.getPos()
v_pos_sat_b = qnv.quatRotate(qi, 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
g = constants.G
v_t_gg_b = 3 * m * g * (np.cross(
v_pos_sat_b, np.matmul(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 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_magmeter(self,value): qBI = np.asarray(value[0]) v_B_i=np.asarray(value[1]) v_pos_i = np.array([1e6,-2.03,-3.0]) v_vel_i = np.array([2.0e3,2.8,-73.2]) qBO = frames.qBI2qBO(qBI,v_pos_i,v_vel_i) state = np.hstack((qBO,np.zeros(3))) mySat = satellite.Satellite(state,12.0) mySat.setMag_i(v_B_i) mySat.setPos(v_pos_i) mySat.setVel(v_vel_i) qOI = qnv.quatMultiplyNorm(qnv.quatInv(qBO),qBI) result = defblock.magnetometer(mySat) v_expected = value[2]; self.assertTrue(np.allclose(result,v_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 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 Solar_torque(satellite):
q = satellite.getQ()
qi = qnv.quatInv(q)
P = 1 # yet to include
e = 1 # yet to include
r_com_b = [1, 1, 1] # yet to include
l = 0.1 # metres, length of cube
v_sv_i = satellite.getSun_i() #sun vector in eci frame
v_sv_b = qnv.quatRotate(qi, v_sv_i) #sun vector in body frame
cosw1 = np.dot([1., 0., 0.], v_sv_b) / np.linalg.norm(v_sv_b)
cosw2 = np.dot([0., 1., 0.], v_sv_b) / np.linalg.norm(v_sv_b)
cosw3 = np.dot([0., 0., 1.], v_sv_b) / np.linalg.norm(v_sv_b)
area = (abs(cosw1) + abs(cosw2) + abs(cosw3)) * l * l
v_coswsq = np.array(
[abs(cosw1) * (cosw1),
abs(cosw2) * (cosw2),
abs(cosw3) * (cosw3)])
v_T_sd_b = P * area * (1 - e) / np.linalg.norm(v_sv_b) * np.cross(
v_sv_b, r_com_b) + 2 * e * P * l * l * np.cross(r_com_b, v_coswsq)
return v_T_sd_b
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 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 gravityForceTorque(state):
#gives force in ECIF and torque in body frame due to gravity
v_dL = v_L_b / nLg
dL = np.linalg.norm(v_dL)
dm = mu_m * dL
v_F_i = np.zeros((3, 1))
v_T_b = np.zeros((3, 1))
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)
if nLg > 1:
for i in range(1, int(nLg / 2 + 1)):
v_F_i, v_T_b = simpsonG(v_F_i, v_T_b, 4, g_dFdT, dm, (2 * i - 1),
v_dL, v_pos_sat_b, q)
for i in range(2, int(nLg / 2 + 1)):
v_F_i, v_T_b = simpsonG(v_F_i, v_T_b, 2, g_dFdT, dm, (2 * i - 2),
v_dL, v_pos_sat_b, q)
for i in [1, int(nLg)]:
v_F_i, v_T_b = simpsonG(v_F_i, v_T_b, 1, g_dFdT, dm, i, v_dL,
v_pos_sat_b, q)
v_F_i = v_F_i / 3
v_T_b = v_T_b / 3
else:
v_F_i, v_T_b = simpsonG(v_F_i, v_T_b, 1, g_dFdT, dm, nLg / 2, v_dL,
v_pos_sat_b, q)
return v_F_i, v_T_b
import forceTorque as ft import frames as fs import qnv import numpy as np from math import sqrt pos0 = np.array([[R], [0], [0]]) dist0 = np.linalg.norm(pos0) #v0 = np.array([[0],[sqrt(G*M/dist0)],[0]]) v0 = np.array([[3e3], [3e3], [0]]) w0 = np.array([[0], [-1 * sqrt(G * M / dist0**3)], [0]]) w0n = np.linalg.norm(w0) m_eu0 = np.array([[0, 0, -1], [1, 0, 0], [0, -1, 0]]) q0 = qnv.rotm2quat(m_eu0) q0 = q0.reshape((4, 1)) qi = qnv.quatInv(q0) v_L_b = L * np.array([[0.], [0.], [1.]]) v_L_i = qnv.quatRotate(q0, v_L_b) t = 100 pos1 = qnv.quatRotate(qi, pos0) v_pos_dL_b = pos1 + v_L_b v_i = v0 dL = L a, b = ft.m_dFdT(v_pos_dL_b, v_L_i / 100, v_i, q0, t, dL) a1 = qnv.quatRotate(qi, a) print a1, b
E=np.array([0,-3,2]) F=np.array([0,0,0]) q1=np.array([1,2,3,4]) q2=np.array([1,-2,-3,-4]) q3=np.array([1,-2,-3,4]) q4=np.array([1,2,3,-4]) q5=np.array([-1,-2,-3,4]) q6=np.array([-1,2,3,-4]) q7=np.array([0,2,-3,0]) q8=np.array([0,-2,3,0]) q9=np.array([0,0,0,0]) flag=1 # tessting for quatInv G=qnv.quatInv(q1) if (G == q2).all() == 0: flag=0 G=qnv.quatInv(q3) if (G == q4).all() == 0: flag=0 G=qnv.quatInv(q5) if (G == q6).all() == 0: flag=0 G=qnv.quatInv(q7) if (G == q8).all() == 0: flag=0
