goddardfuns.f90

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

             elseif (alpha > upper(1)) then
                alpha = upper(1)
                if (outmode > 0) write(0,*) 'Singular control above upper bound !'
             end if

             u(1:m) = - alpha * comp(1:m) / norm_comp

          end if


          case default
             write(0,*) 'ERROR: Control >>> No control provided for controlmode...',controlmode
             stop

          end select   


       case (2,4)

          !Atmosphere/gravity  homotopy (equivalent to lambda=0 for quadratic perturbation homotopy)
          coeffc = 2d0

          !Set coefficient for ||u|| term depending on criterion formulation

          if (norm_comp <= coeff) then
             u(1:m) = 0d0
          elseif (norm_comp <= coeff + coeffc) then
             u(1:m) = - comp(1:m) * (norm_comp-coeff) / (coeffc*norm_comp)
          else
             u(1:m) = - comp(1:m) / norm_comp
          end if


    case default
       write(0,*) 'ERROR: Control >>> Unknown homotop...',homotop
       stop

    end select

  end subroutine Control


  Subroutine Switch(lambda,x,p,psi)
    use auxmod
    implicit none
    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(out) :: psi


    !Local
    real(kind=8) :: coeff, norm_comp
    real(kind=8) :: comp(m)

    call AuxVars(lambda,x,p)

    if (homotop == 1 .or. homotop == 2) then
       coeff = 1d0 - beta * Tmax * pm
    else
       coeff = - beta * Tmax * pm
    end if

    comp(1:m) = p(m+1:2*m) * Tmax / mass
    norm_comp =  sqrt(sum(comp(1:m)*comp(1:m)))
    
    psi = coeff - norm_comp

  end Subroutine Switch


  Subroutine Switchdot(lambda,x,p,u,psidot)
    use auxmod
    implicit none
    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) :: psidot

    call AuxVars(lambda,x,p)

    if (normv == 0d0 .or. normpv == 0d0) then
       psidot = 0d0
    else
       psidot = 1 / normpv * (Kex * (-(2 * pva ** 2 + pvb ** 2 + pvc ** 2) *& 
            &va ** 2 - 2 * pva * (pvc * vc + vb * pvb) * va - (pva ** 2 + pvc *&
            &* 2 + 2 * pvb ** 2) * vb ** 2 - 2 * pvb * vb * vc * pvc - vc ** 2& 
            &* (pva ** 2 + pvb ** 2 + 2 * pvc ** 2)) + ((beta * dragc * normpv& 
            &+ prc * mass) * pvc + (beta * draga * normpv + pra * mass) * pva +&
            & (beta * dragb * normpv + prb * mass) * pvb) * normv) / normv
       
       psidot = psidot * Tmax / mass**2
    end if
 
    
  end Subroutine Switchdot


  Subroutine SwitchGradient(lambda,y,gradpsi)
    implicit none
    real(kind=8), intent(in) :: lambda      
    real(kind=8), intent(in), dimension(ns+nc) :: y         
    real(kind=8), intent(out), dimension(ns+nc) :: gradpsi  

    !Local
    real(kind=8) :: pv(m), normpv, mass

    !out init
    gradpsi = 0d0

!    if (homotop == 1) then
!       coeff = 1d0 - beta * Tmax * pm
!    else
!       coeff = - beta * Tmax * pm
!    end if
!        comp(1:m) = p(m+1:2*m) * Tmax / mass
!    norm_comp =  sqrt(sum(comp(1:m)*comp(1:m)))
!    
!    psi(1) = coeff - norm_comp

    gradpsi = 0d0

    mass = y(2*m+1) / yscal(2*m+1)
    pv = y(ns+m+1:ns+2*m) / yscal(ns+m+1:ns+2*m)
    normpv = sqrt(sum(pv*pv))

    gradpsi(2*m+1) = normpv * Tmax / mass**2
    gradpsi(ns+2*m+1) = - beta * Tmax 
    gradpsi(ns+m+1:ns+2*m) = - Tmax / mass * pv(1:m) / normpv

    gradpsi = gradpsi / yscal(1:ns+nc)

  end Subroutine SwitchGradient

  Subroutine InitAltcontrol(lambda,alpha,x,p,u)
    implicit none   
    real(kind=8), intent(in) :: lambda      
    real(kind=8), intent(inout) :: alpha  
    real(kind=8), intent(in), dimension(ns) :: x         
    real(kind=8), intent(in), dimension(nc) :: p 
    real(kind=8), intent(inout), dimension(m) :: u  

    !Local
    integer :: controlmodebak
    real(kind=8) :: normu
    
    if (arcentry == 1) then
       alpha = (lower(1) + upper(1)) / 2d0
       arcentry = 0
    else
       alpha = prevalpha
    end if

    controlmodebak = controlmode
    controlmode = 0
    call Control(lambda,x,p,u)
    controlmode = controlmodebak
    normu = sqrt(sum(u*u))   
    if (normu < 1d-10) then
       u = 0d0 
    else
       u = u / normu * alpha
    end if

    !write(0,*) 'Initalt',alpha,u

  end Subroutine InitAltcontrol


  Subroutine UpdateAltcontrol(lambda,newalpha,x,p,u)
    implicit none   
    real(kind=8), intent(in) :: lambda      
    real(kind=8), intent(in) :: newalpha  
    real(kind=8), intent(in), dimension(ns) :: x         
    real(kind=8), intent(in), dimension(nc) :: p 
    real(kind=8), intent(inout), dimension(m) :: u  

    !Local  
    integer :: controlmodebak
    real(kind=8) :: normu

    !update control with alpha 
    controlmodebak = controlmode
    controlmode = 0
    call Control(lambda,x,p,u)
    controlmode = controlmodebak
    normu = sqrt(sum(u*u))
    if (normu < 1d-10) then
       u = 0d0 
    else
       u = u / normu * newalpha
    end if

    !write(0,*) 'Updatealt',newalpha,u

  end Subroutine UpdateAltcontrol



  !!**************************************
  !!* Compute state and costate dynamics *
  !!**************************************
  Subroutine Dynamics(dimphi,lambda,x,p,u,phi)
    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) :: phi

    !Local
    integer :: i
    real(kind=8) :: normu, h, normv, ham, cout
    real(kind=8), dimension(m) :: r, v, pr, pv, D, g
    real(kind=8), dimension(m,m) :: dgdr, dDdr, dDdv

    !out init
    phi = 0d0

    !position/speed splitting for state and costate
    r(1:m) = x(1:m)
    v(1:m) = x(m+1:2*m)
    pr(1:m) = p(1:m)
    pv(1:m) = p(m+1:2*m)  

    !control norm
    normu = sqrt(sum(u*u))
    !distance to earth center
    h = sqrt(sum(r*r))
    !speed norm
    normv = sqrt(sum(v*v))

    !gravity field
    g(1:m) =  g0 / h**3 * r(1:m)

    dgdr = 0d0
    if (h /= 0d0) then
       do i=1,m
          dgdr(i,1:m) = - 3d0 / h**2 * r(i) * r(1:m)    
          dgdr(i,i) = dgdr(i,i) + 1d0
       end do
       dgdr = dgdr * g0 / h**3
    end if

    !drag
    D(1:m) = kd * normv * exp(-500d0 * (h - 1d0)) * v(1:m)

    do i=1,m
       dDdr(i,1:m) = - 500d0 / h * D(i) *  r(1:m) 
    end do

    dDdv = 0d0
    if (normv /= 0d0) then  
       do i=1,m
          dDdv(i,1:m) = v(i) * v(1:m) / normv
          dDdv(i,i) = dDdv(i,i) + normv
       end do
       dDdv = dDdv * kd * exp(-500d0 * (h - 1d0))
    end if

    !state dynamics
    phi(1:m) = v(1:m)
    phi(m+1:2*m) = (u(1:m)*Tmax - D(1:m)) / x(2*m+1) - g(1:m)
    phi(2*m+1) = - beta*Tmax*normu
 
    if (freetf == 1) phi(2*m+2) = 0d0 !tff

    !costate dynamics
    phi(ns+1:ns+m) = matmul(pv , dDdr/x(2*m+1) + dgdr)
    phi(ns+m+1:ns+2*m) = - pr + matmul(pv , dDdv) / x(2*m+1)
    phi(ns+2*m+1) = sum(pv*(u * Tmax - D)) / x(2*m+1)**2

    !hamiltonian
    normu = sqrt(sum(u*u))
    select case (homotop)
    case (1)
       cout = normu + (1d0-lambda) * normu**2
    case (2)
       cout = normu + normu**2
    case (3)
       cout = (1d0-lambda) * normu**2
    case (4)
       cout = normu**2
    case default
       write(0,*) 'ERROR: Dynamics >>> Unknown homotop...',homotop
       stop
    end select
    
    hamilton = cout + sum(p(1:2*m+1)*phi(1:2*m+1))

    if (freetf == 1) then 
       tff = x(2*m+2)
       phi(ns+2*m+2) = - hamilton / tff
    end if

    !objective dynamic (NB: this is > mass consumption except for homotopy 1 at lambda=1...
    if (dimphi > ns+nc) then
       phi(ns+nc+1) = cout * beta * Tmax
       !write(0,*) 'cout derivee',phi(ns+nc+1)
    end if

    !multiply all dynamics by time interval length (NB. \dot{pti} must then be set to -H / tff !)
    phi(1:dimphi) = tff * phi(1:dimphi)


  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)

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


    !check tf is positive
    if (freetf == 1 .and. y(ns) < 0d0) then
       if (shootdebug > 0) write(0,*) 'Negative final time...',  y(ns) 
       s(ncf) = y(ns) / yscal(ns)
       dsdy(ncf,1:xpdim) = 0d0
       dsdy(ncf,ns) = 1d0 / yscal(ns)
    end if

    !check mass is positive at tf
    if (y(2*m+1) < 0d0) then
       if (shootdebug > 0) write(0,*) 'Negative final mass...', y(2*m+1)
       s(2*m+1) = y(2*m+1) / yscal(2*m+1)
       dsdy(2*m+1,1:xpdim) = 0d0
       dsdy(2*m+1,2*m+1) = 1d0 / yscal(2*m+1)
    end if


  end Subroutine FinalConditions


  !!*****************
  !!* PreProcessing *
  !!*****************
  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)

  end Subroutine PreProcess

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

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

  end Subroutine PostProcess


end Module SpecFuns