fishingfuns.f90

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

!!*********************************
!!* Shoot package v1.0            *
!!* Functions for Fishing problem *
!!* Author: Pierre Martinon       *
!!* INRIA FUTURS, team COMMANDS   *
!!* CMAP, ECOLE POLYTECHNIQUE     * 
!!* 11/2007                       *
!!*********************************

! Homotopies
! 1: Energy opposite perturbation (u <- u - (1-lambda)u^2 in criterion)


Module SpecFuns
  use shootdefs
  implicit none

  real(kind=8), save :: E, c, r, kk, Umax

contains


  Subroutine InitPar(mode, z, lambda)
    implicit none

    integer, intent(in) :: mode
    real(kind=8), intent(in) :: z(n), lambda


    !mode = 0: first call
    !mode = 1: homotopy call

    if (mode == 0) then

    E = par(1)
    c = par(2)
    r = par(3)
    kk = par(4)
    Umax = par(5)
    lower(1) = par(6)
    upper(1) = par(7)

    end if

  end subroutine InitPar





  !****************************
  ! Compute Optimal control u *
  !****************************
  Subroutine Control(lambda,x,p,u)
    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(inout), dimension(m) :: u


    !Local declarations
    real(kind=8) :: a, b, psi

    !controlmode
    !0: compute usual control via Hamiltonian minimization (Control)
    !1: compute bang-bang control (Control)
    !2: compute exact singular control (Control)

    !out init
    u = 0d0

    if (abs(x(1)) < 1d-12) then
       write(outputfid,*) 'ERROR: Control >>> Numerical Hazard...',x(1)
       stop
    end if

    !compute switching function value
    call Switch(lambda,x,p,psi)

    select case (controlmode)
    
    case (0)
       !usual control
       if (lambda < 1d0) then

          a = (lambda - 1d0) * (c / x(1) - E)
          b = psi

          if (- b / 2d0 / a < 0d0) then
             u(1) = 0d0
          elseif (- b / 2d0 / a  > 1d0) then
             u(1) = 1d0
          else
             u(1) = - b / 2d0 / a
          end if

       else
          
          if (psi > 0d0) then
             u(1) = 0d0
          else
             u(1) = 1d0
          end if

       end if
        
     
    case (2) 
   
       !singular control          
       if (lambda < 1d0) then
          u(1) = sqrt((r*x(1)*(1d0-x(1)/kk) & 
               & - p(1)*r/c*x(1)*x(1)*(1d0-2d0*x(1)/kk))/(1d0-lambda)/Umax) 
       else
          u(1) = kk*r/(2d0*(c/x(1)-p(1))*Umax) &
               & * (c/x(1) - c/kk - p(1) + 2d0*p(1)*x(1)/kk - 2d0*p(1)*x(1)*x(1)/kk/kk) 
       end if
       
       if (u(1) < lower(1)) then
          u(1) = lower(1)
          if (shootdebug > 0) write(0,*) 'Singular control below lower bound !', u, lower
       elseif (u(1) > upper(1)) then
          u(1) = upper(1)
          if (shootdebug > 0) write(0,*) 'Singular control above upper bound !', u, upper
       end if
       
       
    case default
       write(0,*) 'ERROR: Control >>> No control provided for controlmode...',controlmode
       stop

    end select


  end subroutine Control



  Subroutine Switch(lambda,x,p,psi)
    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

    psi = c / x(1) - E - p(1)  

  end Subroutine Switch


  Subroutine Switchdot(lambda,x,p,u,psidot)
    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

    psidot = p(1)*r*(1d0-2d0*x(1)/kk) - c*r/x(1)*(1d0-x(1)/kk)

  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  
    
    !out init
    gradpsi = 0d0

    !psi(1) = c / x(1) - E - p(1)   
    gradpsi(1) = - c / (y(1)/yscal(1))**2
    gradpsi(2) = - 1d0

    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  

    !initialize control and alpha
    if (arcentry == 1) then
       alpha = (lower(1) + upper(1)) / 2d0
       arcentry = 0
    else
       alpha = prevalpha
    end if

  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  

    !update control with alpha
    u(1) = newalpha

  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


    !out init
    phi = 0d0

    select case (homotop)

    case (1)

       phi(1) = r*x(1)*(1d0-x(1)/kk) - u(1)*Umax
       phi(2) = c/(x(1)**2) * (u(1)-(1d0-lambda)*u(1)**2)*Umax - p(1)*r*(1d0-2d0*x(1)/kk)

    case default
       write(outputfid,*) 'ERROR : Dynamics >>> Unknown homotop...',homotop
       stop
    end select

    !objective dynamic
    if (dimphi > ns+nc) phi(ns+nc+1) = (E - c/x(1)) * u(1)*Umax  
    if (dimphi > ns+nc+1) phi(ns+nc+2) = fpiter / (tf - t0)  

  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

  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)

  end Subroutine PreProcess


  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