subroutine bceph2(tin, NAV, navcount, . prn, recf, vecf, svclk, ivel, ierr) c-- VERSION ----------------------------------------------------------- c c-- PURPOSE ----------------------------------------------------------- c broadcast ephemeris evaluation routine; c works on one satellite at a time; c updates choice of ephemeris set based upon c timetags desired for evaluation c -->> see also "kepler" subroutine in gpsGeneral.f for a different c set of arguments c c-- AUTHOR ------------------------------------------------------------ c Andria Bilich c c-- CALLED BY --------------------------------------------------------- c c-- INPUT PARAMETERS -------------------------------------------------- c tin(2) time of clock at transmission [sec week] real*8 c NAV ephemeris storage matrix (from "navread") array c navcount ephemeris count for this satellite integer c prn prn number integer c ivel 1=calculate satellite velocity integer c ierr 0=no problems finding orbit set c c-- OUTPUT PARAMETERS ------------------------------------------------- c recf(3) satellite xyz in ECEF frame m c vecf(3) satellite velocity in ECEF frame m c svclk satellite clock correction m c c-- INTERNAL CONSTANTS ------------------------------------------------ c c-- INTERNAL VARIABLES ------------------------------------------------ c tc time of clock at transmission sec of week c -->> from navigation file: c iprn index to correct prn in NAV matrix integer c ieph index to correct ephemiers in NAV matrix integer c tepoch time stamp for chosen ephemeris real c foundEph does ephemeris exist for SV logical c IDOE issue of data, ephemeris real c Cuc, Cus correction to argument of latitude real c Crc, Crs correction to orbital radius real c Cic, Cis correction to angle of inclination real c OMEGAdot rate of right ascension real c del_n mean motion difference from computed real c M0 mean anomaly reference time real c e eccentricity real c rt_a square root of semimajor axis real c OMEGAo longitude of ascending node (at week epoch) real c i0 inclination angle (at ref time) real c omega argument of perigee real c idot rate of inclination angle real c toe time of ephemeris (seconds of GPS week) real c -->> internal c tk reference time (relative to toe) c n0 computed mean motion c n corrected mean motion c A semimajor axis c Mk mean anomaly c Mkdot mean anomaly, derivative c Ek eccentric anomaly c Ekdot eccentric anomaly, derivative c PHIk argument of latitude c nuk true anomaly c nukdot true anomaly, derivative c uk corrected argument of latitude c ukdot corrected argument of latitude, derivative c rk corrected radius c rkdot corrected radius, derivative c ik corrected inclination c ikdot corrected inclination, derivative c xkp, ykp satellite position in orbital plane c xpkdot, ypkdot satellite position in orbital plane, derivatives c OMEGAk corrected longitude of ascending node c OMEGAkdot corrected longitude of ascending node, derivative c sinEk, cosEk sine/cosine of eccentric anomaly c sin2phik, cos2phik sine/cosine of 2*argument of latitude c sinuk, cosuk sine/cosine of corrected argument of latitude c sinOmk, cosOmk sine/cosine of corrected longitude of ascending node c sinik, cosik sine/cosine of corrected inclination c c-- GLOBAL CONSTANTS -------------------------------------------------- c c-- GLOBAL VARIABLES -------------------------------------------------- c C-- THIS MODULE CALLED BY: c C-- THIS MODULE CALLS: c c-- REFERENCES -------------------------------------------------------- c ICD-200 Table 20-IV, "elements of coordinate systems" c http://www.navcen.uscg.gov/pubs/gps/icd200/icd200cw1234.pdf c Remondi BW (2004) Computing satellite velocity using the c broadcast ephemeris, GPS Solutions, doi:10.1007/s1091-004-0094-6 c c-- MODIFICATION HISTORY ---------------------------------------------- c ALB, 07oct29: decided to abandon Kristine's terminology and use my own c in this subroutine c ALB, 07oct29: copied xenon:~kristine/Src/multi/bceph_new.f; c began modifications for better documentation c c ********************************************* implicit none include 'gpsConstants.h' integer i, prn, iprn, ieph, ivel, ierr, + navcount real*8 NAV(MAXSAT,MAXEPH,MAXORB), tin(2), tc, inweek, + tepoch, tweek real*8 IDOE, Crs, del_n, M0, Cuc, e, Cus, rt_a, + toe, Cic, OMEGAo, Cis, i0, Crc, omega, + OMEGAdot, idot, . bias, drift, driftdt, Frel, svclk real*8 a, n0, tk, Mk, n, + Ek, sinEk, cosEk, Eold, Enew, toler, small, + roote, hh, nuk, + PHIk, sin2phik, cos2phik, + deluk, delrk, delik, uk, rk, ik, + xkp, ykp, OMEGAk, cosOmk, sinOmk, cosik, sinik, + cosuk, sinuk real*8 Mkdot, Ekdot, nukdot, . dukdot, drkdot, dikdot, ukdot, rkdot, ikdot, . xpkdot, ypkdot, OMEGAkdot real*8 recf(3), vecf(3) logical foundEph c ********************************************* tc = tin(1) inweek = tin(2) c print *, 'finding epoch ', tc, inweek c look for available ephemeris sets for specified prn; c when ephemeris set has a non-zero timetag, c compare time of ephemeris to desired epoch: c after matching same GPS week (or greater), c toe < epoch = keep searching c toe >= epoch = use this ephemeris iprn = prn ieph = 0 foundEph = .false. do while (.not. foundEph .and. ieph.lt.MAXEPH) ieph = ieph + 1 tepoch = NAV(iprn,ieph,1) tweek = NAV(iprn,ieph,2) c print *, ieph, navcount, tepoch, tweek c if zero timetag, continue dowhile loop if (tepoch.eq.0.0 .and. tweek.eq.0.0) goto 103 c if search for recent ephemeris gets to c last available ephemeris (i.e. total number of eph available c according to 'navcount'), then use last eph if (ieph.eq.navcount) then foundEph = .true. goto 102 endif c else, if nonzero timetag, increment ephemeris counter c check NAV timetag against input time if (tepoch-tc.ge.0.0 .and. tweek.eq.tin(2)) foundEph = .true. if (tweek.gt.tin(2)) then c print *, 'over week boundary' foundEph = .true. endif 103 continue enddo 102 continue c otherwise no available ephemeris for this PRN; exit with error if (.not. foundEph .and. ieph.eq.1) then print *, 'satellite ', prn, ' not present in nav file' ierr = 1 goto 5000 endif c print *, 'using ephemeris number ', ieph, tepoch-tc c print *, 'NAVIGATION: toe', NAV(iprn,ieph,1), c . ' t_input ', tc,' ...', iprn, ieph c print *, ieph, ': EPOCH TEST: toe', NAV(iprn,ieph,1), c . ' t_input ', tc c ------------------------------------------- c parse out parameters from NAV storage matrix c issue of data, ephemeris (??) IDOE = NAV(iprn,ieph,3) c amplitudes of cosine and sine harmonic correction terms .... c ... argument of latitude [radians] Cuc = NAV(iprn,ieph,7) Cus = NAV(iprn,ieph,9) c ... orbit radius [meters] Crc = NAV(iprn,ieph,16) Crs = NAV(iprn,ieph,4) c ... angle of inclination [radians] Cic = NAV(iprn,ieph,12) Cis = NAV(iprn,ieph,14) c mean motion difference from computed value [rad/sec] del_n = NAV(iprn,ieph,5) c mean anomaly reference time [rad] M0 = NAV(iprn,ieph,6) c eccentricity e = NAV(iprn,ieph,8) c square root of semimajor axis [sqrt(meters)] rt_a = NAV(iprn,ieph,10) c longitude of ascending node of orbit plane at weekly epoch [rad] OMEGAo = NAV(iprn,ieph,13) c inclination angle at reference time [rad] i0 = NAV(iprn,ieph,15) c argument of perigee [rad] omega = NAV(iprn,ieph,17) c rate of right ascension [rad/sec] OMEGAdot = NAV(iprn,ieph,18) c rate of inclination angle [rad/sec] idot = NAV(iprn,ieph,19) c time of ephemeris [seconds of GPS week] toe = NAV(iprn,ieph,11) c satellite clock correction coefficients bias = nav(iprn,ieph,20) drift = nav(iprn,ieph,21) driftdt = nav(iprn,ieph,22) c ... code to check reading of navigation RINEX file ... c7788 format(3x,4d19.12) c write(*,7788) IDOE, Crs, del_n, M0 c write(*,7788) Cuc, e, Cus, rt_a c write(*,7788) toe, Cic, OMEGAo, Cis c write(*,7788) i0, Crc, omega, OMEGAdot c write(*,'(3x,d19.12)') idot c print *, ' ' c ------------------------------------------- c semi-major axis A=rt_a**2 c computed mean motion [rad/sec] n0=dsqrt(mu/A**3) c ** time for which I want position, relative to time of ephemeris tk = tc-toe cc print *, 'tc, toe: ', tc, toe c account for beginning or end of week crossovers if(tk.gt.302400.d0)tk=tk-604800.d0 if(tk.lt.-302400.d0)tk=tk+604800.d0 c print *, 'PRN', prn,':', ieph, tk, toe c corrected mean motion n = n0 + del_n c mean anomaly Mk = M0 + n*tk c correct for 2*pi wraparound if (Mk.lt.0.0) Mk = Mk + twopi if (Mk.gt.twopi) Mk = Mk - twopi c ---- solve Kepler's equation for Ek --------- c (Kepler's equation for eccentric anomaly, Ek) Eold = Mk small = 10e-15 do i=1,100 sinEk = dsin(Eold) cosEk = dcos(Eold) Enew = Eold + (Mk - Eold + e*sinEk) . / (1-e*cosEk) toler = dabs(Eold - Enew) if (toler.lt.small) goto 101 Eold = Enew enddo 101 continue Ek = Eold sinEk = dsin(Ek) cosEk = dcos(Ek) c print *, 'eccentric anomaly ', Ek c print*, i, ' iterations' c true anomaly roote = dsqrt(1.0d0 - e**2) hh = 1.0 - e*cosEk nuk = datan2( roote*sinEk/hh, (cosEk-e)/hh ) c correct for 2*pi wraparound if (nuk.lt.0.0) nuk = nuk + twopi if (nuk.gt.twopi) nuk = nuk - twopi c argument of latitude PHIk = nuk + omega c corrections for 2nd harmonic perturbations sin2phik = dsin(2*PHIk) cos2phik = dcos(2*PHIk) deluk = Cus*sin2phik + Cuc*cos2phik delrk = Crs*sin2phik + Crc*cos2phik delik = Cis*sin2phik + Cic*cos2phik c corrected argument of latitude uk = PHIk + deluk c corrected radius rk = A*hh + delrk c corrected inclination ik = i0 + delik + idot*tk c satellite position in orbital plane ref frame cosuk = dcos(uk) sinuk = dsin(uk) xkp = rk*cosuk ykp = rk*sinuk c corrected longitude of ascending node OMEGAk = OMEGAo + (OMEGAdot - OmDotEarth)*tk . - OmDotEarth*toe c satellite position in earth-fixed coordinates cosOmk = dcos(OMEGAk) sinOmk = dsin(OMEGAk) cosik = dcos(ik) sinik = dsin(ik) recf(1) = xkp*cosOmk - ykp*cosik*sinOmk recf(2) = xkp*sinOmk + ykp*cosik*cosOmk recf(3) = ykp*sinik if (ivel.ne.1) then vecf(1) = 0.0d0 vecf(2) = 0.0d0 vecf(3) = 0.0d0 goto 4000 endif C --------------------------------------------- c compute satellite velocity c -->> see also not_mine/bc_velo.c for Remondi code OMEGAkdot = OMEGAdot - OmDotEarth Mkdot = n c already solved Kepler's eq for Ek Ekdot = Mkdot/hh cc hh = 1.0 - e*cosEk c derivative of true anomaly (nuk) nukdot = sinEk*Ekdot*(1.0+e*dcos(nuk)) . / (dsin(nuk)*hh) c derviatives of 2nd harmonic corrections dukdot = 2*(Cus*cos2phik - Cuc*sin2phik)*nukdot drkdot = 2*(Crs*cos2phik - Crc*sin2phik)*nukdot dikdot = 2*(Cis*cos2phik - Cic*sin2phik)*nukdot c derivatives of 2nd harmonic perturbations (corrected) ukdot = nukdot + dukdot rkdot = A*e*sinEk*Ekdot + drkdot ikdot = idot + dikdot c derviatives of x & y positions in orbital plane xpkdot = rkdot*cosuk - ykp*ukdot ypkdot = rkdot*sinuk + xkp*ukdot vecf(1) = xpkdot*cosOmk - ypkdot*cosik*sinOmk . + ykp*sinik*sinOmk*ikdot - recf(2)*OMEGAkdot vecf(2) = xpkdot*sinOmk + ypkdot*cosik*cosOmk . - ykp*sinik*ikdot*cosOmk + recf(1)*OMEGAkdot vecf(3) = ypkdot*sinik + ykp*cosik*ikdot 4000 continue C --------------------------------------------- c compute satellite clock correction c print *, prn, tc c write(*,'(3d19.12)') bias, drift, driftdt c without relativity correction svclk = (bias + drift*tk + driftdt*(tk**2))*CLIGHT c include relativity correction c Frel = -2*sqrt(MU)/(CLIGHT**2) c svclk = (bias + drift*tk + driftdt*(tk**2) + c . Frel*e*rt_a*sin(Ek) )*CLIGHT 5000 continue return end