orbitalfuns.f90

一个基于打靶法的最优控制求解软件 求解过程中采用参数延续算法

  end Subroutine StateDynamics



  !***************************
  ! Compute costate dynamics *
  !***************************
  Subroutine CostateDynamics(dimphi,lambda,x,p,u,f)
    use auxmod
    implicit none

    integer, intent(in) :: dimphi
    real(kind=8), intent(in) :: lambda
    real(kind=8), intent(in), dimension(ns) :: x
    real(kind=8), intent(in), dimension(nc) :: p
    real(kind=8), intent(in), dimension(m) :: u
    real(kind=8), intent(inout), dimension(dimphi) :: f

    !Local
    real (kind=8) :: Zpoint, Apoint, Bpoint, XXpoint, Hpoint
    real (kind=8) :: pf1, pf2, pf3, pf4, pf5
    real (kind=8) :: ZpsZ, p6P3, p6PH3

    p6P3 = p(6)*Pmu0Z*u(3)
    p6PH3 = p6P3*H


    !Dynamics of costate (wrt time t: dpdt)
    pf1 = p(1)*f(1)
    pf2 = p(2)*f(2)
    pf3 = p(3)*f(3)
    pf4 = p(4)*f(4)
    pf5 = p(5)*f(5)

    !p_Pa
    f(ns+1) = -0.5d0/x(1)*(3.d0*pf1+pf2+pf3+pf4+pf5-3.d0*Pmu03*p(6)+p6pH3)

    Zpoint = co
    Apoint = 1.d0 + co**2
    Bpoint = co*si
    
    ZpsZ = Zpoint/Z

    !p_ex
    f(ns+2) = -pf1*ZpsZ + &
         &     p(2)*Pmu0*((Apoint-A*ZpsZ)/Z*u(2)+ZpsZ/Z*x(3)*Hu3)*Tmax +&
         &     p(3)*Pmu0*((Bpoint-B*ZpsZ)/Z*u(2) + (1.d0-x(2)*ZpsZ)/Z*Hu3)*Tmax -&
         &     pf4*ZpsZ - pf5*ZpsZ + &
         &     p(6)*2.d0*sqrt(mu0/x(1)**3)*Z*Zpoint - &
         &     p(6)*Pmu0/Z*ZpsZ*Hu3*Tmax
    f(ns+2) = -f(ns+2)

    Zpoint = si
    Apoint = si*co
    Bpoint = 1.d0+si**2

    ZpsZ = Zpoint/Z

    !p_ey
    f(ns+3) = -pf1*ZpsZ + &
         &    p(2)*Pmu0*((Apoint-A*ZpsZ)/Z*u(2)-(1.d0-x(3)*ZpsZ)/Z*Hu3)*Tmax +&
         &    p(3)*Pmu0*((Bpoint-B*ZpsZ)/Z*u(2)-ZpsZ/Z*x(2)*Hu3)*Tmax -&
         &    pf4*ZpsZ - pf5*ZpsZ + &
         &    p(6)*2.d0*sqrt(mu0/x(1)**3)*Z*Zpoint - &
         &    p(6)*Pmu0/Z*ZpsZ*Hu3*Tmax
    f(ns+3) = -f(ns+3)

    XXpoint = 2.d0*X(4)
    Hpoint = si

    !p_hx
    f(ns+4) = -p(2)*Pmu0*Tmax*x(3)*Hpoint/Z*u(3)+ &
         &    p(3)*Pmu0*Tmax*x(2)*Hpoint/Z*u(3) + &
         &    XXpoint/XX*(pf4+pf5) + &
         &    p(6)*Hpoint*Pmu0/Z*u(3)*Tmax
    f(ns+4) = -f(ns+4)

    XXpoint = 2.d0*X(5)
    Hpoint = -co

    !p_hy
    f(ns+5) = -p(2)*Pmu0*Tmax*x(3)*Hpoint/Z*u(3)+ &
         &    p(3)*Pmu0*Tmax*x(2)*Hpoint/Z*u(3) + &
         &    XXpoint/XX*(pf4+pf5) + &
         &    p(6)*Hpoint*Pmu0/Z*u(3)*Tmax
    f(ns+5) = -f(ns+5)

    Zpoint = -x(2)*si + x(3)*co
    Apoint = -si*(1.d0+Z) + Zpoint*co
    Bpoint = co*(1.d0+Z) + Zpoint*si
    Hpoint = x(4)*co + x(5)*si

    ZpsZ = Zpoint/Z

    !p_L
    f(ns+6) = -pf1*ZpsZ + &
         &    p(2)*Pmu0*(co*u(1)+(Apoint-A*ZpsZ)/Z*u(2)-x(3)*(Hpoint-H*ZpsZ)/Z*u(3))*Tmax+ &
         &    p(3)*Pmu0*(si*u(1)+(Bpoint-B*ZpsZ)/Z*u(2)+x(2)*(Hpoint-H*ZpsZ)/Z*u(3))*Tmax+ &
         &    p(4)*Pmu0Z/2.d0*XX*(-si-co*ZpsZ)*u(3)+ &
         &    p(5)*Pmu0Z/2.d0*XX*(co-si*ZpsZ)*u(3)+&
         &    p(6)*2.d0*Zpoint*sqrt(mu0/x(1)**3)*Z + &
         &    Pmu0*(Hpoint-H*ZpsZ)/Z*u(3)*Tmax*p(6)
    f(ns+6) = -f(ns+6)

    !p_m
    f(ns+7) = (pf1 + pf2 + pf3 + pf4 + pf5 + p(6)*Pmu0/Z*Hu3*Tmax) / x(7)

    !p_tf
    if (nc > 7) then
       !dtf/dt = dtf/ds ds/dt = -dH/dtf * 1/tf
       !note: f(8) = 0 donc pas de pf8...
       tff = x(8)
       if (tff > 0.d0) then
          f(ns+8) = sum(p(1:nc)*f(1:ns)) + lambda*normu+(1.d0-lambda)*normu**2
          f(ns+8) = - f(ns+8) / tff
       else
          f(ns+8) = 0.d0
       end if
    end if

    !p_s
    if (nc > 8) f(ns+9) = 0d0

  end Subroutine CostateDynamics



  !***************************
  ! Compute costate dynamics *
  !***************************
  Subroutine ObjectiveDynamics(dimphi,lambda,x,u,f)
    use auxmod
    implicit none

    integer, intent(in) :: dimphi
    real(kind=8), intent(in) :: lambda
    real(kind=8), intent(in), dimension(ns) :: x
    real(kind=8), intent(in), dimension(m) :: u
    real(kind=8), intent(inout), dimension(dimphi) :: f


    f(ns+nc+1) = Beta*Tmax*normu
    !time (useful only for full longitude formulation)
    if (dimphi > ns+nc+1) f(ns+nc+2) = 1d0

  end Subroutine ObjectiveDynamics
  


  !*****************************************
  ! Compute Hamiltonian (time formulation) *
  !*****************************************
  Subroutine Hamiltonian(dimphi,lambda,x,p,f,u,ham)
    use auxmod
    implicit none

    integer, intent(in) :: dimphi
    real(kind=8), intent(in) :: lambda   
    real(kind=8), intent(in), dimension(ns) :: x
    real(kind=8), intent(in), dimension(nc) :: p
    real(kind=8), intent(in), dimension(dimphi) :: f
    real(kind=8), intent(in), dimension(m) :: u
    real(kind=8), intent(out) :: ham

    !out init
    ham = 0d0

    !Hamiltonian for time formulation (time t, not reduced time s)
    ham = sum(p(1:nc)*f(1:ns)) + lambda*normu + (1.d0-lambda)*normu**2

  end Subroutine Hamiltonian



!!$  !************************************************
!!$  ! Compute Hamiltonian for longitude formulation *
!!$  !************************************************
!!$  Subroutine HamiltonianL(lambda,x,p,u,h)
!!$    implicit none
!!$
!!$    real(kind=8), intent(in) :: lambda   
!!$    real(kind=8), intent(out) :: h
!!$    real(kind=8), intent(in), dimension(ns) :: x
!!$    real(kind=8), intent(in), dimension(nc) :: p
!!$    real(kind=8), intent(in), dimension(m) :: u
!!$
!!$
!!$    ! Local
!!$    real(kind=8) :: normu
!!$    real(kind=8), dimension(ns) :: f
!!$
!!$    call AuxVars(x,p,u)
!!$    call StateDynamics(ns,lambda,x,p,u,f)
!!$    normu = sqrt(u(1)**2+u(2)**2+u(3)**2)
!!$
!!$    !Full longitude Hamiltonian: H^L = dtdL H^t
!!$    h = (sum(p(1:7)*f(1:7)) + lambda*normu + (1.d0-lambda)*normu**2) / f(6)
!!$
!!$  end Subroutine HamiltonianL



  !*************************************
  ! Compute state and costate dynamics *
  !*************************************
  Subroutine Dynamics(dimphi,lambda,x,p,u,f)
    use auxmod
    implicit none

    integer, intent(in) :: dimphi
    real(kind=8), intent(in) :: lambda
    real(kind=8), intent(in), dimension(ns) :: x
    real(kind=8), intent(in), dimension(nc) :: p
    real(kind=8), intent(in), dimension(m) :: u
    real(kind=8), intent(out), dimension(dimphi) :: f

    !Local
    real(kind=8) :: dtdL, hamilt
    real(kind=8), dimension(ns) :: d2tdLdx

    !out init
    f = 0d0

    !reduced time t = tf . s  with s in [0,1]
    !NOTE: LA DYNAMIQUE EST ICI EXPRIMEE A LA BASE POUR LE TEMPS t
    
    
    call AuxVars(x,p,u)
    call StateDynamics(dimphi,lambda,x,p,u,f)
    call CostateDynamics(dimphi,lambda,x,p,u,f)
    if (dimphi > ns+nc) call ObjectiveDynamics(dimphi,lambda,x,u,f)


    select case (longitude)

    case (0)

       if (hamiltonianoutput == 1) call Hamiltonian(dimphi,lambda,x,p,f,u,hamilton)
       if (x(6) == t0) then
          if (hamiltonianoutput == 0) call Hamiltonian(dimphi,lambda,x,p,f,u,hamilton)
          trueH0 = hamilton
          !if (outmode==1) write(0,*) 'trueH0',trueH0
       end if

       !retrieve dy/ds = tf . dy/dt
       f(1:ns+nc) = tff * f(1:ns+nc)
       if (dimphi > ns+nc) f(ns+nc+1:dimphi) = tff * f(ns+nc+1:dimphi)

    case (1)

       if (hamiltonianoutput == 1) call Hamiltonian(dimphi,lambda,x,p,f,u,hamilton)
       if (x(6) == t0) then
          if (hamiltonianoutput == 0) call Hamiltonian(dimphi,lambda,x,p,f,u,hamilton)
          trueH0 = hamilton
          !if (outmode==1) write(0,*) 'trueH0',trueH0
       end if

       !compute derivatives wrt longitude from derivatives wrt time
       if (f(6) > 0d0) then
          dtdL = 1d0 / (f(6) * yscal(6))
       else
          dtdL = 0d0
       endif

       !retrieve dy/dL = dy/dt . dt/dL
       f(1:ns+nc) = dtdL * f(1:ns+nc)
       if (dimphi > ns+nc) f(ns+nc+1:dimphi) = dtdL * f(ns+nc+1:dimphi)

       !variable change (L <- t at x(6))
       f(6) = dtdL / tff


    case (2)

       call Hamiltonian(dimphi,lambda,x,p,f,u,hamilt)
       if (x(6) == t0) then
          Hamilton0 = hamilt
          trueH0 = (hamilt-Hamilton0) / f(6)
          !if (outmode==1) write(0,*) 'trueH0',trueH0
       end if
       if (hamiltonianoutput == 1) hamilton = (hamilt-Hamilton0) / f(6)

       !compute derivatives wrt longitude from derivatives wrt time
       if (f(6) > 0d0) then
          dtdL = 1d0 / (f(6) * yscal(6))
       else
          dtdL = 0d0
       endif

       !retrieve dx/dL = dt/dL . dx/dt   (d2t/dLdp = 0)
       f(1:ns) = dtdL * f(1:ns)
       !NB: dL/dL = 1...
       f(6) = 1d0 

       !compute d2t/dLdx = - d2L/dtdx * dtdL**2
       d2tdLdx(1) = -1.5d0*Pmu03/x(1) + 0.5d0*Tmax/sqrt(mu0*x(1))*Hu3/Z/x(7)   
       d2tdLdx(2) = 2d0*Pmu03*co/Z - Pmu0Z*Hu3*co/Z
       d2tdLdx(3) = 2d0*Pmu03*si/Z - Pmu0Z*Hu3*si/Z
       d2tdLdx(4) = Pmu0Z*si*u(3)
       d2tdLdx(5) = -Pmu0Z*co*u(3)
       d2tdLdx(6) = 2d0*Pmu03*(x(3)*co-x(2)*si)/Z  &
            &                      + Pmu0Z*u(3)*((x(4)*co+x(5)*si)-H*(x(3)*co-x(2)*si)/Z)
       d2tdLdx(7) = - Pmu0Z*Hu3/x(7)
       if (ns > 7) d2tdLdx(8) = 0d0
       if (ns > 8) d2tdLdx(9) = 0d0         

       d2tdLdx = - d2tdLdx * dtdL**2

       !retrieve dp/dL =  dt/dL . dp/dt - (H-H0) . d2t/dLdx      (cf Geometric p262)
       f(ns+1:ns+nc) = dtdL * f(ns+1:ns+nc) - (hamilt-Hamilton0) * d2tdLdx(1:ns)    

       !objective
       if (dimphi > ns+nc) f(ns+nc+1:dimphi) = dtdL * f(ns+nc+1:dimphi)

    case default
       write(0,*) 'ERROR: Dynamics >>> Unknown value for longitude...',longitude
       stop

    end select

       

  end subroutine Dynamics


  Subroutine FinalConditions(y,s,dsdy)
    implicit none
    
    real(kind=8), dimension(xpdim), intent(in) :: y !!IVP final value
    real(kind=8), dimension(ncf), intent(out) :: s !!Shooting function value
    real(kind=8), dimension(ncf,xpdim), intent(out) :: dsdy !!Shooting function derivatives

    !Local
    integer :: i
    
    !out init
    s = 0d0
    dsdy = 0d0

    s(1:ncf) = y(fixedT) / yscal(fixedT) - cf(1:ncf)

    dsdy = 0d0
    do i=1,ncf
       dsdy(i,fixedT(i)) = 1d0 / yscal(fixedT(i))
    end do

  end Subroutine FinalConditions


  Subroutine PreProcess(time,y,x,p)
    implicit none
    real(kind=8), intent(in) :: time
    real(kind=8), intent(in), dimension(ns+nc) :: y
    real(kind=8), intent(out), dimension(ns) :: x
    real(kind=8), intent(out), dimension(nc) :: p

    !out init
    x = 0d0
    p = 0d0

    x(1:ns) = y(1:ns) / yscal(1:ns)  
    p(1:nc) = y(ns+1:ns+nc) / yscal(ns+1:ns+nc)      

    !Force nonnegative orbit parameter and mass
    if (x(1) < 1d-1) x(1) = 1d-1
    if (x(7) < 1d-1) x(7) = 1d-1
 
    !variable change (t <- L at x(6))
    if (longitude == 1)  then
       if (time > 0d0) then 
          x(6) = time
       else 
          x(6) = 0d0
       end if
    end if

  end Subroutine PreProcess



  Subroutine PostProcess(dim,phi)
    implicit none
    integer, intent(in) :: dim
    real(kind=8), intent(inout), dimension(dim) :: phi

    phi(1:ns+nc) = phi(1:ns+nc) * yscal(1:ns+nc)

  end Subroutine PostProcess







end Module SpecFuns