[Fortran] 纯文本查看 复制代码
program xpl
implicit none
type prnu
integer prnnumber
real*8 d(12)
integer fflag(12),ffsnr(12)
end type
integer stime(5),flag,number,dnumber,i,dataty(12),k,j,n,ff
real*8 second,rsecond,sat_coord(12,3)
integer,parameter::nav_fileid=10,fileid=11,outfileid=12,ofileid=13
integer:: error=0
character*255 inputfile_nav,inputfile,outputfile
logical alive
character*80 buffer
character*2 swap
character*1 char
type(prnu)::prn(32)
real*8,parameter:: pi=3.14159263251637
real*8 x,y,z,elv,azi,sat(3),sta(3),e(12),a(12),d(4,4),f_rf,f_cef
. ,p1,p2,hpl,vpl,uere,p_uere(12)
p1=1.0d-9
p2=1.68d-7
write(*,*) "please input obs filename"
read(*,*) inputfile
100 inquire(file=inputfile,exist=alive)
if (alive) then
write(*,*) inputfile,"exist"
else
write(*,*) "inputfile, doesn't exist,please enter again"
read(*,*) inputfile
goto 100
end if
write(*,*) "please input nav filename"
101 read(*,*) inputfile_nav
inquire(file=inputfile,exist=alive)
if (alive) then
write(*,*) inputfile_nav,"exist"
else
write(*,*) "inputfile, doesn't exist,please enter again"
read(*,*) inputfile_nav
goto 101
end if
write(*,*) "please input output filename"
read(*,*) outputfile
open(fileid,file=inputfile)
open(nav_fileid,file=inputfile_nav)
open(outfileid,file=outputfile)
write(outfileid,"(1x,5(1X,a,1X),2(2x,a,2x),1x,a)") "年","月","日",
. "时","分","秒","HPL","VPL"
do while (.true.)
read(fileid,"(A79)",iostat=error) buffer
if(buffer(61:66)=='APPROX') then
read(buffer,"(3(1x,f13.4))") sta(1),sta(2),sta(3)
end if
if (buffer(61:63)=='TIM') then
char=buffer(6:6)
end if
if (buffer(61:61)=='E') exit
end do
do while(.true.)
read(fileid,"(A79)",iostat=error) buffer
if (error/=0) exit
if (buffer(1:1)==">") then
read(buffer,"(2x,i4,4(1x,i2.2),f11.7,2x,i1,i3)")
. stime, second,flag,number
j=1
do i=1,number
read(fileid,"(A79)",iostat=error) buffer
if(buffer(1:1)=="C") then
read(buffer,"(1x,i2)") prn(j)%prnnumber
j=j+1
end if
end do
! write(*,*) prn(j)%prnnumber
! if (buffer(3:3)==char) then
! read(buffer,"(5(1X,I2.2),F11.7,2X,I1,2X,I1,12(1X,I2),F12.9)"
! . ,iostat=error) stime, second,flag,number,
! . prn(1)%prnnumber,prn(2)%prnnumber,prn(3)%prnnumber,
! . prn(4)%prnnumber,prn(5)%prnnumber,prn(6)%prnnumber,
! . prn(7)%prnnumber,prn(8)%prnnumber,prn(9)%prnnumber,
! . prn(10)%prnnumber,prn(11)%prnnumber,
! . prn(12)%prnnumber,rsecond
! do i=1,12
! if (prn(i)%prnner==0) then
! ff=i-1
c go to 111
! else
! ff=i
! end if
! end do
c111 write(12,"(5(1X,I2.2),F11.7,1x,i2)") stime, second,ff
! ff=j-1
do i=1,j
if (prn(i)%prnnumber/=0) then
call cal_foot_point(prn(i)%prnnumber,nav_fileid,
. stime(1),stime(2),stime(3),stime(4),stime(5),second,
. x,y,z)
sat_coord(i,1)=x
sat_coord(i,2)=y
sat_coord(i,3)=z
c write(12,"(i2,3(1x,f24.12))") prn(i)%prnnumber, x,y,z
end if
end do
j=0
i=1
do i=1,ff
c read(fileid,"(i2,3(1x,f24.12))")prn,x,y,z
sat(1)=sat_coord(i,1)
sat(2)=sat_coord(i,2)
sat(3)=sat_coord(i,3)
if( sat_coord(i,1)==0 .and. sat_coord(i,2)==0 .and.
. sat_coord(i,3)==0) then
j=j+1
cycle
else
call cal_elv_azi(sat,sta,elv,azi)
c if (elv>5*pi/180) then
elv=elv*180/pi
call cal_pre_uere(elv,uere)
c write(ofileid,*) prn,elv,uere
if (elv>5) then
e(i-j)=elv*pi/180
a(i-j)=azi
p_uere(i-j)=uere
else
j=j+1
cycle
end if
end if
end do
if ((ff-j)>4) then
call cal_design_g(e,a,p_uere,ff-j,d)
call cal_raly_fun(p1,f_rf)
call dinvnorm(p2,f_cef)
call cal_hpl(d,f_rf,f_cef,hpl,vpl)
C write(outfileid,*) hpl,vpl
write(outfileid,"(5(2X,I2.2),3(1x,F5.2))") stime,second
. ,hpl,vpl
else
cycle
end if
end if
end do
close(fileid)
close(nav_fileid)
close(outfileid)
end
[Fortran] 纯文本查看 复制代码
SUBROUTINE cal_foot_point(obs_prn,fileid,obs_year,obs_month,
. obs_day,obs_hour, obs_minute,obs_second,x,y,z)
implicit none
type cansu1
real*8 a0,a1,a2,a3
end type
type cansu2
integer prn,toc,year,month,day,hour,minute
real*8 second
real*8 svcb,svcd,svcdr
end type
type(cansu1)::p1
type(cansu2)::p2
real*8 :: IODE,crs,delta_n,M0,
. cuc,e,cus,sqrt_a,
. toe,cic,omega,cis,
. i0,crc,w,omegadot,
. idot,l2code,gpsweek,l2pdataflag,
. svaccuracy,svhealth, tgd,iodc,
. transtime,fitinterval,
. svcb,svcd,svcdr,obs_second,
. n0,
n
. m,
. es,
. fs,
. t,
. r,
. u,
. i,
. x0,y0,
. x,y,z
!sec,omega_dot,fi0
character*80 buffer
integer :: j
integer :: nav_prn,obs_prn,nav_year,obs_year,nav_month,obs_month,
. nav_day,obs_day,nav_hour,obs_hour,
. obs_minute,nav_minute
real*8,parameter:: omega_e=7.2921151467d-5
character*15 inputfile,outputfile,outputffile
character*20 c
! integer, parameter ::fileid=101
logical alive
integer ::err=0,fileid
buffer=' '
x=0d0
y=0d0
z=0d0
do while (.true.)
read(fileid,"(A79)") buffer
if (buffer(61:61)=='E') exit
end do
do while(.true.)
if (err/=0) exit
read(fileid,"(a)",iostat=err) buffer
if (buffer(1:2)/=" ") then
read(buffer,"(i2)") nav_prn
if (nav_prn==obs_prn) then
read(buffer,"(3x,i2)") nav_year
if (nav_year==obs_year) then
read(buffer,"(6x,i2)") nav_month
if (nav_month==obs_month) then
read(buffer,"(9x,i2)") nav_day
if (nav_day==obs_day) then
read(buffer,"(12x,2i2)") nav_hour,nav_minute
c if (abs(nav_hour-obs_hour)==1 .or.
c . nav_hour==obs_hour) then
if (abs(nav_hour*60+nav_minute)-
. (obs_hour*60+obs_minute)<70) then
read(buffer,"(I2,1x,I2.2,4(1X,I2),
. F5.1,3D19.12)") p2%prn,
. p2%year,p2%month,p2%day,p2%hour,
. p2%minute,p2%second,svcb,svcd,svcdr
do j=1 ,7
read(fileid,"(3X,4F19.12)")
. p1%a0,p1%a1,p1%a2,p1%a3
select case (j)
case (1)
IODE=p1%a0
crs=p1%a1
delta_n=p1%a2
M0=p1%a3
case(2)
cuc=p1%a0
e=p1%a1
cus=p1%a2
sqrt_a=p1%a3
case(3)
toe=p1%a0
cic=p1%a1
omega=p1%a2
cis=p1%a3
case(4)
i0=p1%a0
crc=p1%a1
w=p1%a2
omegadot=p1%a3
case (5)
idot=p1%a0
l2code=p1%a1
gpsweek=p1%a2
l2pdataflag=p1%a3
case (6)
svaccuracy=p1%a0
svhealth=p1%a1
tgd=p1%a2
iodc=p1%a3
case (7)
transtime=p1%a0
fitinterval=p1%a1
end select
end do
call g_gps(obs_year,obs_month,obs_day,obs_hour,
. obs_minute,obs_second,t)
call calu_fs(svcb,svcd,svcdr,cuc,cus,crc,crs,cic,
. cis,i0,e,sqrt_A,delta_n,m0,toe,t,w,r,u,i)
call cal_orit_scoord(r,u,x0,y0)
call cal_sjdju(omega_e,omegadot,omega,t,
. toe,i,x0,y0,x,y,z)
rewind(unit=fileid)
return
else
rewind(unit=fileid)
return
end if
end if
end if
end if
end if
end if
end do
rewind(unit=fileid)
! close(unit=fileid)
end
SUBROUTINE calu_fs(a0,a1,a2,fcuc,fcus,fcrc,fcrs,fcic,fcis, fi0,
. fe,fsqrt_A,fdelta_n,fm0,ftoe,ft,fw,fr,fu,fi)
implicit none
real*8,parameter:: pi=3.1415926535897932D0
real*8 es,n0,m,n,fsqrt_A,fdelta_n,fm0,ft,ftoe,fs,fe,fw,fu,
. delta_u,delta_r,delta_i,fcuc,fcus,fcrc,fcrs,fcic,fcis,fi0,
. fr,fi,a0,a1,a2,i,j,cf,sf
real,parameter ::GM=3.986005e+14
n0=sqrt(GM)/fsqrt_A**3
n=n0+fdelta_n
m=fm0+n*(ft-ftoe)
es=m+fe*sin(m)
do while (abs(es-j)>1.0d-12)
j=es
es=m+fe*sin(es)
end do
cf=(cos(es)-fe)/(1-fe*cos(es))
sf=sqrt(1-fe**2)*sin(es)/(1-fe*cos(es))
if (cf>0 .and. sf>0) then
fs=atan(sqrt(1-fe**2)*sin(es)/(cos(es)-fe))
else if (cf<0) then
fs=pi+atan(sqrt(1-fe**2)*sin(es)/(cos(es)-fe))
else if (cf>0 .and. sf<0) then
fs=2*pi+atan(sqrt(1-fe**2)*sin(es)/(cos(es)-fe))
end if
c fs=sqrt((1+fe)/(1-fe))*tan(es/2)
c fs=2*ATAN(fs)
fu=fw+fs
delta_u=fcuc*cos(2*fu)+fcus*sin(2*fu)
delta_r=fcrc*cos(2*fu)+fcrs*sin(2*fu)
delta_i=fcic*cos(2*fu)+fcis*sin(2*fu)
fu=fu+delta_u
fr=fsqrt_A**2*(1-fe*cos(es))+delta_r
fi=fi0+delta_i
return
end
SUBROUTINE cal_sjdju(omega_e,omega_dot,omega,t,toe,i,x0,y0,x,y,z)
real*8 omega_dot,omega,t,toe,i,x0,y0,x,y,z,l,omega_e
l=omega+(omega_dot-omega_e)*t-omega_dot*toe
x=x0*cos(l)-y0*sin(l)*cos(i)
y=x0*sin(l)+y0*cos(l)*cos(i)
z=y0*sin(i)
return
end
SUBROUTINE g_gps(y,m,d,h,min,sec,ftow)
implicit none
real*8 sec,ftow,jd,hh
integer y,m,d,h,min,fwn
if (y>0 .and. y<80) then
y=y+2000
else
y=y+1900
end if
if (m<=2) then
y=y-1
m=m+12
else
y=y
m=m
end if
jd=INT(365.25*y)+INT(30.6001*(m+1))+d+1720981.5
fwn=INT((jd-2444244.5)/7)
ftow=(jd-2444244.5-7*fwn)*86400.0+h*3600+min*60+sec
if (m>12) then
y=y+1
m=m-12
else
y=y
m=m
end if
if (y>2000 ) then
y=y-2000
else
y=y-1900
end if
return
end
SUBROUTINE cal_orit_scoord(fr,fu,x0,y0)
real*8 fr,fu,x0,y0
x0=fr*cos(fu)
y0=fr*sin(fu)
z0=0
return
end