shoot.f90

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

!!**********************************
!!* Shoot v1.0 package             *  
!!* Module for Shooting functions  *
!!* Author: Pierre Martinon        *
!!* INRIA FUTURS, team COMMANDS    *
!!* CMAP, ECOLE POLYTECHNIQUE      *  
!!* 12/2007                        *
!!**********************************


!!Problem class
!!1: single shoot (merged with structured and singular shoot)



Module Shooting
  use CommonFuns
  use Integration
  use SpecFuns
  implicit none

contains

  !!*********************
  !!* Shooting Function *
  !!*********************
  Subroutine ShootFun(n,z,s,iflag)
    implicit none

    integer, intent(in) :: n !!problem dimension
    integer, intent(out) :: iflag
    real(kind=8), dimension(n), intent(in) :: z !!shooting function unknown
    real(kind=8), dimension(n), intent(out) :: s !!shooting function value

    !Local declarations
    integer :: mode, dim
    real(kind=8) :: zd(n), y(xpdim+dimcrit)
    real(kind=8) :: dsdz(n,n)

    !out init
    iflag = 0
    s = 0d0

    !Compute shooting function only

    !IVP initialization
    !call init
    zd = z
    !dim = xpdim
    !mode = 0
    !on rajoute l'objectif sinon a pas variable ca merdouille parfois quand on veut calculer obj
    !pas les memes pas, malgre les tols vectorielles, et patatras... -_-
    !d'ailleurs c'est bizarre que ca marche pour VAR, a checker
    dim = xpdim + dimcrit
    mode = 1
    call InitIVP(zd,dim,y(1:dim),mode)

    !IVP integration
    call IVP(dim,y(1:dim),s,mode,dsdz)


  end subroutine ShootFun


  !!************************************************
  !!* Interface to ShootFun for Simplicial program *
  !!************************************************
  Subroutine Hom(n,x,hlambda,h)
    implicit none

    integer, intent(in) :: n !!problem dimension
    real(kind=8), intent(in) :: hlambda !!homotopic parameter
    real(kind=8), dimension(n), intent(in) :: x !!unknown
    real(kind=8), dimension(n), intent(out) :: h !!homotopy value at (x,lambda)

    !Local
    integer :: iflag

    !out init
    h = 0d0

    !call init
    lambda = hlambda
    call ShootFun(n,x,h,iflag)


  end Subroutine Hom


  !!***************************************************
  !!* Interface for nonlinear solver HYBRJ (Jacobian) *
  !!***************************************************
  Subroutine JacFun(n,x,fvec,fjac,ldfjac,iflag)
    implicit none
    integer, intent(in) :: n !!problem dimension
    integer, intent(in) :: ldfjac !!jacobian matrix leading dimension
    integer, intent(inout) :: iflag
    real(kind=8), intent(in), dimension(n) :: x !!shooting function unknown
    real(kind=8), intent(out), dimension(n) :: fvec !!shooting function value
    real(kind=8), intent(out), dimension(ldfjac,n) :: fjac !!shooting function jacobian

    !Local declarations
    integer :: i, j, mode, dim
    real(kind=8) :: normjac, normfdjac, error, h
    real(kind=8) :: zd(n), s(n), y(xpdim*(n+1))
    real(kind=8) :: fdjac(n,n), dsdz(n,n), diff(n,n)


    ! iflag = 1 calculate the functions at x and return this vector in fvec. do not alter fjac.
    ! iflag = 2 calculate the jacobian at x and return this matrix in fjac. do not alter fvec.

    if (iflag == 1 .and. forcevareq == 0) then

       !Shooting Function evaluation
       !out init
       fvec = 0d0

       !call init
       mode = 0
       call ShootFun(n,x,fvec,mode)

    else

       !Jacobian evaluation
       !out init
       fjac = 0d0

       !IVP+VAR initialization
       !call init
       zd = x
       dim = xpdim * (n+1)   !y holds [y + dydz by columns]
       mode = 2
       call InitIVP(zd,dim,y,mode)

       !IVP+VAR integration
       call IVP(dim,y,s,mode,dsdz)

       !retrieve jacobian
       do i = 1,n
          fjac(i,1:n) = dsdz(i,1:n)
       end do
       
       !CHECK with manual finite diff
       !NE PAS APPELER LA ROUTINE DANS HYBRD, CA FAIT N'IMPORTE QUOI
       if (shootdebug > 0) then

          h = 1d-6
          call FDJacFun(n,x,fdjac,h)
          diff = fjac - fdjac
          normjac = sqrt(sum(fjac*fjac))
          normfdjac = sqrt(sum(fdjac*fdjac))
          if (normjac * normfdjac > 1d-14) then
             error = sum(diff*diff) / normjac / normfdjac
             !if (.not. (error < 1d0) .or. shootdebug > 0) write(0,*) 'Jac REL error', error
          else
             error = sqrt(sum(diff*diff))
             !if (.not. (error < 1d0) .or. shootdebug > 0) write(0,*) 'Jac ABS error, jacnorm, fdjacnorm', error, normjac, normfdjac
          end if

          if (.not. (error < 1d-2)) then
             !display VAR internals
             !write(0,*) 'dsdz computed during ivp'
             !do i=1,n
             !   write(0,*) dsdz(i,1:n)
             !end do
             !write(0,*) ''
             write(0,*) 'VAR jac'
             do i=1,n
                write(0,*) fjac(i,1:n)
             end do
             write(0,*) ''
             write(0,*) 'FD jac'
             do i=1,n
                write(0,*) fdjac(i,1:n)
             end do
             write(0,*) ''
             write(0,*) 'diff',error      
             do i=1,n
                write(0,*) diff(i,1:n)
             end do


             !check FD with h from 1d-3 to 1d-8
             h = 1d-2
             do j=1,6
                h = h * 1d-1
                call FDJacFun(n,x,fdjac,h)
                write(0,*) 'MANUAL FD error for step',h, sqrt(sum((fjac-fdjac)*(fjac-fdjac)))
             end do

             write(0,*) ''
             !write(0,*) 'yscal',yscal
             !write(0,*) ''

             stop
          else
             write(0,*) 'Jac check ok', error
          end if
       end if

    end if !iflag=1


  end subroutine JacFun


  !!*****************************************
  !!* Basic Finite differences for Jacobian *
  !!*****************************************
  Subroutine FDJacFun(n,x,jac,h)
    implicit none

    integer, intent(in) :: n
    real(kind=8), intent(in) :: h
    real(kind=8), dimension(n), intent(in) :: x
    real(kind=8), dimension(n,n), intent(out) :: jac

    !Local
    integer :: i, mode
    real(kind=8) :: s0(n), s(n), z(n)

    !out init
    jac = 0d0

    !call init
    mode  = 0
    call ShootFun(n,x,s0,mode) 

    do i=1,n
       z = x
       z(i) = x(i) + h
       call ShootFun(n,z,s,mode)
       jac(1:n,i) = (s - s0) / h
    end do

  end Subroutine FDJacFun


  !!***************************************
  !!* Initializations for Shooting method *
  !!* Read .in file                       *
  !!***************************************
  Subroutine InitShoot(prefname,dim,zstart,scal,debug,verbose)
    implicit none

    character(len=66), intent(inout) :: prefname
    integer, intent(in) :: dim, verbose, debug
    real(kind=8), dimension(dim+1), intent(out) :: zstart
    real(kind=8), dimension(dim), intent(out) :: scal

    !Local    
    logical :: datafile
    character(len=79) :: text, blank
    integer, parameter :: fid = 9
    integer :: i, mode

    !out init
    zstart = 0d0
    scal = 0d0

    !debug and verbose flags
    shootdebug = debug
    shootverbose = verbose

    !Open init file
    inquire (file=trim(prefname)//'.in', exist=datafile)
    do while (.not. datafile)
       write(outputfid,*) 'Init file ', trim(prefname)//'.in' ,' not found, enter problem files prefix: '
       read (*,*) prefname
       inquire (file=trim(prefname)//'.in', exist=datafile)
    end do

    if (shootverbose > 1) write(outputfid,*) 'Using input file ', trim(prefname)//'.in'

    open(unit=fid, file=trim(prefname)//'.in')

    !Read state and control dimensions
    read(fid,'(a79)') text
    read(fid,*) homotop, problemclass
    read(fid,'(a79)') text
    read(fid,*) n, ns, nc, m
    xpdim = ns+nc
    if (n .ne. dim) then
       write(0,*) 'ERROR: InitHomotopy >>> Unknown dimension mismatch...',n,dim
       stop
    end if
    allocate(currentpoint(xpdim),utry(m))
    read(fid,'(a79)') text
    read(fid,*) dimcrit, dimpsi!, hamiltonianoutput
    hamiltonianoutput = 0

    !IVP unknown
    read(fid,'(a79)') blank
    read(fid,'(a79)') text
    read(fid,*) nz
    allocate(free0(nz))
    read(fid,'(a79)') text
    read(fid,*) (free0(i), i=1,nz)

    !Initial values
    read(fid,'(a79)') blank
    read(fid,'(a79)') text
    read(fid,*) nci
    read(fid,'(a79)') text
    allocate(fixed0(nci),ci(nci),ci0(nci))
    read(fid,*) (fixed0(i), i=1,nci)
    read(fid,'(a79)') text
    read(fid,*) (ci(i), i=1,nci)

    !Terminal values
    read(fid,'(a79)') blank
    read(fid,'(a79)') text
    read(fid,*) ncf
    allocate(fixedT(ncf),cf(ncf),cf0(ncf))
    read(fid,'(a79)') text
    read(fid,*) (fixedT(i), i=1,ncf)
    read(fid,'(a79)') text
    read(fid,*) (cf(i), i=1,ncf)

    !Initial and Final Time
    read(fid,'(a79)') blank
    read(fid,'(a79)') text
    read(fid,*) nbfixedtimes
    allocate(fixedtimes(nbfixedtimes))
    read(fid,*) (fixedtimes(i), i=1,nbfixedtimes)

    !Nodes and switching times
    read(fid,'(a79)') blank
    read(fid,'(a79)') text
    read(fid,*) nbtimes1
    allocate(structure1(nbtimes1))
    read(fid,*) (structure1(i), i=1,nbtimes1)

    !Starting point
    allocate(xstart(n+1))
    read(fid,'(a79)') blank
    read(fid,'(a79)') text
    read(fid,*) (xstart(i), i=1,n+1)
    zstart = xstart

    !Integrator parameters
    read(fid,'(a79)') blank
    read(fid,'(a79)') text
    read(fid,*) scalemode
    read(fid,'(a79)') text
    read(fid,*) integmode0, fixedsteps0, atol0, rtol0   

    !Switching detection
    read(fid,'(a79)') blank
    read(fid,'(a79)') text
    read(fid,*) ntry
    !Jacobian mode
    read(fid,'(a79)') text
    read(fid,*) vareqmode
    !Singular conditions and control
    read(fid,'(a79)') text
    read(fid,*) dimsing, singularcontrolmode

    !Parameters
    read(fid,'(a79)') blank
    read(fid,'(a79)') text
    read(fid,*) npar
    allocate (par(npar))
    read(fid,'(a79)') text
    read(fid,*) par

    close(fid)

     !check options wrt switching function derivatives availability and vareq mode
    if (dimpsi == 1 .and. dimsing == 2) then
       write(0,*) 'WARNING: InitShoot >>> Resetting dimsing to 1 due to dimpsi...',dimpsi
       dimsing = 1
    end if
    if (vareqmode > 0 .and. dimsing == 2) then
       write(0,*) 'WARNING: InitShoot >>> Resetting dimsing to 1 due to VAR...',vareqmode
       dimsing = 1
    end if
    if (dimpsi == 1 .and. singularcontrolmode == 2) then
       write(0,*) 'WARNING: InitShoot >>> Resetting singularcontrolmode to 1 due to dimpsi...',dimpsi
       singularcontrolmode = 1
    end if


    !scaling vectors allocation
    allocate(zscal(n),yscal(xpdim))
    zscal = 1d0
    yscal = 1d0

    !Times basic information
    !tff = 0 indicates fixed final time if not reset in InitPar
    tff = 0d0
    t0 = fixedtimes(1)
    tf = fixedtimes(nbfixedtimes)
    tf0 = tf
    !basic structure (might be needed in InitPar)
    nbphase = 1 + count(abs(structure1) == 1) + count(abs(structure1) == 7)
    nbswitch = count(abs(structure1) == 2) + count(abs(structure1) == 3)

    !disable VAR for singular arcs (for now)
    if (nbswitch > 0 .and. vareqmode > 0) then
       write(0,*) 'INFO: Disabling VAR for singular arcs...'
       vareqmode = 0
    end if

    !Some integration parameters
    integmode = integmode0
    fixedsteps = fixedsteps0
    atol = atol0
    rtol = rtol0
    macheps = 1d-12 !epsilon(1d0)
    maxswitch = 50
    matchDt = (tf-t0)/fixedsteps 

    !set integration precision for nonlinear solver
    if (integmode > 4)  then
       epsfun = rtol
    else
       !NB ca marche pas bien de fixer la precision a pas fixe en fonction du pas et ordre...
       epsfun = 1d-12 !ca fera un pas dans les 1d-6 pour la diff finie interne a hybrd
    end if

    !hamiltonian correction mode
    !hcorr = 0
    !symcross = 0

    !Fixed point parameters initialization
    totalfpiter = 0
    fpiter = 0
    totalfpcount = 0

    !Bissection for switching detection
    globaldicho = 0
    globaldichoiter = 0

    !Switching detection and Variational system
    switchmode = 0
    forcevareq = 0