shoot.f90

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

  
  end subroutine IVP





  !!*******************
  !!* Objective value *
  !!*******************
  Subroutine Objective(n,z,s,crit)
    implicit none
    integer, intent(in) :: n !!problem dimension
    real(kind=8), dimension(n), intent(in) :: z !!shooting function unknown    
    real(kind=8), dimension(n), intent(out) :: s !!shooting function value
    real(kind=8), dimension(dimcrit), intent(out) :: crit

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

    !out init
    crit = 0d0
    s = 0d0

    !Compute shooting function + objective

    !IVP initialization
    !call init
    zd = z
    dim = xpdim + dimcrit
    mode = 1
    call InitIVP(zd,dim,y,mode)

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

    !retrieve objective
    crit(1:dimcrit) = y(xpdim+1:xpdim+dimcrit)

    if (shootdebug > 0) write(0,*) 'crit',crit


  end subroutine Objective



  !!****************************
  !!* Shooting method scalings *
  !!****************************
  Subroutine ShootScale(dim,z,scalemode)
    implicit none
    integer, intent(in) :: dim !!IVP unknown dimension
    integer, intent(in) :: scalemode !!scaling mode
    real(kind=8), dimension(dim), intent(inout) :: z !!unscaled IVP unknown 

    !Local
    integer :: offset, i
    real(kind=8) :: localscal(2*n)

    ! scaling mode 1: classic scaling to [0.1 1]

    !Scale IVP unknown z
    zscal = 1d0
    call Subscale(nz,z,zscal(1:nz),scalemode)  

    !State-Costate scaling
    yscal = 1d0
    yscal(free0) = zscal(1:nz)
    localscal = 1d0
    call Subscale(nci,ci,localscal(1:nci),scalemode)
    yscal(fixed0) = localscal(1:nci)  
    
    !Additional interior point scaling
    !manual state-costate scaling propagation  
    do i=1,nodes-1 
       offset = nz + (i-1)*xpdim 
       zscal(offset+1:offset+ns) = yscal(1:ns) 
       zscal(offset+ns+1:offset+xpdim) = yscal(ns+1:xpdim)
       z(offset+1:offset+xpdim) = z(offset+1:offset+xpdim) * zscal(offset+1:offset+xpdim)
    end do
    offset = nz + (nodes-1)*xpdim


  end Subroutine ShootScale


  !!**********************************
  !!* Solution file .sol generation  *
  !!**********************************
  Subroutine ShootSol(n,z,hlambda,mode,prefname,zscal)
    implicit none

    integer, intent(in) :: n, mode
    character(len=66), intent(inout) :: prefname
    real(kind=8), intent(in) :: hlambda
    real(kind=8), dimension(n), intent(in) :: zscal
    real(kind=8), dimension(n+1), intent(inout) :: z

    !Local declarations
    integer :: i, j, fid, file, iflag
    character(len=3) :: numero
    real(kind=8) :: crit(dimcrit), s(n)
    real(kind=8), dimension(:), allocatable :: tsol, hamiltsol
    real(kind=8), dimension(:,:), allocatable :: psisol, usol, ysol


    !Initializations and allocations
    lambda = hlambda

    !local allocations
    allocate (timeout(maxsteps+1),yout(maxsteps+1,xpdim),uout(maxsteps+1,m))
    allocate (psiout(maxsteps+1,dimpsi))
    if (hamiltonianoutput == 1) allocate (hamiltout(maxsteps+1))

    outmode = 1
    indout = 1

    !Generate solution trajectory    
    rhscalls = 0
    nbsteps = 0
    accsteps = 0
    rejsteps = 0

    if (shootverbose > 0) write(outputfid,*) 'Generating solution...'
    call Objective(n,z(1:n),s,crit)
    if (shootverbose > 0) write(outputfid,*) 'Criterion value:',crit(1:dimcrit)

    outmode = 0
    finalsteps = indout-1

    if (shootverbose > 0) write(outputfid,*) 'Homotopy norm at lambda=',lambda,':',dsqrt(sum(s*s))
    if (shootverbose > 1) then
       write(outputfid,*) 'Finalsteps',finalsteps
       if (integmode > 4) then 
          write(outputfid,*) 'RHS calls',rhscalls,'Total steps',nbsteps,'Accepted steps',accsteps,'Rejected steps',rejsteps
       else
          write(outputfid,*) 'Total steps',finalsteps
       end if
    end if

    allocate (tsol(finalsteps),psisol(finalsteps,dimpsi),usol(finalsteps,m))
    allocate (ysol(finalsteps,xpdim))
    if (hamiltonianoutput == 1) allocate (hamiltsol(finalsteps))

    tsol(1:finalsteps) = timeout(1:finalsteps)
    ysol(1:finalsteps,:) = yout(1:finalsteps,:)
    usol(1:finalsteps,:) = uout(1:finalsteps,:)
    psisol(1:finalsteps,:) = psiout(1:finalsteps,:)
    if (hamiltonianoutput == 1) hamiltsol(1:finalsteps) = hamiltout(1:finalsteps)

    if (discshoot > 0) write(0,*) 'Switchings detected...',swdetect


    !File output
    file = 0
    fid = 9
    select case (mode)

    case (0)
       open(fid,file=trim(prefname)//'.sol')
       file = 1

    case (1:)
       !generate "prefname.vertxxx.sol" string for solutions at vertices (simplicial continuation)
       write(numero,'(i3)') mode
       open(fid,file=trim(prefname)//'.vert'//trim(adjustl(numero))//'.sol')
       file = 1

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


    if (file == 1) then

       write(fid,*) 'Parameters'
       write(fid,*) npar
       write(fid,*) par
       write(fid,*) 'Unknown, State, Costate and Control dimension'
       write(fid,*) n, ns, nc, m
       write(fid,*) 'Terminal values indices'
       write(fid,*) fixedT
       write(fid,*) 'Terminal values'
       do i=1, ncf
          write(fid,'(es21.14)') cf(i)
       end do
       write(fid,*) ''

       !unscale solution
       write(fid,*) 'Solution value'
       do j = 1, n
          write(fid,'(es21.14)') z(j) / zscal(j)
       end do
       write(fid,'(es21.14)') z(n+1)

       write(fid,*) ''

       write(fid,*) 'Homotopy value and norm at the solution'
       do i=1,n
          write(fid,'(es21.14)') s(i)
       end do
       write(fid,*) ''
       write(fid,'(es21.14)') dsqrt(sum(s*s))
       write(fid,*) ''

       write(fid,*) 'Criterion value:'
       write(fid,*) dimcrit
       write(fid,'(es21.14)') crit
       write(fid,*) ''

       !Optimal control and trajectory corresponding to solution
       write(fid,*) 'Optimal control and trajectory:'
       write(fid,*) finalsteps
       write(fid,*) ''

       do i = 1, finalsteps
          write(fid,'(es21.14)') tsol(i)
          write(fid,*) ''
          do j=1,xpdim
             write(fid,'(es21.14)') ysol(i,j)
          end do
          do j=1,m
             write(fid,'(es21.14)') usol(i,j)
          end do
          write(fid,*) ''
       end do

       write(fid,*) 'Switching function psi'
       write(fid,*) dimpsi
       do i = 1, finalsteps
          do j=1, dimpsi
             write(fid,'(es21.14)') psisol(i,j)
          end do
       end do
       write(fid,*) ''

       write(fid,*) 'Hamiltonian'
       write(fid,*) hamiltonianoutput
       if (hamiltonianoutput == 1) then
          do i = 1, finalsteps
             write(fid,'(es21.14)') hamiltsol(i)
          end do
       end if
       write(fid,*) ''

       close (fid)
    end if

    deallocate(tsol,psisol,usol,ysol,timeout,yout,uout,psiout)
    if (hamiltonianoutput == 1) deallocate(hamiltsol,hamiltout) 

  end subroutine ShootSol


  !!*****************************************************************
  !!* Estimate Conditionning number of the Jacobian at the solution *
  !!*****************************************************************
  Subroutine ConditionNumber(z,hlambda,cond)
    implicit none
    real(kind=8), intent(in) :: hlambda
    real(kind=8), dimension(n), intent(in) :: z !!solution
    real(kind=8), intent(out) :: cond !!conditioning number

    !local 
    integer :: iflag
    real(kind=8) :: normjac, norminvjac, norms, h
    real(kind=8), dimension(n) :: fvec, toto
    real(kind=8), dimension(n,n) :: jac, invjac

    !out init
    cond = 0d0


    lambda = hlambda
    call ShootFun(n,z,fvec,iflag)

    jac = 0d0
    if (vareqmode == 0) then
       !ne pas appeler a la boeuf la routine dans hybrd, apparemment ca vasouille
       h = sqrt(epsfun)
       call FDJacFun(n,z,jac,h)
    else
       call JacFun(n,z,toto,jac,n,iflag)
    end if

    iflag = 0
    invjac = 0d0
    call Invert(n,jac,invjac,iflag,shootverbose)
    if (iflag .ne. 0) write(0,*) 'WARNING: ConditionNumber >>> Invert returns...',iflag

    !Froebinius norm
    normjac = sqrt(sum(jac*jac))
    norminvjac = sqrt(sum(invjac*invjac))

    cond = normjac * norminvjac

    norms = sqrt(sum(fvec*fvec))
    if (shootverbose > 1) write(outputfid,*) 'Cond at', z,lambda
    if (shootverbose > 1) write(outputfid,*) 'Function, jacobian and jacobian inverse norm:',norms, normjac, norminvjac

  end Subroutine ConditionNumber




  !!*******************
  !!* Shooting method *
  !!*******************
  Subroutine Shoot(z,shootlambda,info,normhyb,hcall,cond,debug,verbose)
    implicit none
    integer, intent(in) :: cond, debug, verbose
    integer, intent(inout) :: info, hcall
    real(kind=8), intent(in) :: shootlambda
    real(kind=8), intent(inout) :: normhyb
    real(kind=8), intent(inout), dimension(n) :: z !!Shooting initial point (scaled)


    !Local
    real(kind=8) :: condnum

    !local declarations for HYBRD
    integer :: ml, mu, mode, np, l,  nfev, njac
    real(kind=8) :: fa
    real(kind=8), dimension(n) :: diag, s, qtf, w1, w2, w3, w4
    real(kind=8), dimension((n*(n+1))/2) :: r
    real(kind=8), dimension(n,n) :: fjac


    !set flags
    shootdebug = debug
    shootverbose = verbose

    !Solver choice (nonlinsolver)
    !0: HYBRD/HYBRJ
    !1: NLEQ1

    if (outmode*shootdebug > 0) write(0,*) 'Shoot initial point (unscaled)',z/zscal

    nonlinsolver = 0
    lambda = shootlambda

    if (discshoot == 1) then
       select case (integmode)
       case (-4,-2,1,4,6,8)
          if (shootverbose > 0) write(0,*) 'Switching detection enabled...'
       case default
          write(0,*) 'ERROR >>> hyb: switching detection incompatible with integmode...',integmode
          stop
       end select
    end if


    !write(0,*) 'epsfun',epsfun
    select case (nonlinsolver)

    case(0)
       !HYBRD / HYBRJ solver
       info = -1
       ml = n - 1
       mu = n - 1
       mode = 1
       fa = 100d0
       np = 0
       l = (n*(n+1))/2
       njac = 0
       nfev = 0
       w1 = 0d0
       w2 = 0d0
       w3 = 0d0
       w4 = 0d0
       qtf = 0d0
       s = 0d0
       diag = 0d0
       fjac = 0d0
       r = 0d0


       if (vareqmode == 0) then
          call hybrd(ShootFun,n,z,s,xtol,maxfev,ml,mu,epsfun,diag,mode,fa,np,info,nfev,njac &
               & ,fjac,n,r,l,qtf,w1,w2,w3,w4)
          if (shootverbose > 0) write(outputfid,*) 'HYBRD solver returns: ', info, 'NFEV:',nfev, '(NJAC:',njac,')'
       else
          call hybrj(JacFun,n,z,s,fjac,n,xtol,maxfev,diag,mode,fa,np,info,nfev,njac &
               & ,r,l,qtf,w1,w2,w3,w4)
          if (shootverbose > 0) write(outputfid,*) 'HYBRJ solver returns: ', info, 'NFEV:',nfev, 'NJAC:',njac
       end if
       normhyb = dsqrt(sum(s*s))
       hcall = nfev



       !NOTE: RETESTER UN JOUR NLEQ1, APPAREMMEMT MEILLEUR QUE NLEQ2...


    case default
       write(outputfid,*) 'ERROR: Hyb >>> Unknown nonlinear solver...',nonlinsolver
       stop
    end select

    !condition number at solution
    if (cond > 0) then
!!$       write(outputfid,*) 'Condition number at solution:'
!!$       call ConditionNumber(z,lambda,condnum)
!!$       write(outputfid,*) 'Condition number at solution:',condnum
    end if



  end subroutine Shoot


  !!*************************
  !!* Dynamic deallocations *
  !!*************************
  Subroutine DeallocShoot()
    implicit none

    deallocate(yscal,zscal)
    deallocate(free0,fixedT,cf,cf0,fixed0,ci,ci0,xstart,par)
    deallocate(currentpoint)
    deallocate(structure1,times1,structure,times,ynodes,fixedtimes)
    deallocate(utry)
    if (allocated(tscal)) deallocate(tscal)
    if (allocated(swdetect)) deallocate(swdetect)

  end Subroutine DeallocShoot

end Module Shooting