rhs.f90

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

    !NOTE: for both switching conditions
    !switchflag = -1: first vector field, |u| = 1
    !switchflag = 1: second vector field, u = 0 
    !switchflag is initialized in control according to switching function psi 

    !mode 0: typical switching function, switchflag = sign(psi)
    !mode 1: hamiltonian gap, switchflag = sign(H1 - H2), see below

    !out init
    val = 0d0


    select case (switchmode)

    case (0)  ! usual switching function in Control

       call PreProcess(time,y,x,p)
       call Switch(lambda,x,p,val)

    case (1)  ! use Hamiltonian gap

       !backup
       aux = hamiltonianoutput
       aux2 = switchflag

       !Check Hamiltonian with 1st vector field
       hamiltonianoutput = 1
       switchflag = -1
       call RHS(dim,time,y,phi,lambda)
       h1 = hamilton

       !Check Hamiltonian with 2nd vector field
       switchflag = 1
       call RHS(dim,time,y,phi,lambda)
       h2 = hamilton

       !Hamiltonian gap
       !NB: this difference has the same sign as the switching function psi
       !psi > 0 corresponds to switchflag = 1, ie u=0, thus h2 < h1
       !so we set the gap as h1 - h2
       val = h1 - h2

       !restore
       hamiltonianoutput = aux
       switchflag = aux2

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

    end select



  end Subroutine SwitchCond



  !!**************************************************************************************
  !!* Discontinuity handling for integration (originally from Prof. E.Hairer)            *
  !!* Also used for variational system integration to compute shooting function jacobian *
  !!**************************************************************************************
  Subroutine DiscHandle(dim,time,tceh,y,yeh,lambda,irtrn)
    implicit none
    integer, intent(in) :: dim
    integer, intent(out) :: irtrn
    real(kind=8), intent(inout) :: time
    real(kind=8), intent(in) :: tceh, lambda
    real(kind=8), dimension(dim), intent(inout) :: y
    real(kind=8), dimension(dim), intent(in) :: yeh


    !Local declarations
    integer :: i, j, offset
    real(kind=8) :: switch, dgdt
    real(kind=8), dimension(xpdim) :: dgdy, phi1, phi2, deltaphi
    real(kind=8), dimension(nz) :: tauprime
    real(kind=8), dimension(xpdim,nz) :: dydz, jump

    !out init
    irtrn = 0

    !signal alteration of numerical solution to the integrator
    irtrn = 2
    !reset integration back to localized switching time
    time = tceh
    y = yeh

    if (vareqinteg == 0) then
       !IVP case

!!$       !Check Hamiltonian with pre-switch vector field
!!$       aux = hamiltonianoutput
!!$       hamiltonianoutput = 1
!!$       call RHS(dim,time,y,phi,lambda)
!!$       h1 = hamilton

       !control switch
       switchflag = - switchflag

!!$       !Check Hamiltonian with pre-switch vector field
!!$       call RHS(dim,time,y,phi,lambda)
!!$       h2 = hamilton
!!$       hamiltonianoutput = aux
!!$


    else
       !VAR case

       !dim n vareq (dydz)
       !IND case, compute jump for PSI=dy/dz at commutation

       !retrieve dy/dz in y
       offset = xpdim
       do j = 1, nz
          dydz(1:xpdim,j) = y(offset + (j-1)*xpdim + 1 : offset + j*xpdim )
       end do

       !compute f1(y)=phi1
       call RHS(xpdim,time,y(1:xpdim),phi1,lambda)

       !compute switching functions derivatives dg/dy and dg/dt
       call Switchprime(xpdim,time,lambda,y(1:xpdim),dgdy,dgdt,switch)

       !tauprime (dtau / dz en fait d'apres la formule, ce qui est pareil en autonome)
       tauprime = - matmul(dgdy,dydz) / (dot_product(dgdy,phi1) + dgdt)

       !control switch - compute f2(y)
       switchflag= - switchflag
       call RHS(xpdim,time,y(1:xpdim),phi2,lambda)

       !jump = (phi1-phi2) * dtau/dz
       deltaphi = phi1 - phi2

       do i=1,xpdim
          do j=1,nz
             jump(i,j) = deltaphi(i) * tauprime(j)
          end do
       end do

       !update dydz in y
       do j = 1, nz
          y(offset+(j-1)*xpdim+1 : offset+j*xpdim) = dydz(:,j) + jump(:,j)
       end do

    end if  !vareqinteg == 0

  end Subroutine DiscHandle


  !!*********************************************
  !!* Switch derivative for IND jump evaluation *
  !!*********************************************
  Subroutine Switchprime(dim,time,lambda,y,dgdy,dgdt,g)
    implicit none
    integer, intent(in) :: dim 
    real(kind=8), intent(in) :: time, lambda
    real(kind=8), dimension(dim), intent(in) :: y
    real(kind=8), dimension(dim), intent(out) :: dgdy
    real(kind=8), intent(out) :: dgdt, g

    !Local declarations
    integer :: j
    real(kind=8) :: time2, h, psi1, psi2
    real(kind=8), dimension(ns) :: x(ns), p(nc), y2(dim)

    !out init
    dgdy = 0d0
    dgdt = 0d0
    g = 0d0

    select case (vareqmode)
    case (1)

       !switch value
       call PreProcess(time,y,x,p)
       call Switch(lambda,x,p,psi1)
       g = psi1

       !derivatives with respect to y
       do j=1,dim
          y2 = y
          h = sqrt(macheps*(1d0+abs(y2(j))))
          y2(j) = y2(j) + h
          call PreProcess(time,y2,x,p)
          call Switch(lambda,x,p,psi2) 
    
          dgdy(j) = (psi2 - psi1) / h
       end do

       !derivatives with respect to time (non autonomous case)
       h = sqrt(macheps*(1d0+abs(time)))
       time2 = time + h
       call PreProcess(time2,y,x,p)
       call Switch(lambda,x,p,psi2)

       dgdt = (psi2 - psi1) / h

    case default
       write(0,*) 'ERROR: Switchprime >>> Unknown vareqmode...',vareqmode
       stop

    end select


  end Subroutine Switchprime



  !!****************************************
  !!* Dense Output for Switching detection *
  !!****************************************
  Subroutine DenseOutput(dim,tceh,told,h,con,lcon,icomp,licomp,yeh)
    implicit none
    integer, intent(in) :: dim, lcon, licomp
    integer, dimension(licomp), intent(in) :: icomp
    real(kind=8), intent(in) :: tceh, told, h
    real(kind=8), dimension(lcon), intent(in) :: con
    real(kind=8), dimension(dim), intent(out) :: yeh 

    real(kind=8) :: contd5, contr5

    !Local
    integer :: ieh

    !out init
    yeh = 0d0

    do ieh = 1 , dim
       select case (integmode)
!!$          case (-4)
!!$             yeh(ieh) = InterpGauss2(ieh,tceh,told)
!!$          case (4)
!!$             yeh(ieh) = InterpRK4(ieh,tceh,told,h)
       case (-2,1)
          yeh(ieh) = InterpMidpoint(ieh,tceh,told)
       case (-4,3,4)
          yeh(ieh) = InterpHermite(ieh,tceh,told,h)
       case (6)
          write(0,*) 'Check consistency with solout in dop853...'
          stop
          !yeh(ieh) = contd8(ieh,tceh,con,icomp,nd)
       case (8)
          !NB: licomp = nd
          yeh(ieh) = contd5(ieh,tceh,con,icomp,licomp)
       case (9)
          yeh(ieh) = contr5(ieh,tceh,con,lcon)
       case default
          write(outputfid,*) 'ERROR: Solout >>> No dense output for integmode...',integmode
          stop
       end select
    end do

  end subroutine DenseOutput




  !!**************************************************
  !!* Prepare hermite interpolation for dense output *
  !!**************************************************
  Subroutine PrepareHermite(neqn,t,yold,y,k1,f,lambda)
    implicit none
    integer, intent(in) :: neqn
    real(kind=8), intent(in) :: t, lambda
    real(kind=8), dimension(neqn), intent(in) :: yold, y, k1

    interface
       Subroutine f(dim,time,y,phi,lambda)
         implicit none
         integer, intent(in) :: dim
         real(kind=8), intent(in) :: time, lambda
         real(kind=8), intent(in), dimension(dim) :: y
         real(kind=8), intent(out), dimension(dim) :: phi
       end Subroutine f
    end interface

    !Local
    integer :: j   
    real(kind=8) :: h, told
    real(kind=8), dimension(neqn) :: yalpha

    yy0(1:neqn) = yold(1:neqn)
    yy1(1:neqn) = y(1:neqn)
    ff0(1:neqn) = k1(1:neqn)
    call f(neqn,t,y,ff1,lambda)

    if (hermiteorder == 4) then
       h = stepsize
       told = t - h
       do j=1,neqn
          yalpha(j) = InterpHermite(j,told+alphaboot*h,told,h)
       end do
       call f(neqn,told+alphaboot*h,yalpha,ffalpha,lambda)
       
       ee = yy0
       dd = h * ff0
       cc = (-(4d0*alphaboot**3-3d0*alphaboot**2)*(4d0*(yy1-yy0)-h*(ff1+3d0*ff0)) + 4d0*alphaboot**3*(yy1-yy0-h*ff0) & 
            & - h*(ffalpha-ff0)) /(-4d0*alphaboot**3 + 6d0*alphaboot**2 - 2d0*alphaboot)
       bb = 4d0*(yy1-yy0)-h*(ff1+3d0*ff0)-2*cc
       aa = yy1 - yy0 - h*ff0 - bb - cc
    end if

  end Subroutine PrepareHermite


  !!****************************************
  !!* Interpolation polynom for RK4 method *
  !!****************************************

  !Prepare polynom for dense output
!!$          kk1 = k1 
!!$          kk2 = k2 
!!$          kk3 = k3 
!!$          kk4 = k4 
!!$          yyold = yold


  function InterpRK4(i,t,told,h) result(y)
    implicit none

    integer, intent(in) :: i
    real(kind=8), intent(in) :: t, told, h
    real(kind=8) :: y

    !Local
    real(kind=8) :: dt, theta, aux, b1, b2, b3, b4

    dt = t - told
    if (dt < 0d0) then
       write(0,*) 'InterpRK4 dt neg',t,told
       stop
    end if

    theta = dt / h
    if (dt < 0d0) then
       write(0,*) 'InterpRK4 theta > 1',t,told,h,theta
       stop
    end if
    aux = 2d0 * theta**2 * theta / 3d0
    !toto modif: on tronque le polynome au degre 2: aux = 0d0
    b1 = theta - 1.5d0 * theta**2 + aux
    b2 = theta**2 - aux 
    b3 = b2
    b4 = - 0.5d0 * theta**2 + aux

    y = yyold(i) + h * (b1*kk1(i) + b2*kk2(i) + b3*kk3(i) + b4*kk4(i))
    !write(0,*) 'Interp at',t,'from',told,'is',y

  end function InterpRK4



  !!******************************************
  !!* Hermite interpolation for dense output *
  !!******************************************
  function InterpHermite(i,t,told,h) result(y)
    implicit none

    integer, intent(in) :: i
    real(kind=8), intent(in) :: t, told, h
    real(kind=8) :: y

    !Local
    real(kind=8) :: theta

    theta = (t - told) / h
    if (theta < 0d0 .or. theta > 1d0) then
       write(0,*) 'ERROR: Hermite >>> theta should be in [0,1]...',t,told,h,theta

       stop
    end if

    if (hermiteorder == 3) then
       
       y = (1d0-theta)*yy0(i) + theta*yy1(i) + theta*(theta-1)*((1d0-2d0*theta)*(yy1(i)-yy0(i)) &
            &       + (theta-1d0)*h*ff0(i) + theta*h*ff1(i))

    else
       !Hermite interpolation extended to 4th order by bootstrapping
       y = ee(i) + theta * (dd(i) + theta * (cc(i) + theta * (bb(i) + theta*aa(i))))

    end if

  end function InterpHermite


  !!***************************************************
  !!* Dense output for explicit and implicit midpoint *
  !!***************************************************
  function InterpMidpoint(i,t,told) result(y)
    implicit none

    integer, intent(in) :: i
    real(kind=8), intent(in) :: t, told
    real(kind=8) :: y

    !Local
    real(kind=8) :: dt

    dt = t - told
    if (dt < 0d0) then
       write(0,*) 'InterpMidpoint dt neg',t,told
       stop
    end if

    y = midy0(i) + dt * midf0(i)

  end function InterpMidpoint




  !!***********************************************
  !!* Gauss2 Colocation polynom for interpolation *
  !!***********************************************

!!$          !Faire une grosse subroutine pour la preparation, avec un flag pour le choix 
!!$          !Prepare collocation polynom for interpolation
!!$          ag2 = (k1 - k2) / (2d0 * h * (c1 - c2))
!!$          bg2 = (c2*k1 - c1*k2) / (c2 - c1)
!!$          cg2 = yold


  function InterpGauss2(i,t,told) result(y)
    implicit none

    integer, intent(in) :: i
    real(kind=8), intent(in) :: t, told
    real(kind=8) :: y

    !Local
    real(kind=8) :: dt

    dt = t - told
    if (dt < 0d0) then
       write(0,*) 'Interp dt neg',t,told
       stop
    end if
    y = ag2(i) * dt**2 + bg2(i) * dt + cg2(i) 
    !write(0,*) 'Interp at',t,'from',told,'is',y

  end function InterpGauss2


end Module RHSmod