'Satellite State Vector to Keplerian Elements (Canonical Data)

DECLARE FUNCTION Vecsq# (x#, y#, z#)
DECLARE FUNCTION Mag# (x#, y#, z#)
DECLARE FUNCTION DotProd# (x1#, y1#, z1#, x2#, y2#, z2#)
DECLARE FUNCTION asin# (x#)
DECLARE FUNCTION acos# (x#)
DECLARE FUNCTION asinh# (x#)
DECLARE FUNCTION arc# (sinx#, cosx#)

DEFDBL A-Z
DEFINT I-K

'define some constants
pi = 3.1415926536#
dr = pi / 180               'degrees to radians
ec = 0                      'obliquity of ecliptic (geocentric orbit)
gravc = .0743668#           'gravitational constant = GM
um = 1
Re = 6378.14               'Earth's equatorial radius (km)


'read in satellite info
OPEN "satvec.dat" FOR INPUT AS #1
LINE INPUT #1, name$            'geocentric object name
LINE INPUT #1, equinox$         'equinox  eg J2000
INPUT #1, t0                       'epoch time
INPUT #1, r0(1), r0(2), r0(3)   'position vector
INPUT #1, v0(1), v0(2), v0(3)   'velocity vector
CLOSE #1

'display input information                                              
CLS
PRINT "KEPLERIAN ELEMENTS FROM STATE VECTOR"
PRINT "   Object Name: "; name$; "   Equinox: "; equinox$
PRINT USING "   Epoch time = #######.#######"; t0
PRINT "   State Vector"
PRINT USING "     r(k) =  ##.#######   ##.#######  ##.#######"; r0(1); r0(2); r0(3)
PRINT USING "     v(k) =  ##.#######   ##.#######  ##.#######"; v0(1); v0(2); v0(3)
PRINT

'compute vectors E,H,N
r = Mag(r0(1), r0(2), r0(3))
v2 = Vecsq(v0(1), v0(2), v0(3))
rv = DotProd(r0(1), r0(2), r0(3), v0(1), v0(2), v0(3))
'E(k)
 FOR k = 1 TO 3
   E(k) = (v2 / um - 1 / r) * r0(k) - rv / um * v0(k)
 NEXT k
'H(k)
 H(1) = r0(2) * v0(3) - r0(3) * v0(2)
 H(2) = r0(3) * v0(1) - r0(1) * v0(3)
 H(3) = r0(1) * v0(2) - r0(2) * v0(1)
'N(k)
 NN(1) = -H(2)
 NN(2) = H(1)
 NN(3) = 0

'Compute elements a,e and q
ai = 2 / r - v2 / um
E = Mag(E(1), E(2), E(3))
sp = Vecsq(H(1), H(2), H(3)) / um
q = sp / (1 + E)

'Compute elements i, OMEGA, w
H = Mag(H(1), H(2), H(3))
I = acos(H(3) / H) / dr
NN = Mag(NN(1), NN(2), NN(3))
omega = acos(NN(1) / NN) / dr
IF NN(2) < 0 THEN omega = 360 - omega
NE = DotProd(NN(1), NN(2), NN(3), E(1), E(2), E(3))
w = acos(NE / (NN * E)) / dr
IF E(3) < 0 THEN w = 360 - w

'setup for elements MM and TT
xb = (sp - r) / E
yb = rv * SQR(sp / um) / E

'check that object is in (elliptical) orbit
IF E > .999 THEN
  PRINT "Object is not in closed orbit (ie elliptical)"
  END
END IF

'elliptical orbit
aq = 1 / ai
b = SQR(1 - E * E) / ai
cx = xb * ai + E
sx = yb / b
x = arc(sx, cx)
MM = x - E * sx
n = gravc * ai * SQR(um * ai)
tp = t0 - MM / n
period = 2 * pi / gravc * SQR(1 / (um * ai * ai * ai))
perigee = aq * (1 - E)
apogee = aq * (1 + E)

'print elements
PRINT "Keplerian Elements"
 PRINT USING "  a = ###.####### Re ( ######.# km)"; aq; aq * Re
 PRINT USING "  e = ###.#######"; E
 PRINT USING "  M = ###.#######"; MM / dr
 PRINT USING "  i = ###.#######"; I
 PRINT USING "  O = ###.#######"; omega
 PRINT USING "  w = ###.#######"; w
 PRINT
 PRINT USING "  Perigee time = ########.#####"; tp
 PRINT USING "  Period  = #####.##### mins"; period
 PRINT USING "  Mean motion (n) = ##.####### orbits/day"; 1440 / period
 PRINT USING "  Perigee = ###.####### Re  (Hq = ######.# km)"; perigee; (perigee - 1) * Re
 PRINT USING "  Apogee  = ###.####### Re  (Hp = ######.# km)"; apogee; (apogee - 1) * Re

END

FUNCTION acos (x)
acos = 1.5707963263# - ATN(x / SQR(1 - x * x))
END FUNCTION

FUNCTION arc (sinx, cosx)
pi = 3.1415926536#
IF ABS(sinx) <= .707107 THEN x = asin(ABS(sinx))
IF ABS(cosx) <= .707107 THEN x = acos(ABS(cosx))
IF cosx >= 0 AND sinx >= 0 THEN arc = x
IF cosx < 0 AND sinx >= 0 THEN arc = pi - x
IF cosx < 0 AND sinx < 0 THEN arc = pi + x
IF cosx >= 0 AND sinx < 0 THEN arc = pi * 2 - x
END FUNCTION

FUNCTION asin (x)
asin = ATN(x / SQR(1 - x * x))
END FUNCTION

FUNCTION asinh (x)
asinh = LOG(x + SQR(x * x + 1))
END FUNCTION

FUNCTION DotProd (x1, y1, z1, x2, y2, z2)
DotProd = x1 * x2 + y1 * y2 + z1 * z2
END FUNCTION

FUNCTION Mag (x, y, z)
Mag = SQR(x * x + y * y + z * z)
END FUNCTION

FUNCTION Vecsq (x, y, z)
Vecsq = x * x + y * y + z * z
END FUNCTION