def satorb_prop_sp3(iX, iY, iZ, recv, Tp, ij):
"""
for satellite number prn
and receiver coordinates rrec0
find the x,y,z coordinates at time secweek
sp3 has the orbit information in it
"""
# start wit 70 milliseconds as the guess for the transmission time
nx = iX(Tp[ij] - 0.07)
ny = iY(Tp[ij] - 0.07)
nz = iZ(Tp[ij] - 0.07)
oE = constants.omegaEarth
c = constants.c
# get initial deltaA
SatOrb = np.array([nx, ny, nz]).T
r = np.subtract(SatOrb, recv)
tau = g.norm(r) / c
error = 0
k = 0
while (k < 2):
nx = iX(Tp[ij] - tau)
ny = iY(Tp[ij] - tau)
nz = iZ(Tp[ij] - tau)
SatOrb = np.array([nx, ny, nz]).T
Th = -oE * tau
xs = SatOrb[0] * np.cos(Th) - SatOrb[1] * np.sin(Th)
ys = SatOrb[0] * np.sin(Th) + SatOrb[1] * np.cos(Th)
SatOrbn = np.array([xs, ys, SatOrb[2]]).T
tau = g.norm(SatOrbn - recv) / c
k += 1
return SatOrbn
def satorb_prop(week, secweek, prn, rrec0, closest_ephem):
"""
Calculates and returns geometric range (in metres) given
time (week and sec of week), prn, receiver coordinates (cartesian, meters)
this assumes someone was nice enough to send you the closest ephemeris
returns the satellite coordinates as well, so you can use htem
in the A matrix
Kristine Larson, April 2017
"""
error = 1
# might as well start with 70 milliseconds
SatOrb = satorb(week, secweek - 0.07, closest_ephem)
# first estimate of the geometric range
geo = g.norm(SatOrb - rrec0)
deltaT = g.norm(SatOrb - rrec0) / constants.c
k = 0
#while (error > 1e-8) or (k < 2):
# should not need more than two iterations, since i am
#starting with 70 msec
while (k < 2):
SatOrb = satorb(week, secweek - deltaT, closest_ephem)
Th = -constants.omegaEarth * deltaT
xs = SatOrb[0] * np.cos(Th) - SatOrb[1] * np.sin(Th)
ys = SatOrb[0] * np.sin(Th) + SatOrb[1] * np.cos(Th)
SatOrbn = [xs, ys, SatOrb[2]]
# try this ???
geo = g.norm(SatOrbn - rrec0)
deltaT_new = g.norm(SatOrbn - rrec0) / constants.c
error = np.abs(deltaT - deltaT_new)
deltaT = deltaT_new
k += 1
return SatOrbn