program gpr1f *** kinematic gps, double difference pseudorange *** version (0.1 initiated 10-mar-02) *** debug -- apply relativity correction *** debug -- read nav file and check health bits *** debug -- add all sorts of corrections implicit double precision(a-h,o-z) parameter(mxprn=32,mxepch=192) dimension orbits(mxepch + 4*mxprn*mxepch) common/lus/lst,lin,leph,lout common/stugg/pi,rad,sol,f1,f2,fw,w1,w2,ww,we pi=4.d0*datan(1.d0) rad=180.d0/pi sol=299792458.d0 f1 =1575.42d6 f2 =1227.60d6 fw =f1-f2 w1 =sol/f1 w2 =sol/f2 ww =sol/fw we=7.2921151467d-5 *** note: input file names currently hard-coded lst=1 open(lst,file='gpr1f.lst',form='formatted') write(lst,*) 'gpr1f -- dd short range -- 10mar02' lin=2 open(lin,file='gmerge.out',form='formatted',status='old') leph=3 open(leph,file='orbit.sp3',form='formatted',status='old') lout=4 open(lout,file='gpr1f.out',form='formatted') *** get header info from merge file call mrhead *** load ephemeris -- then close file rewind(leph) call lodorb(orbits) close(leph) leph=6 *** process the epochs call epochs(orbits) *** end of processing write(lst,*) 'End of Processing' end subroutine epochs(orbits) *** process the epochs implicit double precision(a-h,o-z) logical getepch parameter(mxprn=32,mxepch=192) dimension orbits(mxepch + 4*mxprn*mxepch) common/lus/lst,lin,leph,lout common/stugg/pi,rad,sol,f1,f2,fw,w1,w2,ww,we call gettsuf(tmn,tmx,dlt) nloop=idnint((tmx-tmn)/dlt)+1 do 200 iloop=1,nloop if(.not.getepch(orbits,dlt)) return call doit 200 continue return end ************************************************************************ subroutine doit *** do point position with an epoch of data implicit double precision(a-h,o-z) logical xyzgeo common/lus/lst,lin,leph,lout parameter(rnull2=99999999.d0,tiny=1.d-2) parameter(rave=6371000.d0) parameter(mxprn=32) parameter(mxob=12,mxx=3+mxob) dimension is(mxob),js(mxob),cmop(mxob),cy(mxob),cs(3,mxob) dimension an(mxx,mxx),u(mxx),x(mxx),c(mxx),ic(mxx) save /dat0/,/dat1/,/dat2/ dimension iprns(mxprn) common/dat0/nsat,iprns,iprnhi,idxhi dimension r11(mxprn),p11(mxprn),r21(mxprn),p21(mxprn),s1(mxprn), * dx1(mxprn),dy1(mxprn),dz1(mxprn),va1(mxprn),az1(mxprn), * dts1(mxprn) common/dat1/t1,r11,p11,r21,p21,s1,dx1,dy1,dz1,va1,az1,dts1 dimension r12(mxprn),p12(mxprn),r22(mxprn),p22(mxprn),s2(mxprn), * dx2(mxprn),dy2(mxprn),dz2(mxprn),va2(mxprn),az2(mxprn), * dts2(mxprn) common/dat2/t2,r12,p12,r22,p22,s2,dx2,dy2,dz2,va2,az2,dts2 common/stugg/pi,rad,sol,f1,f2,fw,w1,w2,ww,we *** r-range, p-phase, s-model, 1st number-frequency, 2nd number-station *** cmo dd i=idxhi if(r11(i).gt.rnull2.or.r12(i).gt.rnull2.or. * s1(i).lt.tiny .or. s2(i).lt.tiny) return *** first differences (between stations) dri=r12(i)-r11(i) dsi= s2(i)- s1(i) *** direction cosines for station 2(only) cs12=-dx2(i)/s2(i) cs22=-dy2(i)/s2(i) cs32=-dz2(i)/s2(i) *** loop over epoch -- extract observation equation elements nunk=3 nobs=0 do 100 j=1,nsat if(i.ne.j) then if(r11(j).gt.rnull2.or.r12(j).gt.rnull2.or. * s1(j).lt.tiny .or. s2(j).lt.tiny) go to 100 drj=r12(j)-r11(j) dsj= s2(j)- s1(j) ddr=drj-dri dds=dsj-dsi nobs=nobs+1 if(nobs.gt.mxob) stop 39483 is(nobs)=iprns(i) js(nobs)=iprns(j) cmop(nobs)=dds-ddr cs(1,nobs)=-dx2(j)/s2(j) - cs12 cs(2,nobs)=-dy2(j)/s2(j) - cs22 cs(3,nobs)=-dz2(j)/s2(j) - cs32 ***** debug print of model misclosures **** write(lout,'(2i3,f8.1,2f14.2,f7.2)') **** * iprns(j),iprns(i),t1,cmop(nobs),cy(nobs), **** * dabs(cmop(nobs)-cy(nobs)) endif 100 continue if(nobs.le.2) return *** build and solve normals call nitil2(an,u,eltpel,nunk) *** dd phase and ambiguity equations (units of meters) pwt=1.d0/(1.d0*1.d0) *** dd range (meters) nc=3 ic(1)=1 ic(2)=2 ic(3)=3 do 200 i=1,nobs c(1)=cs(1,i) c(2)=cs(2,i) c(3)=cs(3,i) p=pwt cmo=cmop(i) call nrmal2(an,u,c,cmo,p,eltpel,ic,nc,nunk) 200 continue *** end of accumulation -- invert, solve, and update coordinates call fill(an,nunk) call invert(an,nunk) call ab(an,u,x,nunk,nunk,1) call getl12(x20,y20,z20) if(.not.xyzgeo(x20,y20,z20,glat0,glon0,eht0)) stop 90430 x2=x20-x(1) y2=y20-x(2) z2=z20-x(3) if(.not.xyzgeo(x2,y2,z2,glat,glon,eht)) stop 90431 *** output ***** dgla=(glat-glat0)*rave ***** dglo=(glon-glon0)*rave*dcos(glat) ***** deht= eht-eht0 ***** write(lout,9) t1,dgla,dglo,deht,nobs glad=glat*rad glod=glon*rad write(lout,9) t1,glad,glod,eht,nobs 9 format(f8.1,2f15.9,f10.4,i4) return end ************************************************************************ logical function getepch(orbits,delt) *** read an epoch of data implicit double precision(a-h,o-z) common/lus/lst,lin,leph,lout logical comrng parameter(rnull=99999999.999d0,rnull2=99999999.d0) dimension orbits(*) parameter(mxprn=32) save /dat0/,/dat1/,/dat2/ dimension iprns(mxprn) common/dat0/nsat,iprns,iprnhi,idxhi dimension r11(mxprn),p11(mxprn),r21(mxprn),p21(mxprn),s1(mxprn), * dx1(mxprn),dy1(mxprn),dz1(mxprn),va1(mxprn),az1(mxprn), * dts1(mxprn) common/dat1/t1,r11,p11,r21,p21,s1,dx1,dy1,dz1,va1,az1,dts1 dimension r12(mxprn),p12(mxprn),r22(mxprn),p22(mxprn),s2(mxprn), * dx2(mxprn),dy2(mxprn),dz2(mxprn),va2(mxprn),az2(mxprn), * dts2(mxprn) common/dat2/t2,r12,p12,r22,p22,s2,dx2,dy2,dz2,va2,az2,dts2 common/stugg/pi,rad,sol,f1,f2,fw,w1,w2,ww,we *** get l2 to l1 phase center offsets (for l2 range corrections) call getd12(dn121,de121,du121,dn122,de122,du122) *** read epoch header read(lin,*,end=10000) t1,t2,nsat if(nsat.le.0.or.nsat.gt.mxprn) stop 84343 *** compute receiver clock correction (dtr) to nearest epoch targ1=delt*idnint(t1/delt) targ2=delt*idnint(t2/delt) dtr1=t1-targ1 dtr2=t2-targ2 t1=targ1 t2=targ2 *** load data and find "closest sat" *** r11 pseudorange, L1, point 1 *** r12 pseudorange, L1, point 2 *** p11 phase (cy.), L1, point 1 *** p12 phase (cy.), L1, point 2 *** r21 pseudorange, L2, point 1 *** r22 pseudorange, L2, point 2 *** p21 phase (cy.), L2, point 1 *** p22 phase (cy.), L2, point 2 rmin=rnull iprnhi=0 do 100 i=1,nsat read(lin,*) iprns(i),r11(i),r12(i),p11(i),p12(i), * r21(i),r22(i),p21(i),p22(i) *** apply receiver clock correction to nearest epoch (c.f. RINEX2-2.TXT) if(r11(i).lt.rnull2) r11(i)=r11(i)-dtr1*sol if(r21(i).lt.rnull2) r21(i)=r21(i)-dtr1*sol if(p11(i).lt.rnull2) p11(i)=p11(i)-dtr1*f1 if(p21(i).lt.rnull2) p21(i)=p21(i)-dtr1*f1 if(r12(i).lt.rnull2) r12(i)=r12(i)-dtr2*sol if(r22(i).lt.rnull2) r22(i)=r22(i)-dtr2*sol if(p12(i).lt.rnull2) p12(i)=p12(i)-dtr2*f2 if(p22(i).lt.rnull2) p22(i)=p22(i)-dtr2*f2 call getl11 (x,y,z) call getl11g(gla,glo,eht) if(.not.comrng(iprns(i),orbits,t1,x,y,z,gla,glo,r11(i),s1(i), * dx1(i),dy1(i),dz1(i),va1(i),az1(i),dts1(i)))then s1(i)=0.d0 *** orbit ok -- test for high sat (as ref sat support) else if(r11(i).lt.rmin) then rmin =r11(i) iprnhi=iprns(i) idxhi =i endif *** add geometric range corrector to l2 obs at point 1 *** cf. "geometrical calibration term", eq.(4) and eqs.(1), pg. 10188 *** blewitt, "carrier phase ambiguity...", jgr 94(b8), 10187-10203, 10-aug-89 dp12= -dcos(va1(i))*(de121*dsin(az1(i))+dn121*dcos(az1(i))) * -du121*dsin(va1(i)) if(r21(i).lt.rnull2) r21(i)=r21(i) + dp12 if(p21(i).lt.rnull2) p21(i)=p21(i) + dp12/w2 endif *** point 2 call getl12 (x,y,z) call getl12g(gla,glo,eht) if(.not.comrng(iprns(i),orbits,t2,x,y,z,gla,glo,r12(i),s2(i), * dx2(i),dy2(i),dz2(i),va2(i),az2(i),dts2(i)))then s2(i)=0.d0 *** apply geometric range corrector to l2 obs at point 2 else dp12= -dcos(va2(i))*(de122*dsin(az2(i))+dn122*dcos(az2(i))) * -du122*dsin(va2(i)) if(r22(i).lt.rnull2) r22(i)=r22(i) + dp12 if(p22(i).lt.rnull2) p22(i)=p22(i) + dp12/w2 endif 100 continue getepch=.true. return *** end of file -- trying to read header 10000 getepch=.false. return end ************************************************************************ *** header data routines *********************************************** ************************************************************************ subroutine mrhead *** read merge file header implicit double precision(a-h,o-z) character*4 aid1,aid2 dimension tena1(2,19),tena2(2,19) save /hdr/ common/hdr/iyr,imo,idy, * aid1,xmk1,ymk1,zmk1,gak1,gok1,ehk1, * xar1,yar1,zar1,gar1,gor1,ehr1, * xl11,yl11,zl11,ga11,go11,eh11, * xl21,yl21,zl21,ga21,go21,eh21, * tena1,dn121,de121,du121, * aid2,xmk2,ymk2,zmk2,gak2,gok2,ehk2, * xar2,yar2,zar2,gar2,gor2,ehr2, * xl12,yl12,zl12,ga12,go12,eh12, * xl22,yl22,zl22,ga22,go22,eh22, * tena2,dn122,de122,du122, * tmin,tmax,delt common/stugg/pi,rad,sol,f1,f2,fw,w1,w2,ww,we common/lus/lst,lin,leph,lout read(lin,*) iyr,imo,idy 1 format(a4,12x,5f14.4,f10.4) 2 format(4x,12x,5f14.4,f10.4) read(lin,1) aid1,xmk1,ymk1,zmk1,gak1,gok1,ehk1 read(lin,2) xar1,yar1,zar1,gar1,gor1,ehr1 read(lin,2) xl11,yl11,zl11,ga11,go11,eh11 read(lin,2) xl21,yl21,zl21,ga21,go21,eh21 read(lin,'(19f5.1)') (tena1(1,i),i=1,19) read(lin,'(19f5.1)') (tena1(2,i),i=1,19) read(lin,1) aid2,xmk2,ymk2,zmk2,gak2,gok2,ehk2 read(lin,2) xar2,yar2,zar2,gar2,gor2,ehr2 read(lin,2) xl12,yl12,zl12,ga12,go12,eh12 read(lin,2) xl22,yl22,zl22,ga22,go22,eh22 read(lin,'(19f5.1)') (tena2(1,i),i=1,19) read(lin,'(19f5.1)') (tena2(2,i),i=1,19) read(lin,*) tmin,tmax,delt *** convert lat/long to radians gak1=gak1/rad gok1=gok1/rad gar1=gar1/rad gor1=gor1/rad ga11=ga11/rad go11=go11/rad ga21=ga21/rad go21=go21/rad gak2=gak2/rad gok2=gok2/rad gar2=gar2/rad gor2=gor2/rad ga12=ga12/rad go12=go12/rad ga22=ga22/rad go22=go22/rad *** initialize time handler call setjd0(iyr,imo,idy) write(lst,'(''yr,mo,dy = '',i4,2i3)') iyr,imo,idy *** get l2 to l1 offsets in geodetic horizon (of l1 center) dx=xl11-xl21 dy=yl11-yl21 dz=zl11-zl21 call rge(ga11,go11, dn121,de121,du121, dx,dy,dz) dx=xl12-xl22 dy=yl12-yl22 dz=zl12-zl22 call rge(ga12,go12, dn122,de122,du122, dx,dy,dz) return end subroutine getl11(x,y,z) *** return l1 phase center point 1 implicit double precision(a-h,o-z) character*4 aid1,aid2 dimension tena1(2,19),tena2(2,19) save /hdr/ common/hdr/iyr,imo,idy, * aid1,xmk1,ymk1,zmk1,gak1,gok1,ehk1, * xar1,yar1,zar1,gar1,gor1,ehr1, * xl11,yl11,zl11,ga11,go11,eh11, * xl21,yl21,zl21,ga21,go21,eh21, * tena1,dn121,de121,du121, * aid2,xmk2,ymk2,zmk2,gak2,gok2,ehk2, * xar2,yar2,zar2,gar2,gor2,ehr2, * xl12,yl12,zl12,ga12,go12,eh12, * xl22,yl22,zl22,ga22,go22,eh22, * tena2,dn122,de122,du122, * tmin,tmax,delt x=xl11 y=yl11 z=zl11 return end subroutine getl12(x,y,z) *** return l1 phase center point 2 implicit double precision(a-h,o-z) character*4 aid1,aid2 dimension tena1(2,19),tena2(2,19) save /hdr/ common/hdr/iyr,imo,idy, * aid1,xmk1,ymk1,zmk1,gak1,gok1,ehk1, * xar1,yar1,zar1,gar1,gor1,ehr1, * xl11,yl11,zl11,ga11,go11,eh11, * xl21,yl21,zl21,ga21,go21,eh21, * tena1,dn121,de121,du121, * aid2,xmk2,ymk2,zmk2,gak2,gok2,ehk2, * xar2,yar2,zar2,gar2,gor2,ehr2, * xl12,yl12,zl12,ga12,go12,eh12, * xl22,yl22,zl22,ga22,go22,eh22, * tena2,dn122,de122,du122, * tmin,tmax,delt x=xl12 y=yl12 z=zl12 return end subroutine getl21(x,y,z) *** return l2 phase center point 1 implicit double precision(a-h,o-z) character*4 aid1,aid2 dimension tena1(2,19),tena2(2,19) save /hdr/ common/hdr/iyr,imo,idy, * aid1,xmk1,ymk1,zmk1,gak1,gok1,ehk1, * xar1,yar1,zar1,gar1,gor1,ehr1, * xl11,yl11,zl11,ga11,go11,eh11, * xl21,yl21,zl21,ga21,go21,eh21, * tena1,dn121,de121,du121, * aid2,xmk2,ymk2,zmk2,gak2,gok2,ehk2, * xar2,yar2,zar2,gar2,gor2,ehr2, * xl12,yl12,zl12,ga12,go12,eh12, * xl22,yl22,zl22,ga22,go22,eh22, * tena2,dn122,de122,du122, * tmin,tmax,delt x=xl21 y=yl21 z=zl21 return end subroutine getl22(x,y,z) *** return l2 phase center point 2 implicit double precision(a-h,o-z) character*4 aid1,aid2 dimension tena1(2,19),tena2(2,19) save /hdr/ common/hdr/iyr,imo,idy, * aid1,xmk1,ymk1,zmk1,gak1,gok1,ehk1, * xar1,yar1,zar1,gar1,gor1,ehr1, * xl11,yl11,zl11,ga11,go11,eh11, * xl21,yl21,zl21,ga21,go21,eh21, * tena1,dn121,de121,du121, * aid2,xmk2,ymk2,zmk2,gak2,gok2,ehk2, * xar2,yar2,zar2,gar2,gor2,ehr2, * xl12,yl12,zl12,ga12,go12,eh12, * xl22,yl22,zl22,ga22,go22,eh22, * tena2,dn122,de122,du122, * tmin,tmax,delt x=xl22 y=yl22 z=zl22 return end subroutine getl11g(gla,glo,eht) *** return l1 phase center point 1 -- geodetic implicit double precision(a-h,o-z) character*4 aid1,aid2 dimension tena1(2,19),tena2(2,19) save /hdr/ common/hdr/iyr,imo,idy, * aid1,xmk1,ymk1,zmk1,gak1,gok1,ehk1, * xar1,yar1,zar1,gar1,gor1,ehr1, * xl11,yl11,zl11,ga11,go11,eh11, * xl21,yl21,zl21,ga21,go21,eh21, * tena1,dn121,de121,du121, * aid2,xmk2,ymk2,zmk2,gak2,gok2,ehk2, * xar2,yar2,zar2,gar2,gor2,ehr2, * xl12,yl12,zl12,ga12,go12,eh12, * xl22,yl22,zl22,ga22,go22,eh22, * tena2,dn122,de122,du122, * tmin,tmax,delt gla=ga11 glo=go11 eht=eh11 return end subroutine getl12g(gla,glo,eht) *** return l1 phase center point 2 -- geodetic implicit double precision(a-h,o-z) character*4 aid1,aid2 dimension tena1(2,19),tena2(2,19) save /hdr/ common/hdr/iyr,imo,idy, * aid1,xmk1,ymk1,zmk1,gak1,gok1,ehk1, * xar1,yar1,zar1,gar1,gor1,ehr1, * xl11,yl11,zl11,ga11,go11,eh11, * xl21,yl21,zl21,ga21,go21,eh21, * tena1,dn121,de121,du121, * aid2,xmk2,ymk2,zmk2,gak2,gok2,ehk2, * xar2,yar2,zar2,gar2,gor2,ehr2, * xl12,yl12,zl12,ga12,go12,eh12, * xl22,yl22,zl22,ga22,go22,eh22, * tena2,dn122,de122,du122, * tmin,tmax,delt gla=ga12 glo=go12 eht=eh12 return end subroutine getd12(dn1,de1,du1,dn2,de2,du2) *** return l2 to l1 phase center differences implicit double precision(a-h,o-z) character*4 aid1,aid2 dimension tena1(2,19),tena2(2,19) save /hdr/ common/hdr/iyr,imo,idy, * aid1,xmk1,ymk1,zmk1,gak1,gok1,ehk1, * xar1,yar1,zar1,gar1,gor1,ehr1, * xl11,yl11,zl11,ga11,go11,eh11, * xl21,yl21,zl21,ga21,go21,eh21, * tena1,dn121,de121,du121, * aid2,xmk2,ymk2,zmk2,gak2,gok2,ehk2, * xar2,yar2,zar2,gar2,gor2,ehr2, * xl12,yl12,zl12,ga12,go12,eh12, * xl22,yl22,zl22,ga22,go22,eh22, * tena2,dn122,de122,du122, * tmin,tmax,delt dn1=dn121 de1=de121 du1=du121 dn2=dn122 de2=de122 du2=du122 return end subroutine gettsuf(tmn,tmx,dlt) *** return l1 phase center point 2 implicit double precision(a-h,o-z) character*4 aid1,aid2 dimension tena1(2,19),tena2(2,19) save /hdr/ common/hdr/iyr,imo,idy, * aid1,xmk1,ymk1,zmk1,gak1,gok1,ehk1, * xar1,yar1,zar1,gar1,gor1,ehr1, * xl11,yl11,zl11,ga11,go11,eh11, * xl21,yl21,zl21,ga21,go21,eh21, * tena1,dn121,de121,du121, * aid2,xmk2,ymk2,zmk2,gak2,gok2,ehk2, * xar2,yar2,zar2,gar2,gor2,ehr2, * xl12,yl12,zl12,ga12,go12,eh12, * xl22,yl22,zl22,ga22,go22,eh22, * tena2,dn122,de122,du122, * tmin,tmax,delt tmn=tmin tmx=tmax dlt=delt return end ************************************************************************ *** forward model stuff ********************************************** ************************************************************************ logical function comrng(iprn,orbits,t,x,y,z,gla,glo,r,s,dx,dy,dz, * va,az,dts) *** compute range to satellite from point *** input times -- biased receiver time of receipt, t *** input coord -- ECEF coord of a point, x,y,z (also lat/lon) *** input range -- measured pseudorange (for rcvr. time bias), r implicit double precision(a-h,o-z) logical oterp dimension orbits(*) common/lus/lst,lin,leph,lout common/stugg/pi,rad,sol,f1,f2,fw,w1,w2,ww,we *** iterate the light loop a fixed number of times *** dt -- delta time (transit time -- tau) *** tt -- transmit time *** dtr-- receiver time bias *** dts-- satellite time bias dtr=0.d0 dt=0.075d0 do 1 i=1,2 tt=(t-dtr)-dt if(.not.oterp(iprn,tt,orbits,xs,ys,zs,dts,ierr)) then comrng=.false. return endif dx=xs-x dy=ys-y dz=zs-z s=dsqrt(dx*dx+dy*dy+dz*dz) dt=s/sol dtr=r/sol-dt+dts 1 continue *** now apply sagnac effect for the range sag=we*dt dx =xs+sag*ys-x dy =ys-sag*xs-y dz =zs -z s =dsqrt(dx*dx+dy*dy+dz*dz) *** vert angle and azimuth in local geodetic horizon call rge(gla,glo, dn,de,du, dx,dy,dz) va=datan(du/dsqrt(dn*dn+de*de)) az=datan2(de,dn) comrng=.true. return end ************************************************************************ *** geodesy utilities ************************************************** ************************************************************************ subroutine geoxyz(gla,glo,eht,x,y,z) *** convert geodetic lat, long, ellip ht. to x,y,z *** input units of radians, output in meters ******NOTE: e2 is from wgs84 ***********CHECK THIS implicit double precision(a-h,o-z) parameter(a=6378137.d0,e2=0.00669437999013d0) sla=dsin(gla) cla=dcos(gla) w2=1.d0-e2*sla*sla w=dsqrt(w2) en=a/w x=(en+eht)*cla*dcos(glo) y=(en+eht)*cla*dsin(glo) z=(en*(1.d0-e2)+eht)*sla return end logical function xyzgeo(x,y,z,glat,glon,eht) *** convert x,y,z into geodetic lat, lon, and ellip. ht *** input units of meters, output in radians ******NOTE: e2 is from wgs84 ***********CHECK THIS *** ref: eq a.4b, p. 132, appendix a, osu #370 *** ref: geom geod notes gs 658, rapp implicit double precision(a-h,o-z) parameter(maxint=10,tol=5.d-15) parameter(a=6378137.d0,e2=0.00669437999013d0) ae2=a*e2 *** compute initial estimate of reduced latitude (eht=0) p=dsqrt(x*x+y*y) icount=0 tgla=z/p/(1.d0-e2) *** iterate to convergence, or to max # iterations 1 if(icount.le.maxint) then tglax=tgla tgla=z/(p-(ae2/dsqrt(1.d0+(1.d0-e2)*tgla*tgla))) icount=icount+1 if(dabs((tgla-tglax)/tgla).gt.tol) go to 1 *** convergence achieved xyzgeo=.true. glat=datan(tgla) slat=dsin(glat) clat=dcos(glat) glon=datan2(y,x) w=dsqrt(1.d0-e2*slat*slat) en=a/w if(dabs(glat).le.0.7854d0) then eht=p/clat-en else eht=z/slat-en+e2*en endif glon=datan2(y,x) *** too many iterations else xyzgeo=.false. glat=0.d0 glon=0.d0 eht=0.d0 endif return end subroutine rge(gla,glo,u,v,w,x,y,z) *** given a rectangular cartesian system (x,y,z) *** compute a geodetic h cartesian sys (u,v,w) implicit double precision(a-h,o-z) sb=dsin(gla) cb=dcos(gla) sl=dsin(glo) cl=dcos(glo) u=-sb*cl*x-sb*sl*y+cb*z v=- sl*x+ cl*y w= cb*cl*x+cb*sl*y+sb*z return end ************************************************************************ *** orbit load and interpolation *************************************** ************************************************************************ subroutine lodorb(orbits) *** load ephemeris in sp3 ascii format implicit double precision(a-h,o-z) character*60 card character*14 osource parameter(mxprn=32,mxepch=192) dimension orbits(mxepch + 4*mxprn*mxepch) parameter(tiny=1.d-6) save /satstf/ common/satstf/tstr,tstp,dlt,t0mx,imx,npt,n2,nst,nep,it,ix,iy,iz,id common/lus/lst,lin,leph,lout *** number of epochs and number of sats (for loop control -- nloop) nsat=mxprn read(leph,1) iyr,imo,idy,ihr,imn,sec,nepoch,osource 1 format(3x,i4,4i3,f12.8,i8,7x,a14) if(nepoch.lt.9.or.nepoch.gt.mxepch) then write(*,*) 'ephem nepoch error, nepoch= ',nepoch stop 47394 endif read(leph,'(a60)') card read(leph,'(4x,i2)') nloop *** skip over rest of the sp3 format header do 2 i=4,22 2 read(leph,'(a60)') card *** compute indicies for the orbit data block and load common it =1 ix =it+ nepoch iy =ix+nsat*nepoch iz =iy+nsat*nepoch id =iz+nsat*nepoch itotal=id+nsat*nepoch-1 nst=nsat nep=nepoch *** initialize the orbit data block to 0 do 3 i=1,itotal 3 orbits(i)=0.d0 *** master loop over ephemeris file *** read the epoch headers, then find position (P) records *** exit the loop after loading 'nepoch' epochs (counted by nt) nt=0 tstr=+1.d38 tstp=-1.d38 100 read(leph,'(a60)',end=777) card if(card(1:1).eq.'*') then read(card,'(3x,i4,4i3,f12.8)') iyr,imo,idy,ihr,imn,sec call civjts(iyr,imo,idy,ihr,imn,sec,tsec) *** module assumes orbits in monotonically ascending time order nt=nt+1 orbits(nt)=tsec if(tsec.lt.tstr) tstr=tsec if(tsec.gt.tstp) tstp=tsec *** immediately make ephemeris data into meters and seconds *** bypass null markers (dt = 999999.999999) do 10 i=1,nloop 101 read(leph,'(a60)') card if(card(1:1).eq.'*') stop 89756 if(card(1:1).ne.'P'.and.card(1:1).ne.'p') go to 101 read(card,'(1x,i3,4f14.6)') is,x,y,z,dt if(is.lt.1.or.is.gt.mxprn) then write(*,*) 'ephem prn error, prn= ',is stop 47395 endif if(dt.le.999990.d0) then x=x*1000.d0 y=y*1000.d0 z=z*1000.d0 dt=dt/1.d6 call lodor2(nt,is,x,y,z,dt, * orbits(ix),orbits(iy),orbits(iz),orbits(id)) endif 10 continue if(nt.eq.nepoch) go to 777 endif go to 100 *** end of file encountered 777 continue *** set remaining constants for index work dlt=dble( idnint((tstp-tstr)/(nep-1)) ) npt=9 n2=(npt-1)/2 imx=nep-npt+1 t0mx=(imx-1)*dlt+tstr *** initialize interpolation coefficients (hard coded for 9) call itrini return end subroutine lodor2(nt,is,x,y,z,dt,xs,ys,zs,ds) *** place data into storage locations in orbit data block implicit double precision(a-h,o-z) dimension xs(nep,nst),ys(nep,nst),zs(nep,nst),ds(nep,nst) save /satstf/ common/satstf/tstr,tstp,dlt,t0mx,imx,npt,n2,nst,nep,it,ix,iy,iz,id xs(nt,is)=x ys(nt,is)=y zs(nt,is)=z ds(nt,is)=dt return end logical function oterp(iprn,tsec,orbits,x,y,z,dt,ierr) *** interpolation of orbit *** iprn is prn identifier for a satellite *** input time, tsec, is gps time (maintained by usaf) w.r.t. mjd0 *** output (xyz) units of m in orbit reference system *** output (dt) units of sec *** data in /satstf/ loaded by lodsat() *** ierr=1 not currently used *** ierr=2 prn not in orbit file *** ierr=3 time boundary violation (extrapolation prohibited) implicit double precision(a-h,o-z) logical oterp2 dimension orbits(*) save /satstf/ common/satstf/tstr,tstp,dlt,t0mx,imx,npt,n2,nst,nep,it,ix,iy,iz,id *** use indicies to extract sub-arrays of orbits() if(.not.oterp2(iprn,tsec, * orbits(it),orbits(ix),orbits(iy),orbits(iz),orbits(id), * x,y,z,dt,ierr)) then oterp=.false. else oterp=.true. endif return end logical function oterp2(is,tsec,ts,xs,ys,zs,ds,x,y,z,dt,ierr) *** orbit interpolation subroutine (called by oterp) implicit double precision(a-h,o-z) parameter(tiny=1.d-2) dimension ts(nep),xs(nep,nst),ys(nep,nst),zs(nep,nst),ds(nep,nst) save /satstf/ common/satstf/tstr,tstp,dlt,t0mx,imx,npt,n2,nst,nep,it,ix,iy,iz,id common/lus/lst,lin,leph,lout *** is -- prn for a satellite (1 <= is <= nst) *** nst -- number of satellite prns that were loaded *** nep -- number of orbit epochs that were loaded *** this line is bogus -- keep compiler quiet ****************** ts(1)=ts(1) *** satellite prn number not loaded from the sp3 file (1st epoch) r=xs(1,is)*xs(1,is)+ys(1,is)*ys(1,is)+zs(1,is)*zs(1,is) if(r.le.tiny) then ierr=2 oterp2=.false. return endif *** prevent extrapolation if(tsec.lt.tstr.or.tsec.gt.tstp) then ierr=3 oterp2=.false. return endif *** routine for finding index into table *** produces normalized time [0,1,...,8] --> should be 3.5 < tnorm < 4.5 i=(idnint((tsec-tstr)/dlt)+1)-n2 t0=(i-1)*dlt+tstr *** boundary conditions if(i.lt.1) then i=1 t0=tstr elseif(i.gt.imx) then i=imx t0=t0mx endif t=(tsec-t0)/dlt call iterp9(t,xs(i,is),x) call iterp9(t,ys(i,is),y) call iterp9(t,zs(i,is),z) call iterp9(t,ds(i,is),dt) ierr=0 oterp2=.true. return end *********************************************************************** subroutine iterp9(t,xs,x) *** 9-th order interpolator -- equally-spaced points *** time normalized 0,1,...,8 -- should be 3.5 < t < 4.5 *** coefficents, c(), initialized by itrini -- see Ben Remondi dissertation *** patched 19-jan-96 for t exactly equal to 4.0 implicit double precision(a-h,o-z) parameter(eps=1.d-13) dimension xs(9) save /coeff9/ common/coeff9/c(9) *** return stored value if t is exact integer time (avoid division by 0) intt=idnint(t) tint=dble(intt) if(dabs(t-tint).le.eps) then if(intt.ge.0.and.intt.le.8) then x=xs(intt+1) return else stop 88998 endif endif *** normal interpolation powr=1.d0 x=0.d0 do 100 i=1,9 dt=(t-dble(i-1)) powr=dt*powr x=x+xs(i)/(c(i)*dt) 100 continue x=x*powr/24.d0 return end *********************************************************************** * subroutine iterp9(t,xs,x) *** 9-th order interpolator -- equally-spaced points *** time normalized 0,1,...,8 -- should be 3.5 < t < 4.5 *** coefficents, c(), initialized by itrini -- see Ben Remondi dissertation * implicit double precision(a-h,o-z) * dimension xs(9) * save /coeff9/ * common/coeff9/c(9) * powr=1.d0 * x=0.d0 * do 100 i=1,9 * dt=(t-dble(i-1)) * powr=dt*powr * x=x+xs(i)/(c(i)*dt) * 100 continue * x=x*powr/24.d0 * return * end *********************************************************************** subroutine itrini *** initialize coefficients for 9-th order "lagrangean" interpolator *** follows theory of Ben Remondi (c.f. his dissertation) implicit double precision(a-h,o-z) save /coeff9/ common/coeff9/c(9) c(1)=40320.d0 /24.d0 c(2)=-5040.d0 /24.d0 c(3)= 1440.d0 /24.d0 c(4)= -720.d0 /24.d0 c(5)= 576.d0 /24.d0 c(6)= -720.d0 /24.d0 c(7)= 1440.d0 /24.d0 c(8)=-5040.d0 /24.d0 c(9)=40320.d0 /24.d0 return end ************************************************************************ *** time conversion **************************************************** ************************************************************************ subroutine setjd0(iyr,imo,idy) *** set the integer part of a modified julian date as epoch, mjd0 *** the modified julian day is derived from civil time as in civmjd() *** allows single number expression of time in seconds w.r.t. mjd0 implicit double precision(a-h,o-z) integer y save /mjdoff/ common/mjdoff/mjd0 if(iyr.lt.1900) stop 34587 if(imo.le.2) then y=iyr-1 m=imo+12 else y=iyr m=imo endif it1=365.25d0*y it2=30.6001d0*(m+1) mjd=it1+it2+idy-679019 *** now set the epoch for future time computations mjd0=mjd return end subroutine civjts(iyr,imo,idy,ihr,imn,sec,tsec) *** convert civil date to time in seconds past mjd epoch, mjd0 *** requires initialization of mjd0 by setjd0() *** imo in range 1-12, idy in range 1-31 *** only valid in range mar-1900 thru feb-2100 (leap year protocols) *** ref: hofmann-wellenhof, 2nd ed., pg 34-35 *** adapted from civmjd() implicit double precision(a-h,o-z) integer y save /mjdoff/ common/mjdoff/mjd0 if(iyr.lt.1900) stop 34589 if(imo.le.2) then y=iyr-1 m=imo+12 else y=iyr m=imo endif it1=365.25d0*y it2=30.6001d0*(m+1) mjd=it1+it2+idy-679019 tsec=(mjd-mjd0)*86400.d0+3600*ihr+60*imn+sec return end subroutine jtsciv(tsec,iyr,imo,idy,ihr,imn,sec) *** convert time in seconds past mjd0 epoch into civil date *** requires initialization of mjd0 by setjd0() *** imo in range 1-12, idy in range 1-31 *** only valid in range mar-1900 thru feb-2100 *** ref: hofmann-wellenhof, 2nd ed., pg 34-35 *** adapted from mjdciv() implicit double precision(a-h,o-z) save /mjdoff/ common/mjdoff/mjd0 mjd=mjd0+tsec/86400.d0 *** the following equation preserves significant digits fmjd=dmod(tsec,86400.d0)/86400.d0 rjd=mjd+fmjd+2400000.5d0 ia=(rjd+0.5d0) ib=ia+1537 ic=(ib-122.1d0)/365.25d0 id=365.25d0*ic ie=(ib-id)/30.6001d0 *** the fractional part of a julian day is (fractional mjd + 0.5) *** therefore, fractional part of julian day + 0.5 is (fractional mjd) it1=ie*30.6001d0 idy=ib-id-it1+fmjd it2=ie/14.d0 imo=ie-1-12*it2 it3=(7+imo)/10.d0 iyr=ic-4715-it3 tmp=fmjd*24.d0 ihr=tmp tmp=(tmp-ihr)*60.d0 imn=tmp sec=(tmp-imn)*60.d0 return end ********************************************************************8 subroutine civmjd(iyr,imo,idy,ihr,imn,sec,mjd,fmjd) *** convert civil date to modified julian date *** imo in range 1-12, idy in range 1-31 *** only valid in range mar-1900 thru feb-210 (leap year protocols) *** ref: hofmann-wellenhof, 2nd ed., pg 34-35 *** operation confirmed against table 3.3 values on pg.34 implicit double precision(a-h,o-z) integer y if(iyr.lt.1900) stop 34588 if(imo.le.2) then y=iyr-1 m=imo+12 else y=iyr m=imo endif it1=365.25d0*y it2=30.6001d0*(m+1) mjd=it1+it2+idy-679019 fmjd=(3600*ihr+60*imn+sec)/86400.d0 return end subroutine mjdciv(mjd,fmjd,iyr,imo,idy,ihr,imn,sec) *** convert modified julian date to civil date *** imo in range 1-12, idy in range 1-31 *** only valid in range mar-1900 thru feb-2100 *** ref: hofmann-wellenhof, 2nd ed., pg 34-35 *** operation confirmed for leap years (incl. year 2000) implicit double precision(a-h,o-z) rjd=mjd+fmjd+2400000.5d0 ia=(rjd+0.5d0) ib=ia+1537 ic=(ib-122.1d0)/365.25d0 id=365.25d0*ic ie=(ib-id)/30.6001d0 *** the fractional part of a julian day is fractional mjd + 0.5 *** therefore, fractional part of julian day + 0.5 is fractional mjd it1=ie*30.6001d0 idy=ib-id-it1+fmjd it2=ie/14.d0 imo=ie-1-12*it2 it3=(7+imo)/10.d0 iyr=ic-4715-it3 tmp=fmjd*24.d0 ihr=tmp tmp=(tmp-ihr)*60.d0 imn=tmp sec=(tmp-imn)*60.d0 return end ************************************************************************ *** matrix mashers ***************************************************** ************************************************************************ subroutine writev(a,m) *** routine to write a vector implicit double precision(a-h,o-z) dimension a(m) common/lus/lst,lin,leph,lout do 10 i=1,m 10 write(lst,1) a(i) 1 format(' ',1pd30.20) return end subroutine writem(a,m,n) *** routine to write a matrix implicit double precision(a-h,o-z) dimension a(m,n) common/lus/lst,lin,leph,lout do 10 i=1,m 10 write(lst,1) (a(i,j),j=1,n) 1 format(' ',1p4d18.10) return end subroutine vecprd(x,u,n,val) *** compute inner product of two vectors, val = x'u implicit double precision(a-h,o-z) dimension x(n),u(n) val=0.d0 do 1 i=1,n 1 val=val+x(i)*u(i) return end subroutine ab(a,b,r,l,m,n) *** form the matrix product r=ab *** the matrices a and b are returned unchanged implicit double precision(a-h,o-z) dimension a(l,m),b(m,n),r(l,n) do 5 i=1,l do 5 j=1,n r(i,j)=0.d0 do 5 k=1,m 5 r(i,j)=r(i,j)+a(i,k)*b(k,j) return end subroutine invert(a,n) *** routine to invert a matrix in place implicit double precision(a-h,o-z) dimension a(n,n) do 2 i=1,n aii=1.d0/a(i,i) a(i,i)=aii do 2 j=1,n if(i.eq.j) go to 2 aji=a(j,i)*aii a(j,i)=aji do 1 k=1,n if(i.eq.k) go to 1 a(j,k)=a(j,k)-aji*a(i,k) if(j.ne.n) go to 1 a(i,k)=-aii*a(i,k) 1 continue 2 continue ann=-a(n,n) k=n-1 do 3 j=1,k 3 a(n,j)=ann*a(n,j) return end subroutine nrmal2(an,u,d,f,p,bw,lc,nd,n) *** form normals for observation equations one observation at a time *** *****(nx + u = 0)***** *** n=a'pa (n matrix stored in an) *** u=a'pl (l stored in f) *** calling sequence *** 1) call nitial (one time to clear normals) *** 2) call normal (once for each observation) *** 3) call fill (one time to fill in lower triangular) *** *** d = non-zero elements of a row of the a matrix *** lc= column numbers of non-zero elements in d. elements *** of lc must be in ascending order and correspond to *** d, i.e., d(j) is the non-zero element in column lc(j) *** nd= number of elements in d and lc *** n = order of the system (number of unknowns) *** bw= l'pl accumulated (basement window) implicit double precision(a-h,o-z) dimension an(n,n),u(n),d(nd),lc(nd) pf=p*f bw=bw+f*pf do 5 i=1,nd li=lc(i) u(li)=u(li)+d(i)*pf do 5 j=i,nd lj=lc(j) 5 an(li,lj)=an(li,lj) + d(i)*d(j)*p return *** clear normals entry nitil2(an,u,bw,n) do 10 i=1,n u(i)=0.d0 do 10 j=i,n 10 an(i,j)=0.d0 bw=0.d0 return *** fill in the lower triangular portion of an entry fill(an,n) do 15 i=1,n do 15 j=i,n 15 an(j,i)=an(i,j) return end