shootcont.f90

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

!************************************************
! Program Shootcont                             *
! Discrete continuation for the shooting method *
! Author: Pierre Martinon                       *
! INRIA FUTURS, team COMMANDS                   *
! CMAP, ECOLE POLYTECHNIQUE                     *
! 10/20007                                      *
!************************************************


Program ShootCont
  use Shooting
  implicit none


  !Local
  integer, parameter :: fid = 77
  logical :: datafile, success, victory, belltoll, swamp
  character(len=66) :: prefname
  character(len=256) :: line
  integer :: dim, info, hcall, iter, maxiter, outputiter, verbose, debug, cond, mode
  integer :: pathindex, i, j, k, linearmode, maxshoot
  real(kind=8) :: lambda0, lambda1, tol, begintime, endtime, time, ninthgate, step, minstep
  real(kind=8), dimension(:), allocatable :: z, scal, h, z0
  real(kind=8), dimension(:,:), allocatable :: contpath


  !Inits
  iter = 0
  info = 0
  hcall = 0
  begintime = 0d0
  endtime = 0d0
  time = 0d0
  normh = 0d0
  success = .false.
  datafile = .false.
  victory = .false.
  belltoll = .false.
  swamp = .false.
  ninthgate = 1d-6
  !minimal stepsize (nb: moins de 1d-6 ne semble guere utile, ca n'avance quasi plus a ce stade)
  minstep = 1d-6
  !linearize path
  linearmode = 1
  maxshoot = 3

  !Open continuation file
  prefname = "Problem"
  inquire (file=trim(prefname)//'.cont', exist=datafile)
  do while (.not. datafile)
     write(outputfid,*) 'Continuation file ', trim(prefname)//'.cont' ,' not found, enter problem files prefix: '
     read (*,*) prefname
     inquire (file=trim(prefname)//'.cont', exist=datafile)
  end do
  
  open(unit=fid, file=trim(prefname)//'.cont')
  read(fid,*) line
  read(fid,*) dim
  read(fid,*) line
  read(fid,*) lambda0, lambda1 
  read(fid,*) line
  read(fid,*) tol, maxiter, outputiter
  read(fid,*) line
  read(fid,*) debug, verbose
  close(fid)

  !Allocations
  allocate(z(dim+1),scal(dim),h(dim),z0(dim+1),contpath(maxiter+2,dim+1))
  z = 0d0
  scal = 1d0
  h = 0d0

  !Shoot init
  call InitShoot(prefname,dim,z,scal,debug,verbose)
  !NB supprimer le scaling ?!
  z(1:dim) = z(1:dim) * scal
  lambda = z(dim+1)
  z0 = z
  if (lambda /= lambda0) then
     write(0,*) 'Initial lambda mismatch...',lambda,lambda0
     stop
  end if

  !Test initial point
  call Hom(dim,z,lambda,h)
  normh = dsqrt(sum(h*h))
  write(0,*) ''
  write(0,*) 'Testing Initial Shooting point',normh
  success = (normh .le. tol)
  
  call cpu_time(begintime)

  !Initial Shoot
  if ((lambda1 .ge. lambda0) .and. .not. success) then
     lambda = lambda0
     !ask condition number ?
     cond = 1
     write(0,*) ''
     write(0,*) 'Initial Shoot...'
     call Shoot(z,lambda,info,normh,hcall,cond,debug,verbose)
     z(dim+1) = lambda
     !if (verbose > 0) then
     write(0,*) 'Solution:'
     do i=1,dim
        write(0,*) z(i) / scal(i)
     end do
     write(0,*) z(dim+1)
     !end if
     write(0,*) 'Shooting function norm at the solution: ',normh
     success = (normh .le. tol)
     i = 1
     do while (.not. success .and. i < maxshoot)
        i = i+1
        call Shoot(z,lambda,info,normh,hcall,cond,debug,verbose)
        write(0,*) 'Initial shoot refining: ',normh
        success = (normh .le. tol)
     end do
  end if !(lambda1 .ge. lambda0)


  pathindex = 1

  !Continuation
  if (lambda1 > lambda0) then

     write(0,*) ''
     write(0,*) 'Starting discrete homotopy...'
     iter = 0
     belltoll = (iter .ge. maxiter)
     !disable condition number
     cond = 0 
     step = lambda1 - lambda0

     !discrete homotopy
     do while (.not.(victory .or. belltoll .or. swamp))
        iter = iter + 1
        if (success) then
           contpath(pathindex,:) = z
           pathindex = pathindex + 1
           lambda = lambda + step
           if (lambda > lambda1) lambda = lambda1
           if (linearmode == 1 .and. pathindex > 2) then
              z = z + (z-contpath(pathindex-2,:)) * (lambda-z(dim+1)) / (z(dim+1)-contpath(pathindex-2,dim+1)) 
           end if
        else
           z = z0
           step = step / 2d0
           lambda = lambda - step
           if (lambda < lambda0) lambda = lambda0
           if (linearmode == 1 .and. pathindex > 2) then
              z = z + (z-contpath(pathindex-2,:)) * (lambda-z(dim+1)) / (z(dim+1)-contpath(pathindex-2,dim+1)) 
           end if
        end if
        z0 = z
        !z(dim+1) = lambda la ca marche plus...
        call Shoot(z,lambda,info,normh,hcall,cond,debug,0)
        z(dim+1) = lambda !pourquoi apres le shoot ? ca a l'air important ;-) 
        if (mod(iter,outputiter) == 0) then
           write(0,'(a,i4,a,es12.6,a,es12.6)') 'Iteration: ',iter,'    lambda: ',lambda,'    Norm: ',normh
        end if
        success = (normh .le. tol)
        swamp = (step < minstep)
        belltoll = (iter .ge. maxiter)
        victory = success .and. (abs(lambda1-lambda) < ninthgate)
     end do
     if (mod(iter,outputiter) /= 0) then
        write(0,'(a,i4,a,es12.6,a,es12.6)') 'Iteration: ',iter,'    lambda: ',lambda,'    Norm: ',normh
     end if

     !debriefing
     if (victory) write(0,*) 'Discrete Homotopy successfull...'
     if (belltoll) write(0,*) 'Maximum number of iterations reached...',iter
     if (swamp) write(0,*) 'Lambda stepsize fell under minimal value...',step

     !Final shoot
     if (.not. victory) then
        if (.not. success) lambda = lambda - step
        !ask condition number
        cond = 0 
        write(0,*) ''
        write(0,*) 'Final Shoot...',lambda
        call Shoot(z,lambda,info,normh,hcall,cond,debug,verbose)
        z(dim+1) = lambda
     end if

     !Final solution
     write(0,*) 'Solution:'
     do i=1,dim
        write(0,*) z(i) / scal(i)
     end do
     write(0,*) z(dim+1)
     write(0,*) 'Shooting function norm at the solution: ',normh
  end if !(lambda1 > lambda0)

  !save last point (or intial point if no continuation)
  contpath(pathindex,:) = z


  !Cpu time and iterations
  call cpu_time(endtime)
  time = endtime - begintime
  write(0,*) ''
  write(0,*) 'Iterations: ', iter, ' Execution time: ',time

  !Generating solution
  mode = 0
  call ShootSol(dim,z,lambda,mode,prefname,scal)

  !save continuation path
  open(unit=fid, file=trim(prefname)//'.contpath')
  write(fid,*) 'Discrete continuation path'
  write(fid,*) 'Problem dimension and path length'
  write(fid,*) dim, pathindex
  write(fid,*) 'Path'
  do j=1,pathindex
     do k=1,dim
        write(fid,*) contpath(j,k) /  scal(k)
     end do
     write(fid,*) contpath(j,dim+1)
     write(fid,*) ''
  end do
  close(fid)

  !Deallocate shoot
  call DeallocShoot()


end Program ShootCont