regulatorfuns.f90

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

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

! Homotopies
! 1: Energy perturbation


Module SpecFuns
  use shootdefs
  implicit none

  real(kind=8), save :: pertcoeff

contains


  Subroutine InitPar(mode,z,lambda)
    implicit none

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

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

    if (mode == 0) then
       
       pertcoeff = par(1)
       lower(1) = par(2)
       upper(1) = par(3)

    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(out), dimension(m) :: u


    !controlmode
    !0: compute usual control via Hamiltonian minimization (Control)
    !1: compute bang-bang control (Control)
    !2: compute exact singular control (Control)
    
    !Local
    real(kind=8) :: sgnp2

    !out init
    u = 0d0

    sgnp2 = sign(1d0,p(2))



    select case (controlmode)

    case (0)
       !usual control

       if (lambda < 1d0) then 

          if (abs(p(2)) < (1d0 - lambda)*pertcoeff) then
             u(1) = - p(2) / ((1d0 - lambda)*pertcoeff)
          else
             u(1) = - sgnp2
          end if

       else
          u(1) = - sgnp2         
       end if


    case (2) 

       !singular control     
       if (lambda < 1d0) then
          if (abs(p(2)) < (1d0 - lambda)*pertcoeff) then
             u(1) = - p(2) / ((1d0 - lambda)*pertcoeff)
          else
             u(1) = - sgnp2
          end if
       else
          u(1) = x(1)
       end if

       if (u(1) < lower(1)) then
          u(1) = lower(1)
          if (shootdebug > 0) write(0,*) 'Singular control below lower bound !'
       elseif (u(1) > upper(1)) then
          u(1) = upper(1)
          if (shootdebug > 0) write(0,*) 'Singular control above upper bound !'
       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 = p(2)

  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) - x(2)

  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) = p(2)
    gradpsi = 0d0
    gradpsi(4) = 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) = x(2)
       phi(2) = u(1)
       phi(3) = - x(1)
       phi(4) = - p(1) - x(2)

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


    !objective dynamic
    if (dimphi > ns+nc) phi(ns+nc+1) = 0.5d0 * (x(1)**2 + x(2)**2)
    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