shoot.f90

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

    !hermite interpolation order
    hermiteorder = 3
    !coeff for bootstrapping for dense output
    alphaboot = 0.75d0

    !call init
    mode = 0
    call InitPar(mode,xstart(1:n),xstart(n+1))

    !Allocations
    !times
    nbtimes = nbtimes1 + problemclass - 1
    allocate(structure(nbtimes),times(nbtimes),times1(nbtimes1))

    !nodes
    nodes = 1 + count(structure1 > 0) + (problemclass - 1)
    allocate(ynodes(nodes,xpdim))

    !allocate swiching detection array
    if (ntry > 0) then
       discshoot = 1
       allocate(swdetect(ntry))
       swdetect = 0
    end if

    !initial point scaling
    scal = 1d0
    if (scalemode > 0) then
       !hmm, pas tres propre ces vecteurs de scaling planques...
       call ShootScale(n,xstart(1:n),scalemode)
       scal(1:dim) = zscal(1:n)
    end if


  end subroutine InitShoot



  !!**********************
  !!* IVP initialization *
  !!**********************
  Subroutine InitIVP(z,dim,y,mode)
    implicit none

    integer, intent(in) :: dim !!IVP dimension
    integer, intent(in) :: mode !!IVP mode
    real(kind=8), dimension(n), intent(in) :: z !!IVP unknown
    real(kind=8), dimension(dim), intent(out) :: y !!IVP initial value
 
    !Local
    integer :: i, j, k, mode2, offset
    real(kind=8) :: timestep, tk

    !IVP mode
    !0: compute shooting function only
    !1: compute shooting function + objective
    !2: compute shooting function + jacobian (VAR)

    !out init
    y = 0d0

    !check that singular cond dimension has been specified
    if (nbswitch > 0 .and. dimsing == 0) then
       write(0,*) 'InitIVP: ERROR >>> Please set singular conditions dimension (to 1 or 2)...',dimsing
       stop
    end if
    
    !Parameters initialization
    mode2 = 1
    call InitPar(mode2,z,lambda)
   
    !initialize y(0)
    y = 0d0  
    y(fixed0) = ci
    y(free0) = z(1:nz)

    !build structure for single/multiple shooting
    !nodes initial values
    ynodes = 0d0
    ynodes(1,1:xpdim) = y(1:xpdim)
    if (nodes > 1) then
       do j = 1, nodes - 1
          ynodes(j+1,1:xpdim) = z(nz + (j-1)*xpdim + 1 : nz + j * xpdim)
       end do
    end if



    !DEROULER LA BOUCLE EN METTANT LES T FIXES AUX STRUCT 0
    offset = nz+(nodes-1)*xpdim
    j = 1
    k = 1
    do i=1, nbtimes1
       if (structure1(i) == 0 .or. abs(structure1(i)) == 7) then
          !FIXED TIME
          times1(i) = fixedtimes(k)
          k = k + 1
       else
          !UNKNOWN TIME, PART OF Z
          times1(i) = z(offset + j) / zscal(offset + j)
          j = j + 1
       end if
    end do



    !complete structure (add interior nodes for multiple shooting)
    structure  = 0
    times = 0d0

    if (problemclass == 1) then
       !single shooting
       structure = structure1
       times = times1
    else
       !multiple shooting
       times(1) = times1(1)
       times(nbtimes) = times1(nbtimes1)
       timestep = (times(nbtimes) - times(1)) / problemclass
  
       k = 1
       j = 2
       do i = 2, nbtimes - 1
          tk = times(1) + k * timestep
          if (j > nbtimes1 - 1 .or. (tk < times1(j)) .and. (tk < tf) ) then
             structure(i) = 0
             times(i) = times(1) + k * timestep
             k = k + 1
          else
             structure(i) = structure1(j)
             times(i) = times1(j)
             j = j + 1
          end if
       end do
    end if


    if (outmode * shootdebug > 0) then

       write(0,*) 'Fixed times: ', fixedtimes

       write(0,*) 'Structure: ',structure

       write(0,*) 'Nodes:',nodes,'Phases:',nbphase,'Arcs switchs:',nbswitch
       
       if (tff == 0d0) then
          write(outputfid,*) 'times', times
       else
          write(outputfid,*) 'times*tff', times*tff
       end if

       if (shootdebug > 0) then
          do j=1,nodes
             write(0,*) ynodes(j,1:xpdim)
          end do
       end if

    end if

    !inits for objective computation
    if (mode == 1) then
       y(xpdim+1:xpdim+dimcrit) = obj0
       !if (shootdebug > 0) write(0,*) 'obj0',obj0
    end if

    !inits for jacobian computation (VAR)
    if (mode == 2) then
       !Jacobian evaluation
       switchcount = 0
       !initialize dy/dz: dy/dz(0) = dy0/dz(z)  (with y0(free0) = z)
       do j=1,nz
          y(xpdim + (j-1) * xpdim + free0(j)) = 1d0
       end do
    end if

  end subroutine InitIVP



  !!*******************************************
  !!* Entry / Exit condition for singular arc *
  !!*******************************************
  Subroutine SingularCondition(t,y,sing,dimsing)
    implicit none

    integer, intent(in) :: dimsing !!singular conditions dimension
    real(kind=8), intent(in) :: t !!time
    real(kind=8), dimension(xpdim), intent(in) :: y !!state-costate
    real(kind=8), dimension(dimsing), intent(out) :: sing !!singular conditions

    !Local
    real(kind=8) :: x(ns), p(nc), u(m)

    !out init
    sing = 0d0

    !switch value
    call PreProcess(t,y,x,p)
    call Switch(lambda,x,p,sing(1))

    if (dimsing > 1) then
       call Control(lambda,x,p,u)
       call Switchdot(lambda,x,p,u,sing(2))
    end if

    if (outmode * shootverbose > 0) then
       write(0,*) 'Switching condition at',t,'is',sing  
    end if


  end subroutine SingularCondition



!!$  Subroutine ConstraintCondition(t,y,act,dimact,mode)
!!$    implicit none
!!$    
!!$    integer, intent(in) :: dimact,mode
!!$    real(kind=8), intent(in) :: t
!!$    real(kind=8), dimension(xpdim), intent(in) :: y
!!$    real(kind=8), dimension(dimact), intent(out) :: act
!!$
!!$    !Local
!!$    integer :: controlmodebackup
!!$    real(kind=8) :: h1, h2
!!$    real(kind=8) :: x(ns), p(nc), u(m)
!!$
!!$
!!$    !mode: 1 entry 2 exit
!!$
!!$    call PreProcess(t,y,x,p)
!!$
!!$    controlmodebackup = controlmode
!!$
!!$    !NB: faire constraint et constraintdot (cf switch)
!!$
!!$    !hamiltonian continuity
!!$    controlmode = 0
!!$    call Control(lambda,x,p,u)
!!$    h1 = p(1)*x(3) + p(2)*x(4) + (p(3)+mu)*cos(u(1)) + p(4)*sin(u(1)) 
!!$    !if (outmode > 0) write(0,*) 'U1, mu1, H1, x3',u, mu, h1, x(3)
!!$   
!!$
!!$    !jump +++ pb scaling apparemment, a checker
!!$    if (mode == 1)  then
!!$       p(3) = p(3) - pijump
!!$    end if
!!$    
!!$    controlmode = 4
!!$    call Control(lambda,x,p,u)
!!$    h2 = p(1)*x(3) + p(2)*x(4) + (p(3)+mu)*cos(u(1)) + p(4)*sin(u(1)) 
!!$    !if (outmode > 0) write(0,*) 'U2, mu2, H2, x(3)',u, mu, h2, x(3)
!!$
!!$    act(1) = h2 - h1
!!$
!!$    if (dimact > 1) then
!!$       !tangential entry for order 1: g = 0
!!$       act(2) = x(3) - 1d0
!!$    end if
!!$
!!$    controlmode = controlmodebackup
!!$
!!$
!!$
!!$    !avant
!!$    !entry: g = 0
!!$    !exit: g = 0 ou p3 = 0
!!$
!!$  end Subroutine ConstraintCondition


  !!********************************
  !!* IVP integration for shooting *
  !!********************************
  Subroutine IVP(dim,y,s,mode,dsdz)
    implicit none

    integer, intent(in) :: dim  !!IVP dimension
    integer, intent(in) :: mode !!IVP mode
    real(kind=8), dimension(dim), intent(inout) :: y !!IVP variable
    real(kind=8), dimension(n), intent(out) :: s !!Shooting function value
    real(kind=8), dimension(n,n), intent(out) :: dsdz !!Shooting function jacobian for VAR

    !Local declarations
    integer :: arc, j, k, offset, singoffset, matchoffset, node, switch, dimact
    real(kind=8) :: tin, tout
    real(kind=8) :: ymatch(xpdim), phiprev(xpdim), phinext(xpdim), deltaphi(xpdim)
    real(kind=8) :: gradpsi(xpdim), dtaudz(n)
    real(kind=8) :: dsdy(ncf,xpdim), dydz(xpdim,n), jump(xpdim,n), aux(xpdim,n)

    !out init
    s = 0d0
    dsdz = 0d0

    !IVP mode
    !0: compute shooting function only
    !1: compute shooting function + objective
    !2: compute shooting function + jacobian (VAR)

    !controlmode
    !-1: use given control (utry)
    !0: compute usual control via Hamiltonian minimization (Control)
    !1: compute bang-bang control (Control)
    !2: compute exact singular control (Control)
    !3: compute alternate (singular) control (AlternateControl)
    !4: compute control for active constraint

    !singularcontrolmode
    !0: use exact singular control in Control
    !1: use alternate singular control minimizing psi^2
    !2: use alternate singular control minimizing psi^2 + psi'^2

    !nbtimes, structure[nbtimes], times[nbtimes]
    !type d'instant
    !0: instant fixe: t0, tf  ou instant interieur de tir multiple
    !1: changement de phase (-1: light sans match)
    !2: entree arc sing (-2: light sans match)
    !3: sortie arc sing (-3: light sans match)
    !4: point de contact (-4: light sans match)
    !5: entree arc sature (-5: light sans match)
    !6: sortie arc sature (-6: light sans match)
    !7: changement de phase a instant fixe (-7: light sans match)


    !if (shootdebug > 0 .and. mode == 1) write(outputfid,*) 'Integrating objective...',dimcrit, dim, xpdim*nodes
    if (shootdebug > 0 .and. mode == 2) write(outputfid,*) 'Integrating VAR...', dim, xpdim*nodes, xpdim*(n+1)

    !set offset for (interior) conditions other than CF and CT at final time
    offset = ncf + 1

    !set current node and switch
    node = 1
    switch = 0
    phase = 0

    !initialize control by default
    !singflag = 0
    update = 1
    controlmode = discshoot

    !integrate each arc from t0 to tf
    do arc = 1, nbtimes - 1

       !set times and steps (for fixed step integrators)
       tin = times(arc)
       tout = times(arc+1)
       if (integmode < 5 .and. tf > t0) then 
          fixedsteps = max(floor(fixedsteps0 * (tout - tin) / (tf - t0)),1)
       else
          fixedsteps = 1
       end if

       !**************************
       !PRE INTEGRATION OPERATIONS
       select case (structure(arc))

       case (0)
          !INITIAL TIME
          arcentry = 0


          !********************************************
          !matching condition for simple interior nodes
          if (arc > 1) then
             node = node + 1
             !retrieve y at tin
             y(1:xpdim) = ynodes(node,1:xpdim)
             !matching conditions
             s(offset:offset+xpdim-1) = y(1:xpdim) - ymatch(1:xpdim)
             if (outmode * shootverbose > 0) write(0,*) 'Interior Match',arc,s(offset:offset+xpdim-1)
             !save offset for VAR
             matchoffset = offset
             offset = offset + xpdim

             !*********************
             !matchcond derivatives
             if (mode == 2) then
                !ynodes(node,1:xpdim) indices in z are: nz + (node-2)*xpdim + 1 to nz + (node-1)*xpdim 
                dsdz(matchoffset:matchoffset+xpdim-1,1:n) = 0d0
                do j=1,xpdim
                   dsdz(matchoffset + j - 1, nz + (node-2)*xpdim + j) = 1d0
                end do
                !retrieve dydz after jump
                do j = 1,n
                   dydz(1:xpdim,j) = y(xpdim + (j-1)*xpdim + 1 : xpdim + j*xpdim)
                end do

                
                dsdz(matchoffset:matchoffset+xpdim-1,1:n) = dsdz(matchoffset:matchoffset+xpdim-1,1:n) - dydz(1:xpdim,1:n)

                if (shootdebug > 1) then
                   write(0,*) 'Interior match deriv from',matchoffset
                   do j=1,xpdim
                      write(0,*) dsdz(matchoffset+j-1,1:n)
                   end do
                end if

             end if


          end if


       case (-1,-7)
          phase = phase + 1



       case (-2,2)  !SINGULAR ARC ENTRY
 

          !************************
          !singular entry condition
          if (structure(arc+1)==0) dimsing = 1    !ending arc case
          call SingularCondition(tin,y(1:xpdim),s(offset:offset+dimsing-1),dimsing)
          if (outmode * shootverbose > 0) write(0,*) 'Entry cond',s(offset:offset+dimsing-1)
          !save offset for VAR
          singoffset = offset
          offset = offset + dimsing

          !*************************
          !full matching formulation  
          if (structure(arc) > 0) then
             node = node + 1
             !retrieve y at tin
             y(1:xpdim) = ynodes(node,1:xpdim)
             !matching conditions
             s(offset:offset+xpdim-1) = y(1:xpdim) - ymatch(1:xpdim)
             if (outmode * shootverbose > 0) write(0,*) 'Entry Match error',sqrt(sum(s(offset:offset+xpdim-1)*s(offset:offset+xpdim-1)))
             !save offset for VAR
             matchoffset = offset
             offset = offset + xpdim
          end if

          !**************************
          !switch to singular control
          switch = switch + 1
          !singflag = 1