📄 qiebi xuefu.f90
字号:
PROGRAM QIE_BXF !PRN卫星号,N几皆拟合,X还是Y坐标
implicit none
integer prn,nian,yue,ri,i,nj,k,toc,shi,fen,WN,J
integer::status=0,NHS=3,PRN1,NN,OT
REAL*8 wpian,wpiao,wpsu,iode,crs,on,m0,cuc,e,cus,sqrta,toe,cic,omEga,cis,i0,crc,w,omegad,miao
REAL*8 idot,l2,l2p,wjing,wjian,tgd,iodc,fassk,nhqj,x,y,z,gm,Gpszh,T,TOW
REAL*8 n,n0,M,F,U,SU,SR,SI,R,IL,JD,EK,WE,L,XK,YK,ZK,TX,TQ,XXX(1,1),YYY(1,1),ZZZ(1,1)
REAL*8 ,ALLOCATABLE::XS(:,:),YS(:,:),ZS(:,:),CX(:),SJ(:),TT(:,:),TTT(:,:),TTTT(:,:)
REAL*8 ,ALLOCATABLE::XXS(:,:),YYS(:,:),ZZS(:,:),TTTTT(:,:),TTX(:,:),TTXX(:,:),TXXX(:,:)
character(len=79):: string,forr
character(13):: chara,for
logical::tuichu=.false.
parameter(gm=3.9860047e14,We=.72921151467E-04,PRN1=10) !TQ=551412 !gm地球引力常数,WE地球自传角速度
for="END OF HEADER"
open(unit=10,file="acor1000.08n",iostat=status)
open(unit=11,file="f17390700.nav",status="replace",iostat=status)
I=0
do while(.true.)
if(.not.tuichu)then !遇到给定值时开始录如数据
read(10,'(a79)',iostat=status)string
chara=string(61:73)
if(chara==for)then !判断条件，符合即开始录入数据
tuichu=.true.
endif
end if
if(tuichu)then
!数据赋值顺序意义同 P192（GPS测量与数据处理）
read(10,'(i2,1x,i2.2,4(1x,i2),f5.1,3d19.12/,3x,4d19.12/,3x,4d19.12/,3x,4d19.12/,&
3x,4d19.12/,3x,4d19.12/,3x,4d19.12/,3x,2d19.12)',iostat=status)prn,nian,yue,&
ri,shi,fen,miao,wpian,wpiao,wpsu,iode,crs,on,m0,cuc,e,cus,sqrta,toe,cic,omEga,cis,&
i0,crc,w,omegad,idot,l2,gpszh,l2p,wjing,wjian,tgd,iodc,fassk,nhqj
IF(PRN1==PRN)THEN
I=I+1
ENDIF
IF(STATUS/=0)EXIT
ENDIF
end do
NN=I
!WRITE(*,*)"数量为：",NN
TUICHU=.FALSE.
! NN=40
ALLOCATE(XS(NN,1))
ALLOCATE(YS(NN,1))
ALLOCATE(ZS(NN,1))
ALLOCATE(SJ(NN)) ! 纪录起始和结束时间
IF(NN<=NHS)THEN
WRITE(*,*)"阶数大于观测数，无法计算系数"
STOP
END IF
REWIND(unit=10,iostat=status) !读取位置重置文件头
I=0
do while(.true.)
if(.not.tuichu)then !遇到给定值时开始录如数据
read(10,'(a79)',iostat=status)string
chara=string(61:73)
if(chara==for)then !判断条件，符合即开始录入数据
tuichu=.true.
endif
end if
if(tuichu)then
read(10,'(i2,1x,i2.2,4(1x,i2),f5.1,3d19.12/,3x,4d19.12/,3x,4d19.12/,3x,4d19.12/,&
3x,4d19.12/,3x,4d19.12/,3x,4d19.12/,3x,2d19.12)',iostat=status)prn,nian,yue,&
ri,shi,fen,miao,wpian,wpiao,wpsu,iode,crs,on,m0,cuc,e,cus,sqrta,toe,cic,omEga,&
cis,i0,crc,w,omegad,idot,l2,gpszh,l2p,wjing,wjian,tgd,iodc,fassk,nhqj
IF(STATUS/=0)EXIT
IF(PRN1==PRN)THEN
I=I+1
! write(*,'(i2,1x,i2.2,4(1x,i2),f5.1,3d19.12/,3x,4d19.12/,3x,4d19.12/,3x,4d19.12/,&
! 3x,4d19.12/,3x,4d19.12/,3x,4d19.12/,3x,2d19.12)')prn,nian,yue,ri,shi,fen,&
! miao,wpian,wpiao,wpsu,iode,crs,on,m0,cuc,e,cus,sqrta,toe,cic,omega,cis,i0,&
! crc,w,omegad,idot,l2,gpszh,l2p,wjing,wjian,tgd,iodc,fassk,nhqj
! WRITE(*,'(A,D18.12)')"M0=",M0
!第一轨道参数 平均角速度
N0=SQRT(GM)/(SQRTA**3)
N=N0+ON
! WRITE(*,'(A,I2,A,D18.12)')"第",PRN,"颗卫星的平均角速度n：",n
CALL G_GPS(NIAN,YUE,RI,SHI,FEN,MIAO,WN,T)
! IF(WN/=GPSZH)THEN
! WN=WN-1
! T=T+604800
! END IF
! WRITE(*,*)T
! DO I=1,40
! T=T-180+180*I
SJ(I)=T !把时间赋给数组SJ(NN)
! TOE=SJ(I)
WRITE(*,*)"******",T,"*****"
WRITE(*,*)TOE
! WRITE(10,'(D18.12)')M0
M=M0+N*(T-TOE)
! WRITE(*,'(A,D18.12)')"平近点角M：",M
!计算偏近点角
CALL P_JDJ(M,E,EK)
! WRITE(*,'(A,D18.12)')"偏近点角E：",EK
!计算真近点角
F=ATAN(SQRT(1-E*E)*SIN(EK)/(COS(EK)-E))
! WRITE(*,'(A,D18.12)')"真近点角F：",F
!计算生交距角U'
U=W+F
! WRITE(*,'(A,D18.12)')"生交距角U：",u
!计算摄动改正项SU,SR,SI
SU=CUC*COS(2*U)+CUS*SIN(2*U)
SR=CRC*COS(2*U)+CRS*SIN(2*U)
SI=CIC*COS(2*U)+CIS*SIN(2*U)
! WRITE(*,'(3(A,D21.15))')"SU:",SU,"SR:",SR,"SI：",SI
!对u',r',i0'进行摄动改正
U=U+SU
R=SQRTA*SQRTA*(1-E*COS(EK))+SR
IL=I0+SI+OMEGAD*(T-TOE) !IL即i0
! WRITE(*,'(3(A,D21.15))')"摄动改正项U':",U,"R:",R,"I：",IL
!计算卫星在轨道面坐标系中的位置:
XK=R*COS(U)
YK=R*SIN(U)
! WRITE(*,'(A/,A,D20.12,2X,A,D20.11)')"卫星在轨道平面直角坐标系中的坐标为：","X=",XK,"Y=",YK
!计算观测瞬间升交点的经度L:
L=OMEGA+(OMEGAD-WE)*T-OMEGAD*TOE
! WRITE(*,'(A,D19.12)')"升交点经度L:",L
!计算卫星在瞬时地球坐标系中的位置
X=XK*COS(L)-YK*COS(IL)*SIN(L)
Y=XK*SIN(L)+YK*COS(IL)*COS(L)
Z=YK*SIN(IL)
! WRITE(*,'(3(A,D18.12/))')"X=",X,"Y=",Y,"Z=",Z
XS(I,1)=X
YS(I,1)=Y
ZS(I,1)=Z
! END DO
ENDIF
ENDIF
end do
! DO J=1,NN
! XS(J,1)=J
! END DO
WRITE(*,*)"*********************************************"
WRITE(*,*)XS
WRITE(*,*)"*********************************************"
WRITE(*,*)YS
WRITE(*,*)"*********************************************"
WRITE(*,*)ZS
ALLOCATE(TT(NN,NHS+1))
ALLOCATE(TTT(NHS+1,NN))
ALLOCATE(TTTT(NHS+1,NHS+1))
ALLOCATE(TXXX(NHS+1,NHS+1))
ALLOCATE(TTXX(NHS+1,NHS+1))
ALLOCATE(TTTTT(NHS+1,NN))
ALLOCATE(XXS(NHS+1,1))
ALLOCATE(YYS(NHS+1,1))
ALLOCATE(ZZS(NHS+1,1))
ALLOCATE(TTX(1,NHS+1))
!N阶切比雪夫多项式系数计算
OT=SJ(NN)-SJ(1) !求计算的时间差，用末时间-最初时间OT=T-TO P36(GPS测量与数据处理)
! WRITE(*,*)"OT=",OT
DO I=1,NN
TX=2*(SJ(I)-SJ(1))/OT-1
! WRITE(*,*)TX
TT(I,1)=1
TT(I,2)=TX
DO J=3,NHS+1
TT(I,J)=2*TX*TT(I,J-1)-TT(I,J-2)
END DO
END DO
! CALL XIANSHI(TT,NN,NHS+1)
! WRITE(*,*)"********************************"
TTT=TRANSPOSE(TT) !转置库函数TRANSPOSE()
! CALL XIANSHI(TTT,NHS+1,NN)
! WRITE(*,*)"********************************"
CALL MATMUL(TTTT,TTT,NHS+1,NN,TT,NN,NHS+1)
! WRITE(*,'(4D19.12)')TTTT
! WRITE(*,*)"********************************"
DO I=1,NHS+1
DO J=1,NHS+1
TXXX(J,I)=TTTT(J,I)
END DO
END DO
! CALL XIANSHI(TXXX,NHS+1,NHS+1)
! WRITE(*,*)"********************************"
CALL INV(TTTT,NHS+1)
! CALL XIANSHI(TTTT,NHS+1,NHS+1)
! WRITE(*,'(4D19.12)')TTTT
! WRITE(*,*)"********************************"
CALL MATMUL(TTXX,TXXX,NHS+1,NHS+1,TTTT,NHS+1,NHS+1)
! CALL XIANSHI(TXXX,NHS+1,NHS+1)
! WRITE(*,'(4D19.12)')TTXX
! WRITE(*,*)"&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&"
CALL MATMUL(TTTTT,TTTT,NHS+1,NHS+1,TTT,NHS+1,NN)
! CALL XIANSHI(TTTTT,NHS+1,NN)
! WRITE(*,'(4D19.12)')TTTTT
! WRITE(*,*)"********************************"
CALL MATMUL(XXS,TTTTT,NHS+1,NN,XS,NN,1)
CALL MATMUL(YYS,TTTTT,NHS+1,NN,YS,NN,1)
CALL MATMUL(ZZS,TTTTT,NHS+1,NN,ZS,NN,1)
! write(*,*)"********************************"
! WRITE(*,*)XXS
! WRITE(*,*)YYS
! WRITE(*,*)ZZS
TQ=SJ(3)
! WRITE(*,*)"TQ=",TQ
TX=2*(TQ-SJ(1))/OT-1
! WRITE(*,*)TX
TTX(1,1)=1
TTX(1,2)=TX
DO J=3,NHS+1
TTX(1,J)=2*TX*TTX(1,J-1)-TTX(1,J-2)
END DO
WRITE(*,*)"***************************************************************"
! CALL XIANSHI(TTX,1,NHS+1)
! WRITE(*,*)"********************************"
CALL MATMUL(XXX,TTX,1,NHS+1,XXS,NHS+1,1)
CALL MATMUL(YYY,TTX,1,NHS+1,YYS,NHS+1,1)
CALL MATMUL(ZZZ,TTX,1,NHS+1,ZZS,NHS+1,1)
WRITE(*,*)XXX,YYY,ZZZ
DEALLOCATE(XS)
DEALLOCATE(YS)
DEALLOCATE(ZS)
DEALLOCATE(TT)
DEALLOCATE(TTT)
DEALLOCATE(TTTT)
DEALLOCATE(TTTTT)
DEALLOCATE(XXS)
DEALLOCATE(YYS)
DEALLOCATE(ZZS)
DEALLOCATE(SJ)
DEALLOCATE(TTX)
close(10)
! close(11)
stop
end ⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -