integrators.f90

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

!!****************************************
!!* Simplicial package v1.4              *
!!* Module for IVP integration           *
!!* Author: Pierre Martinon              *
!!* ENSEEIHT-IRIT, UMR CNRS 5505         *
!!* CMAP-INRIA FUTURS, UMR CNRS 7641     *
!!* 12/2006                              *
!!****************************************


Module Integration
  use shootdefs
  use SpecFuns
  use RHSmod
  implicit none

  integer, save :: indout, rhscalls, nbsteps, accsteps, rejsteps, field1, field2, check, dimhyb

  real(kind=8), dimension(:), allocatable, save :: timeout, hamiltout
  real(kind=8), dimension(:,:), allocatable, save :: yout, uout, psiout


contains


  !!***********************************************
  !!* Output subroutine for trajectory generation *
  !!***********************************************
  !! NOTE: SOLOUT a ete modifiee dans DOP853/DOPRI5/RADAU5 pour coincider avec celle de ODEX
  Subroutine Solout(NITER,told,time,y,dim,con,lcon,icomp,licomp,lambda,order,irtrn)
    implicit none

    integer, intent(in) :: NITER,dim,order,lcon, licomp
    integer, intent(inout) :: irtrn
    integer, dimension(licomp), intent(in) :: icomp
    real(kind=8), intent(in) :: told, lambda
    real(kind=8), intent(inout) :: time
    real(kind=8), dimension(dim), intent(inout) :: y
    real(kind=8), dimension(lcon), intent(in) :: con

    !Local declarations
    integer :: iter
    real(kind=8) :: normcorr
    real(kind=8), dimension(ns) :: x
    real(kind=8), dimension(nc) :: p
    real(kind=8), dimension(m) :: u
    real(kind=8), dimension(dimpsi) :: psi
    real(kind=8), dimension(xpdim) :: fdummy, ynew  

    !write(0,*) 'Solout'

    !save current stepsize (NB: stepsize will be 0 for first call at initial point)
    stepsize = time - told

    !!Control and Switch evaluation
    call PreProcess(time,y,x,p)

    call ControlSwitch(lambda,x,p,u,psi)

    !!Discontinuity detection and handling
    if (controlmode == 1) then 
       call DiscDetect(dim,told,time,y,lambda,irtrn,con,lcon,icomp,licomp)
    end if           

    !!Trajectory output for solution file generation 
    !(y might have been changed in DiscDetect)
    if (outmode == 1 .and. integmode > 5) then

       call PreProcess(time,y,x,p)
       !disable alt control update for next step
       update = 0
       call ControlSwitch(lambda,x,p,u,psi)
       update = 1

       timeout(indout) = time
       yout(indout,1:ns) = x
       yout(indout,ns+1:ns+nc) = p
       !objective ?? on enleve pour l'instant
       !if (dim > ns+nc) yout(indout,ns+nc+1:dim) = y(ns+nc+1:dim)
       uout(indout,1:m) = u
       psiout(indout,1:dimpsi) = psi
       if (hamiltonianoutput == 1) then
          call Dynamics(xpdim,lambda,x,p,u,fdummy)
          hamiltout(indout) = hamilton
       end if

       indout = indout + 1
       !write(0,*) 'indout',indout,'time',time
    end if

  end subroutine Solout


  !!********************************
  !!* Simplified output subroutine *
  !!********************************
  Subroutine Solout2(time,y,dim,lambda)
    implicit none
    integer, intent(in) :: dim
    real(kind=8), intent(in) :: lambda
    real(kind=8), intent(inout) :: time
    real(kind=8), dimension(dim), intent(inout) :: y


    !Local declarations
    real(kind=8), dimension(ns) :: x
    real(kind=8), dimension(nc) :: p
    real(kind=8), dimension(m) :: u
    real(kind=8), dimension(dimpsi) :: psi
    real(kind=8), dimension(xpdim) :: fdummy

    if (integmode > 5) then
       write(0,*) 'ERROR: Solout2 >>> Should not be called for variable step integrator...',integmode
       stop
    end if

    !Trajectory output for solution file generation 
    call PreProcess(time,y,x,p)
    !disable alt control update for next step
    update = 0
    call ControlSwitch(lambda,x,p,u,psi)
    update = 1

    timeout(indout) = time
    yout(indout,1:ns) = x
    yout(indout,ns+1:ns+nc) = p
    !objective ?? on enleve pour l'instant
    !if (dim > ns+nc) yout(indout,ns+nc+1:dim) = y(ns+nc+1:dim)
    uout(indout,1:m) = u
    psiout(indout,1:dimpsi) = psi
    if (hamiltonianoutput == 1) then
       call Dynamics(xpdim,lambda,x,p,u,fdummy)
       hamiltout(indout) = hamilton
    end if

    indout = indout + 1


  end Subroutine Solout2



  !!************************************************************************
  !!* Discontinuity detection (bissection, originally from Prof. E.Hairer) *
  !!************************************************************************
  Subroutine DiscDetect(dim,told,time,y,lambda,irtrn,con,lcon,icomp,licomp)
    implicit none
    integer, intent(in) :: dim, lcon, licomp
    integer, intent(out) :: irtrn
    integer, dimension(licomp), intent(in) :: icomp
    real(kind=8), intent(in) :: told, lambda
    real(kind=8), intent(inout) :: time
    real(kind=8), dimension(dim), intent(inout) :: y
    real(kind=8), dimension(lcon), intent(in) :: con

    !Local declarations
    integer ifixpt, i, outmode2, detect
    real(kind=8) :: h, epsilon, h1, val
    real(kind=8) :: tceh, tleft, tright, f(xpdim)
    real(kind=8), dimension(dim) :: yeh, yold

    !out init
    irtrn = 0

    
    check = 0
    detect = 0

    epsilon = 1d-12
    !set values for HamiltFun
    !lambdahyb = lambda
    timeswitch = time

    yeh = 0d0

    if (integmode < 5) then
       yold = con(2:dim+1)
       h = con(1)
       if (abs (time-told-h) > epsilon) then
          write(0,*) 'DiscDetect: Step error ?',told,time,h
          stop
       end if
    else
       h = time - told
    end if

    !!Switching localization between told and time
    call SwitchCond(xpdim,time,y,lambda,val)

    if (switchflag * val < 0.0d0) then
       tleft = told
       tright = time
       tceh = (time + told) / 2.0d0
       if (outmode == 1) swdetect(1) = swdetect(1) + 1
       detect = 1
    else
       if (time > told) then
          do i = 1 , ntry - 1
             tceh = time - i * (time-told) / ntry

             call DenseOutput(xpdim,tceh,told,h,con,lcon,icomp,licomp,yeh(1:xpdim))
             call SwitchCond(xpdim,tceh,yeh,lambda,val)     

             if (switchflag * val < 0.0d0) then
                tleft = told
                tright = tceh
                tceh = (tleft + tright) / 2.0d0
                if (outmode == 1) swdetect(i+1) = swdetect(i+1) + 1
                detect = 1
             end if
          end do
       end if

    end if


    !!if a switching occured between told and time: localization via bissection (50 steps)
    if (detect == 1) then
       ifixpt = 0
       globaldicho = globaldicho + 1

       if (check == 1) then
          hamiltonianoutput = 1
          call RHS(xpdim,told,yold,f,lambda)
          write(0,*) 'Hamiltonian at step before switch',hamilton
       end if

       do while ((tright - tleft) > epsilon .and. ifixpt < maxdichoiter) 
          ifixpt = ifixpt + 1
          globaldichoiter = globaldichoiter + 1

          call DenseOutput(xpdim,tceh,told,h,con,lcon,icomp,licomp,yeh(1:xpdim))
          call SwitchCond(xpdim,tceh,yeh,lambda,val)


          if (switchflag * val < 0.0d0) then
             tright = tceh
             tceh = (tceh + tleft) / 2.0d0
          else
             tleft = tceh
             tceh = (tright + tceh) / 2.0d0
          end if
       end do

       !Final (full dimension) dense output at commutation time
       call DenseOutput(dim,tceh,told,h,con,lcon,icomp,licomp,yeh)

       !!Handle discontinuity
       call DiscHandle(dim,time,tceh,y,yeh,lambda,irtrn)


       if (check == 1) then
          hamiltonianoutput = 1
          call RHS(xpdim,time,y,f,lambda)
          write(0,*) 'Hamiltonian at switch',hamilton
       end if

    end if

  end Subroutine DiscDetect






  !!********************
  !!* Dummy subroutine *
  !!********************
  Subroutine Dummyfun()
  end subroutine Dummyfun


  !!*****************************
  !!* Explicit Euler integrator *
  !!*****************************
  Subroutine Euler(lambda,neqn,f,y,tin,tout,nbsteps)
    implicit none

    integer, intent(in) :: neqn, nbsteps
    real(kind=8), intent(in) :: lambda
    real(kind=8), intent(inout) :: tin, tout
    real(kind=8), intent(inout), dimension(neqn) :: y

    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 declarations
    integer :: i
    real(kind=8) :: h, t
    real(kind=8), dimension(neqn) :: phi

    h = ( tout - tin ) / nbsteps
    stepsize = h
    t = tin

    do i = 1, nbsteps

       call f(neqn, t, y , phi, lambda)
       y = y + h * phi
       t = t + h

       if (outmode == 1) call Solout2(t,y,neqn,lambda)

    end do

    tin = t

  end subroutine Euler




  !!*******************************************************
  !!* Explicit Runge Kutta 2nd order integrator (trapeze) *
  !!*******************************************************
  Subroutine Rk2(lambda,neqn,f,y,tin,tout,nbsteps)
    implicit none

    integer, intent(in) :: neqn, nbsteps
    real(kind=8), intent(in) :: lambda
    real(kind=8), intent(inout) :: tin, tout
    real(kind=8), intent(inout), dimension(neqn) :: y

    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 declarations
    integer :: i
    real(kind=8) :: h, t
    real(kind=8), dimension(neqn):: k1, k2, y1


    h = ( tout - tin ) / nbsteps
    stepsize = h
    t = tin

    do i = 1, nbsteps

       call f(neqn, t, y ,k1, lambda)
       k1 = h * k1
       y1 = y + k1
       t = t + h

       call f(neqn, t, y1, k2, lambda)
       k2 = h * k2

       y = y + (k1 + k2) / 2d0

       if (outmode == 1) call Solout2(t,y,neqn,lambda)

    end do

    tin =  t


  end subroutine Rk2



  !!*********************************************
  !!* Explicit Runge Kutta 3rd order integrator *
  !!*********************************************
  Subroutine Rk3(lambda,neqn,f,y,tin,tout,nbsteps)
    implicit none

    integer, intent(in) :: neqn, nbsteps
    real(kind=8), intent(in) :: lambda
    real(kind=8), intent(inout) :: tin, tout
    real(kind=8), intent(inout), dimension(neqn) :: y

    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 declarations
    integer :: i, crossed, irtrn, order
    real(kind=8) :: h, t, t1, t2, told
    real(kind=8), dimension(neqn):: k1, k2, k3, y1, y2, yold

    !dummy variables for Solout
    integer :: icomp(1), licomp, lcon
    real(kind=8) :: con(neqn+1)
    licomp = 1
    icomp(1) = 1
    lcon = neqn+1

    order = 3

    h = ( tout - tin ) / nbsteps
    stepsize = h
    t = tin
    crossed = 0

    i = 0
    !Note: total number of steps may differ from predicted value due to roundoff error
    do while (t < tout)
       i = i + 1
       !adjust final step
       if (t + h > tout) h = tout - t

       told = t
       yold = y
       t1 = t + h / 3d0
       t2 = t + 2d0 * h / 3d0

       call f(neqn, t, y ,k1, lambda)
       y1 = y + h * k1 / 3d0
       call f(neqn, t1, y1, k2, lambda)
       y2 = y + h * 2d0 * k2 / 3d0
       call f(neqn, t2, y2, k3, lambda)

       y = y + h / 4d0 * (k1 + 3d0 * k3)
       t = t + h

       !Break integration for Switching detection
       if (controlmode == 1) then

          !Prepare Hermite interpolation
          call PrepareHermite(neqn,t,yold,y,k1,f,lambda)

          irtrn = 0
          con(1) = h
          con(2:neqn+1) = yold

          call Solout(i,told,t,y,neqn,con,lcon,ICOMP,LICOMP,lambda,order,irtrn)

          !Adapt/restore stepsize after crossing
          if (irtrn == 2) then
             crossed = 1
             h = told + h - t
          else if (crossed == 1) then
             crossed = 0
             h = ( tout - tin ) / nbsteps 
          end if
         

       end if

       !Manual output
       if (outmode == 1) call Solout2(t,y,neqn,lambda)

    end do

    tin =  t

  end subroutine Rk3



  !!*********************************************
  !!* Explicit Runge Kutta 4th order integrator *
  !!*********************************************
  Subroutine Rk4(lambda,neqn,f,y,tin,tout,nbsteps)
    implicit none

    integer, intent(in) :: neqn, nbsteps
    real(kind=8), intent(in) :: lambda
    real(kind=8), intent(inout) :: tin, tout
    real(kind=8), intent(inout), dimension(neqn) :: y

    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 declarations
    integer :: i, crossed, irtrn, order
    real(kind=8) :: h, t, t1, told
    real(kind=8), dimension(neqn):: k1, k2, k3, k4, y1, y2, y3, yold

    !dummy variables for Solout
    integer :: icomp(1), licomp, lcon
    real(kind=8) :: con(neqn+1)
    licomp = 1