飛船相對測站的位置矢量單位矢量在赤道坐標(biāo)系中的三個分量
real(kind=8)::lamta(17)
real(kind=8)::mu(17)
real(kind=8)::nu(17)
real(kind=8)::tr(0:17),s(0:17) !觀測時刻對應(yīng)的儒略世紀(jì)數(shù)和格林尼治平恒星時
real(kind=8)::obspos(17,3) !測站的地心赤道坐標(biāo)
real(kind=8)::para(17,3,6)
real(kind=8)::rightv(17,3),f(17),g(17)
real(kind=8) :: f_dot(17),g_dot(17)
integer(kind=4)::i,j
!自定義線性方程系數(shù)行列式的維數(shù)
real(kind=8) :: left(6,6),antileft(6,6)
real(kind=8) :: right(6)
real(kind=8) :: ans(6) !飛船的位置矢量和速度矢量
!real(kind=8) :: orbitpara(6) !飛船軌道的軌道根數(shù)
t(0)=28664.0
open(10,file="data.txt")!讀入數(shù)據(jù)
do i=1,17
  read(10,*) t(i),a(i),h(i)
end do
do i=0,17
  tr(i)=(t(i)/(24*3600)+365*2+28+29+31+1)/36525
end do
!計算觀測時刻的格林尼治平恒星時
call ut_s (tr,s)
call obs_pos(s,obspos)
call lmn(lamta,mu,nu,a,h)
print *, mu(1),nu(1)
do i=1,17
  f(i)=1
  g(i)=t(i)-t(0)
end do
100 do i=1,17
    para(i,1,1)=0
    para(i,1,2)=0
    para(i,1,3)=f(i)*nu(i)
    !print *, f(i),nu(i),para(i,1,3)
    para(i,1,4)=g(i)*nu(i)
    para(i,1,5)=-f(i)*mu(i)
    para(i,1,6)=-g(i)*mu(i)
    para(i,2,1)=f(i)*nu(i)
    para(i,2,2)=g(i)*nu(i)
    para(i,2,3)=0
    para(i,2,4)=0
    para(i,2,5)=-f(i)*lamta(i)
    para(i,2,6)=-g(i)*lamta(i)
    para(i,3,1)=f(i)*mu(i)
    para(i,3,2)=g(i)*mu(i)
    para(i,3,3)=-f(i)*lamta(i)
    para(i,3,4)=-g(i)*lamta(i)
    para(i,3,5)=0
    para(i,3,6)=0
    rightv(i,1)=nu(i)*obspos(i,2)-mu(i)*obspos(i,3)
    rightv(i,2)=nu(i)*obspos(i,1)-lamta(i)*obspos(i,3)
    rightv(i,3)=mu(i)*obspos(i,3)-lamta(i)*obspos(i,2)
    
  end do
!自己設(shè)置矩陣right,left,n
!print*, para(1,1,3)
  do j=1,6
    left(1,j)=para(1,2,j)
    left(2,j)=para(1,3,j)
    left(3,j)=para(9,1,j)
    left(4,j)=para(9,3,j)
    left(5,j)=para(17,1,j)
    left(6,j)=para(17,2,j)
end do
      right(1)=rightv(1,2)
    right(2)=rightv(1,3)
    right(3)=rightv(9,1)
    right(4)=rightv(9,3)
    right(5)=rightv(17,1)
    right(6)=rightv(17,2)
do i=1,6
  print*, left(i,1),left(i,2),left(i,3),left(i,4),left(i,5),left(i,6)
end do
do i=1,6
  print*, right(i)
end do
!write(*,*) t(0)
!write(*,*) ans(1)**2+ans(3)**2+ans(5)**2
!根據(jù)定點(diǎn)數(shù)據(jù)確定是不是需要法化參數(shù)
!call transposematrix(left,antileft)
!call multi(left,antileft)
!call laplace(left,right,ans,f,g,s)
!laplace方法定軌

call state(left,right,ans)
!print *, ans(1)**2+ans(3)**2+ans(5)**2
!print *, left(1,3)
do i=1,17
f_dot(i)=1-(t(i)-t(0))**2/(2*(sqrt(ans(1)**2+ans(2)**2+ans(3)**2))**3)
g_dot(i)=t(i)-t(0)-(t(i)-t(0))**3/(6*(sqrt(ans(1)**2+ans(2)**2+ans(3)**2))**3)
end do
do i=1,17
if (.not.((abs(f_dot(i)-f(i))<1d-7).and.(abs(g_dot(i)-g(i))<1d-7))) then
  do j=1,17
    f(i)=f_dot(i)
    g(i)=g_dot(i)
       print *, f(i),g(i) 
    end do
    stop
    goto 100
end if
end do
write(*,*) ans(1)**2+ans(3)**2+ans(5)**2
stop
end
!write(*,*) t(0)
!call orbital_para (ans)


!世界時和格林尼治恒心時轉(zhuǎn)化子函數(shù)
subroutine ut_s (t,s)
real(kind=8)::t(0:17),s(0:17)
real(kind=8),parameter::pi=3.14159265
integer(kind=4)::i
do i=0,17
  s(i)=((18.6973746+879000.0513367*t(i))*15+(0.093104*t(i)**2+6.2D-6*t(i)**2)/240)*pi/180
end do
end

!計算測站赤道直角坐標(biāo)系中的位置矢量
subroutine obs_pos(s,obspos)
real(kind=8),parameter::ae=6378140,pi=3.14159 !地球半長徑
real(kind=8)::long=120.0/180.0*pi,lat=36.0/180.0*pi,high=40 !測站的經(jīng)度,緯度和高度
real(kind=8)::s(17),obspos(17,3)
integer(kind=4)::i,j
do i=1,17
  obspos(i,1)=(ae+high)*cos(s(i))*cos(long)*cos(lat)-sin(s(i))*sin(long)*cos(lat)
  obspos(i,2)=(ae+high)*sin(s(i))*cos(long)*cos(lat)+cos(s(i))*sin(long)*cos(lat)
  obspos(i,3)=(ae+high)*sin(lat)
end do
end

!計算飛船相對測站的位置矢量單位矢量在赤道坐標(biāo)系中的三個分量
subroutine lmn(lamta,mu,nu,a,h)
implicit none
real(kind=8),parameter:: pi=3.1415926
real(kind=8)::long=120.0/180.0*pi,lat=36.0/180.0*pi,high=40
real(kind=8)::lamta(17)
real(kind=8)::mu(17)
real(kind=8)::nu(17)
real(kind=8)::a(17)
real(kind=8)::h(17)
integer(kind=4)::i,j
do i=1,17
  lamta(i)=(sin(lat))**2*cos(h(i))*cos(a(i))-cos(lat)*sin(lat)*sin(h(i))&
  &-cos(h(i))*sin(a(i))*cos(lat)
  mu(i)=cos(lat)*(sin(lat)*cos(h(i))*cos(a(i))-cos(lat)*sin(h(i)))+&
  & sin(lat)*cos(h(i))*sin(a(i))
  nu(i)=-cos(lat)*cos(h(i))*cos(a(i))+sin(lat)*sin(h(i))
end do
end



!解線性方程組
subroutine state(left,right,ans)
implicit none
real(kind=8) :: left(:,:)
real(kind=8) :: right(:)
real(kind=8) :: ans(:)
real,allocatable :: temp(:,:)
integer :: i,n
n=size(left,1)
allocate(temp(n,n))
temp=left
ans=right
call uptrimatrix(temp,ans,n)
call lowtrimatrix(temp,ans,n)
do i=1,n
ans(i)=ans(i)/temp(i,i)
end do
return
end

!求矩陣的上三角矩陣
subroutine uptrimatrix(temp,ans,n)
implicit none
integer::n
real :: temp(n,n)
real :: ans(n)
integer::i,j
real :: e
do i=1,n-1
do j=i+1,n
  e=temp(j,i)/temp(i,i)
  temp(j,i:n)=temp(j,i:n)-temp(i,i:n)*e
  ans(j)=ans(j)-ans(i)*e
end do
end do
return
end

!求矩陣的下三角矩陣
subroutine lowtrimatrix(temp,ans,n)
implicit none
integer::n
real :: temp(n,n)
real :: ans(n)
integer::i,j
real :: e
do i=n,2,-1
do j=i-1,1,-1
  e=temp(j,i)/temp(i,i)
  temp(j,i:n)=temp(j,i:n)-temp(i,i:n)*e
  ans(j)=ans(j)-ans(i)*e
end do
end do
return
end

!求矩陣的轉(zhuǎn)置矩陣
subroutine transposematrix(a,antia)
implicit none
real :: a(:,:)
real :: antia(:,:)
integer m,n,i,j
m=size(a,1)
n=size(a,2)
do i=1,m
do j=1,n
antia(j,i)=a(i,j)
end do
end do
return
end 

!計算矩陣乘法
subroutine multi(left,antileft)
implicit none
real ::left(:,:),antileft(:,:)
integer:: m,n
integer i,j,k,l
real :: ans(6,6)
n=size(left,1)
m=size(left,2)
do i=1,n
do j=1,n
      do l=1,m
      ans(i,j)=ans(i,j)+left(i,l)*antileft(l,j)
      end do
end do
end do
do i=1,n
do j=1,n
    left(i,j)=ans(i,j)
end do
end do
end