def test_periodicity(self): #transformation periodicity should be 2*pi/w_EARTH t1 = 1023.0 t2 = t1 + 2*np.pi/W_EARTH given = np.array([5.0e3,2.30e3,5.89e3]) v1 = fr.ecif2ecef(given,t1) v2 = fr.ecif2ecef(given,t2) self.assertTrue(np.allclose(v1,v2))
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_orthogonality_of_matrix(self,value): #Test to check that the rotation matrix is orthogonal t = (EPOCH - EQUINOX).total_seconds() + value v_x = fr.ecif2ecef(np.array([1.0,0.0,0.0]),t) v_y = fr.ecif2ecef(np.array([0.0,1.0,0.0]),t) v_z = fr.ecif2ecef(np.array([0.0,0.0,1.0]),t) self.assertEqual(type(v_x),np.ndarray) m = np.array([v_x,v_y,v_z]) mT = m.transpose() I = np.dot(m,mT) self.assertTrue(np.allclose(I[0,:],[1.,0.,0.])) self.assertTrue(np.allclose(I[1,:],[0.,1.,0.])) self.assertTrue(np.allclose(I[2,:],[0.,0.,1.]))
def test_cases_from_file_e2e(self,value): t = 12.63e3 u = np.asarray(value) result = fr.ecif2ecef(u,t) t = t + (EPOCH - EQUINOX).total_seconds() v = np.array([cos(W_EARTH*t)*value[0]+sin(W_EARTH*t)*value[1],cos(W_EARTH*t)*value[1]-sin(W_EARTH*t)*value[0],value[2]]) self.assertTrue(np.allclose(result,v))
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 test_at_zero_time(self): #Test when ecef and ecif are aligned
t = (EQUINOX - EPOCH).total_seconds()
u = np.array([5.0e3, -2.30e3, -5.89e3])
v = fr.ecif2ecef(u, t)
self.assertTrue(np.allclose(v, u))
def test_inverse(self):
t = 53e2
u = np.array([-1.0, -5.0, 5e6])
v = fr.ecef2ecif(fr.ecif2ecef(u, t), t)
self.assertTrue(np.allclose(u, v))
This code generates csv file for latitude, longitude, altitude (LLA) corresponding to time data.
ECI frame position is provided from sgp_output.csv
Time: (since epoch) in seconds
latitude: -90 to 90 degrees
longitude: -180 to 180 degrees (-180 excluded)
altitude: in meters
'''
m_sgp_output_i = np.genfromtxt('sgp_output.csv', delimiter=",")
N = m_sgp_output_i.shape[0]
m_sgp_ecef = np.zeros([N,4])
m_LLA = np.zeros([N,4])
for k in range(0,N):
if math.fmod(k,N/100) == 0:
print (int(100.*k/N))
v_i = m_sgp_output_i[k,1:4] #iniertial frame position
time = m_sgp_output_i[k,0] #time in sec
#get position in ecef
v_ecef = fs.ecif2ecef(v_i,time)
#get latitude and longitude and altitude
v_latlon = fs.latlon(v_ecef.copy())
alt = np.linalg.norm(v_i.copy()) - R_EARTH #in meters
m_sgp_ecef[k,0] = time
m_LLA[k,0] = time
m_sgp_ecef[k,1:4] = v_ecef
m_LLA[k,1:4] = np.append(v_latlon,alt)
np.savetxt('LLA.csv',m_LLA, delimiter=",")
print("LLA done")
