# aerospaceresearch/orbitdeterminator

Switch branches/tags
Nothing to show
Fetching contributors…
Cannot retrieve contributors at this time
110 lines (87 sloc) 2.9 KB
 ''' Takes a set of keplerian elements (a, e, i, ω, Ω, v) and transforms it into a state vector (x, y, z, vx, vy, vz) where v is the velocity of the satellite ''' import numpy as np from math import * def kep_state(kep): ''' Converts the keplerian elements to position and velocity vector Args: kep(numpy array): a 1x6 matrix which contains the following variables kep(0): semi major axis (km) kep(1): eccentricity (number) kep(2): inclination (degrees) kep(3): argument of perigee (degrees) kep(4): right ascension of the ascending node (degrees) kep(5): true anomaly (degrees) Returns: numpy array: 1x6 matrix which contains the position and velocity vector r(0),r(1),r(2): position vector (x,y,z) km r(3),r(4),r(5): velocity vector (vx,vy,vz) km/s ''' r = np.zeros((6, 1)) mu = 398600.4405 # unload orbital elements array sma = kep[0, 0] ecc = kep[1, 0] inc = kep[2, 0] inc = radians(inc) argper = kep[3, 0] argper = radians(argper) raan = kep[4, 0] raan = radians(raan) tanom = kep[5, 0] tanom = radians(tanom) slr = sma * (1 - ecc * ecc) rm = slr / (1 + ecc * cos(tanom)) arglat = argper + tanom # argument of latitude sarglat = sin(arglat) carglat = cos(arglat) c4 = sqrt(mu / slr) c5 = ecc * cos(argper) + carglat c6 = ecc * sin(argper) + sarglat sinc = sin(inc) cinc = cos(inc) sraan = sin(raan) craan = cos(raan) # position vector r[0, 0] = rm * (craan * carglat - sraan * cinc * sarglat) r[1, 0] = rm * (sraan * carglat + cinc * sarglat * craan) r[2, 0] = rm * sinc * sarglat # velocity vector r[3, 0] = -c4 * (craan * c6 + sraan * cinc * c5) r[4, 0] = -c4 * (sraan * c6 - craan * cinc * c5) r[5, 0] = c4 * c5 * sinc # # transform r and v into ECI frame # # R1inc = np.array([[1, 0, 0], # [0, cos(-inc), sin(-inc)], # [0, -sin(-inc), cos(-inc)] # ]) # R3raan = np.array([[cos(-raan), sin(-raan), 0], # [-sin(-raan), cos(-raan), 0], # [0, 0, 1] # ]) # R3argper = np.array([[cos(-argper), sin(-argper), 0], # [-sin(-argper), cos(-argper), 0], # [0, 0, 1] # ]) # # r_final1 = np.dot(R3raan, R1inc) # r_final2 = np.dot(R3argper, r[0:3]) # # r_final = np.dot(r_final1, r_final2) # print(r_final) # v_final1 = np.dot(R3raan, R1inc) # v_final2 = np.dot(R3argper, r[3:6]) # v_final = np.dot(v_final1, v_final2) # print(v_final) # # r[0:3] = r_final # r[3:6] = v_final return r if __name__ == "__main__": kep = np.array([[15711.578566], [0.377617], [90.0], [0.887383], [0.0], [28.357744]]) r = kep_state(kep) print(r)