Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
143 lines (110 sloc) 4.59 KB
"""This module computes the state vector from keplerian elements."""
import math
import numpy as np
from scipy.optimize import fsolve
mu = 398600.4418
def __MtoE(M,e):
"""Calculates the eccentric anomaly from the mean anomaly.
Args:
M(float): the mean anomaly (in radians)
e(float): the eccentricity
Returns:
float: The eccentric anomaly (in radians)
"""
E = M
dy = 1
while(abs(dy) > 0.0001):
M2 = E - e*math.sin(E)
dy = M - M2
dx = dy/(1-e*math.cos(E))
E = E+dx
return E
def __TtoE(T,e):
E = math.atan2((1-e**2)**0.5*math.sin(T),e+math.cos(T))
E = E%(2*math.pi)
return E
def __EtoT(E,e):
T = math.atan2((1-e**2)**0.5*math.sin(E),math.cos(E)-e)
T = T%(2*math.pi)
return T
def MtoT(M,e):
return __EtoT(__MtoE(M,e),e)
def tle_to_state(tle):
""" This function converts from TLE elements to position and velocity vector
Args:
tle(1x6 numpy array): tle contains the following variables
tle[0] = inclination (degrees)
tle[1] = right ascension of the ascending node (degrees)
tle[2] = eccentricity (number)
tle[3] = argument of perigee (degrees)
tle[4] = mean anomaly (degrees)
tle[5] = mean motion (revs per day)
Returns:
r: 1x6 numpy array which contains the position and velocity vector
r[0],r[1],r[2] = position vector [rx,ry,rz] km
r[3],r[4],r[5] = velocity vector [vx,vy,vz] km/s
"""
# unload orbital elements array
T = 86400/tle[5]
sma = (mu*(T/2/math.pi)**2)**(1/3)
ecc = tle[2] # eccentricity
inc = tle[0] # inclination
argp = tle[3] # argument of perigee
raan = tle[1] # right ascension of the ascending node
tanom = MtoT(math.radians(tle[4]), ecc) # we use mean anomaly(kep(5)) and the function MtoT to compute true anomaly (tanom)
tanom = math.degrees(tanom)%360
kep = np.array([sma,ecc,inc,argp,raan,tanom])
s = kep_to_state(kep)
print("Keplerian Elements:")
print(kep)
return s
def kep_to_state(kep):
""" This function converts from keplerian elements to the position and velocity vector
Args:
kep(1x6 numpy array): kep contains the following variables
kep[0] = semi-major axis (kms)
kep[1] = eccentricity (number)
kep[2] = inclination (degrees)
kep[3] = argument of perigee (degrees)
kep[4] = right ascension of ascending node (degrees)
kep[5] = true anomaly (degrees)
Returns:
r: 1x6 numpy array which contains the position and velocity vector
r[0],r[1],r[2] = position vector [rx,ry,rz] km
r[3],r[4],r[5] = velocity vector [vx,vy,vz] km/s
"""
r = np.zeros((6,1))
# unload orbital elements array
sma = kep[0] # sma is semi major axis
ecc = kep[1] # eccentricity
inc = math.radians(kep[2]) # inclination
argp = math.radians(kep[3]) # argument of perigee
raan = math.radians(kep[4]) # right ascension of the ascending node
eanom = __TtoE(math.radians(kep[5]), ecc) # we use mean anomaly(kep(5)) and the function MtoE to compute eccentric anomaly (eanom)
smb = sma * math.sqrt(1-ecc**2)
x = sma * (math.cos(eanom) - ecc)
y = smb * math.sin(eanom)
# calculate position and velocity in orbital frame
m_dot = (mu/sma**3)**0.5
e_dot = m_dot/(1 - ecc*math.cos(eanom))
x_dot = -sma * math.sin(eanom) * e_dot
y_dot = smb * math.cos(eanom) * e_dot
# rotate them by argp degrees
x_rot = x * math.cos(argp) - y * math.sin(argp)
y_rot = x * math.sin(argp) + y * math.cos(argp)
x_dot_rot = x_dot * math.cos(argp) - y_dot * math.sin(argp)
y_dot_rot = x_dot * math.sin(argp) + y_dot * math.cos(argp)
# convert them into 3D coordinates
r[0] = x_rot * math.cos(raan) - y_rot * math.sin(raan) * math.cos(inc)
r[1] = x_rot * math.sin(raan) + y_rot * math.cos(raan) * math.cos(inc)
r[2] = y_rot * math.sin(inc)
r[3] = x_dot_rot * math.cos(raan) - y_dot_rot * math.sin(raan) * math.cos(inc)
r[4] = x_dot_rot * math.sin(raan) + y_dot_rot * math.cos(raan) * math.cos(inc)
r[5] = y_dot_rot * math.sin(inc)
return r
if __name__ == "__main__":
#tle = np.array([51.6382,7.1114,0.0002893,211.1340,148.9642,15.568214688])
#tle = np.array([51.6428, 291.0075, 0.0003411, 263.9950, 245.8448, 15.54009155])
tle = np.array([51.6418, 266.6543, 0.0003456, 290.0933, 212.4518, 15.54021918])
r = tle_to_state(tle)
print(r)