integrators.f90

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

    t = tin
    crossed = 0

!!$    !Prepare equistage approximation for initial values
!!$    call f(neqn, t, y, k, lambda)
!!$    z1 = h * c1 * k
!!$    z2 = h * c2 * k
!!$    z1prev = z1
!!$    z1prev2 = z1
!!$    z1prev3 = z1
!!$    z2prev = z2
!!$    z2prev2 = z2
!!$    z2prev3 = z2


    i = 0
!!$    do while (i < nbsteps)
!!$       i = i + 1


    !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 + c1 * h
       t2 = t + c2 * h

       !Initial value (basic)
       !z1 = 0d0
       !z2 = 0d0  
       call f(neqn, t, y, k, lambda)
       z1 = h * c1 * k
       z2 = h * c2 * k

!!$       !Initial value: equistage approximation k=2/3
!!$       !z1 = 2d0 * z1prev - z1prev2
!!$       !z2 = 2d0 * z2prev - z2prev2
!!$       z1 = 3d0 * z1prev - 3d0 * z1prev2 + z1prev3
!!$       z2 = 3d0 * z2prev - 3d0 * z2prev2 + z2prev3 

       !Fixed point iterations
       fpiter = 0
       normprogress = 1d0
       do while (normprogress > fpminprog .and. fpiter < fpmaxiter)
          call f(neqn, t1, y + z1, k1, lambda)
          call f(neqn, t2, y + z2, k2, lambda)

          newz1 = h * (a11*k1 + a12*k2)
          newz2 = h * (a21*k1 + a22*k2)

          progress(1:neqn) = newz1 - z1
          progress(neqn+1:2*neqn) = newz2 - z2          
          normprogress = sqrt(sum(progress*progress)) / (2*neqn)

          z1 = newz1
          z2 = newz2
          fpiter = fpiter + 1
       end do

       !write(0,*) fpiter

       !Integration
       y = y + h / 2d0 * (k1 + k2)
       t = t + h
!!$       !Equistage approximation update for next step
!!$       z1prev3 = z1prev2
!!$       z1prev2 = z1prev
!!$       z1prev = z1
!!$       z2prev3 = z2prev2
!!$       z2prev2 = z2prev
!!$       z2prev = z2

       totalfpiter = totalfpiter + fpiter
       totalfpcount = totalfpcount + 1 

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

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

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

          if (check == 1 .and. crossed == 1) then
             hamiltonianoutput = 1
             call RHS(xpdim,t,y,phi,lambda)
             write(0,*) 'Gauss: Hamiltonian at step after switch',hamilton
          end if

          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 Gauss2



  !!***********************************************
  !!* Last but not least: IVP Integration routine *
  !!***********************************************
  Subroutine Integrate(neq,lambda,tin,tout,y,mode)
    implicit none

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

    !Local declarations
    integer :: ifail, itol, iout, ipar, lw, liw, km, switchflag2
    integer, dimension(:), allocatable :: iwork
    real(kind=8) :: step
    real(kind=8), dimension(:), allocatable :: work, atolv, rtolv



    !!Integrator choice (integmode)
    !!-----------------------------
    !!-4: Symplectic Gauss 2-stage (4th order)
    !!-3: Symplectic Stormer-Verlet (partitioned, 2nd order)
    !!-2: Symplectic Implicit Midpoint (2nd order)
    !!-1: Symplectic Euler (partitioned)
    !!
    !!1: Explicit Euler
    !!2: Explicit Runge Kutta 2
    !!3: Explicit Runge Kutta 3
    !!4: Explicit Runge Kutta 4
    !!
    !!5: Runge Kutta Fehlberg (rkf45)
    !!6: Dormand Prince 8th order (dop853)
    !!7: Gragg Bulirsch Stoer (odex)
    !!8: Dormand Prince 5th order (dopri5)
    !!
    !!9: Radau for stiff problems (radau5)
    
    
    !!System type (mode) IVP or VAR
    !!-----------------------------
    !!0: IVP, usual integration of y = (x,p)                  ---> USE RHS
    !!1: IVP, integration of y + objective                    ---> USE RHS
    !!2: VAR, variational system integration yy = [y, dy/dz]  ---> USE RHS2


    ipar = 0
    vareqinteg = 0
    !initialize switchflag 
    switchflag = 0


    !Manual Output at initial step
    !backup switchflag
    switchflag2 = switchflag
    !(disable for dop853/odex/dopri5/radau5 integrators)
    if (outmode == 1 .and. integmode <= 5) then
       !write(0,*) 'Solout2'
       call Solout2(tin,y,neq,lambda)
    end if
    !restore switchflag after Solout2    
    switchflag = switchflag2


    select case (integmode)


    case (-4)
       !Symplectic Gauss2
       !allocate coefficients for Gauss2 collocation polynom 
       !if (discshoot > 0) allocate(ag2(neq),bg2(neq),cg2(neq))

       !allocate data for Hermite interpolation
       if (controlmode == 1) then
          allocate(yy0(neq),yy1(neq),ff0(neq),ff1(neq))
          if (hermiteorder == 4) allocate(aa(neq),bb(neq),cc(neq),dd(neq),ee(neq),ffalpha(neq))
          !allocate(ycrossing(neq),precrossing(neq),postcrossing(neq),yehyb(neq))
       end if


       if (mode < 2) then
          call Gauss2(lambda,neq,RHS,y,tin,tout,fixedsteps)
       else
          vareqinteg = 1
          call Gauss2(lambda,neq,RHS2,y,tin,tout,fixedsteps)
       end if

       !if (discshoot > 0) deallocate(ag2,bg2,cg2) 
       if (controlmode == 1) then
          deallocate(yy0,yy1,ff0,ff1)
          if (hermiteorder == 4) deallocate(aa,bb,cc,dd,ee,ffalpha)
          !deallocate(ycrossing,precrossing,postcrossing,yehyb)
       end if



    case (-3)
       !Symplectic Stormer-Verlet
       if (mode < 2) then
          call StormerVerlet(lambda,neq,RHS,y,tin,tout,fixedsteps)
       else
          vareqinteg = 1
          call StormerVerlet(lambda,neq,RHS2,y,tin,tout,fixedsteps)
       end if


    case (-2)
       !Symplectic Implicit Midpoint
       if (controlmode == 1) allocate(midy0(neq),midf0(neq))
       if (mode < 2) then
          call MidpointI(lambda,neq,RHS,y,tin,tout,fixedsteps)
       else
          vareqinteg = 1
          call MidpointI(lambda,neq,RHS2,y,tin,tout,fixedsteps)
       end if
       if (controlmode == 1) deallocate(midy0,midf0)

    case (-1)
       !Symplectic Euler
       if (mode < 2) then
          call EulerS(lambda,neq,RHS,y,tin,tout,fixedsteps)
       else
          vareqinteg = 1
          call EulerS(lambda,neq,RHS2,y,tin,tout,fixedsteps)
       end if


    case (1)
       !Explicit Euler
       if (mode < 2) then
          call Euler(lambda,neq,RHS,y,tin,tout,fixedsteps)
       else
          vareqinteg = 1
          call Euler(lambda,neq,RHS2,y,tin,tout,fixedsteps)
       end if


    case (2)
       !Explicit Runge Kutta 2 (rk2) (explicit trapeze)
       if (mode < 2) then
          call Rk2(lambda,neq,RHS,y,tin,tout,fixedsteps)
       else
          vareqinteg = 1
          call Rk2(lambda,neq,RHS2,y,tin,tout,fixedsteps)
       end if


    case (3)

       !Explicit Runge Kutta 3 (rk3)
       if (controlmode == 1) then
          allocate(yy0(neq),yy1(neq),ff0(neq),ff1(neq))
          if (hermiteorder == 4) allocate(aa(neq),bb(neq),cc(neq),dd(neq),ee(neq),ffalpha(neq))
          !allocate(ycrossing(neq),precrossing(neq),postcrossing(neq),yehyb(neq))
       end if
       if (mode < 2) then
          call Rk3(lambda,neq,RHS,y,tin,tout,fixedsteps)
       else
          vareqinteg = 1
          call Rk3(lambda,neq,RHS2,y,tin,tout,fixedsteps)
       end if
       if (controlmode == 1) then
          deallocate(yy0,yy1,ff0,ff1)
          if (hermiteorder == 4) deallocate(aa,bb,cc,dd,ee,ffalpha)
          !deallocate(ycrossing,precrossing,postcrossing,yehyb)
       end if

    case (4)
       !Explicit Runge Kutta 4 (rk4)
       if (controlmode == 1) then
          allocate(yy0(neq),yy1(neq),ff0(neq),ff1(neq))
          if (hermiteorder == 4) allocate(aa(neq),bb(neq),cc(neq),dd(neq),ee(neq),ffalpha(neq))
          !allocate(ycrossing(neq),precrossing(neq),postcrossing(neq),yehyb(neq))
       end if

       if (mode < 2) then
          call Rk4(lambda,neq,RHS,y,tin,tout,fixedsteps)
       else
          vareqinteg = 1
          call Rk4(lambda,neq,RHS2,y,tin,tout,fixedsteps)
       end if

       if (controlmode == 1) then
          deallocate(yy0,yy1,ff0,ff1)
          if (hermiteorder == 4) deallocate(aa,bb,cc,dd,ee,ffalpha)
          !deallocate(ycrossing,precrossing,postcrossing,yehyb)
       end if


    case (5)
       if (mode == 2) then
          write(0,*) 'ERROR: Integrate >>> Full Variational System integration for IND not compatible with integmode...',integmode
          stop
       end if
       !Runge Kutta Fehlberg 4-5 (rkf45)
       lw = 3+6*neq
       liw = 5
       allocate(work(lw), iwork(liw))
       ifail = 1
       iout = outmode
       call rkf45(lambda,RHS,neq,y,tin,tout,rtol,atol,ifail,work,iwork,iout,Solout)
       !NB: initializations at 0 are in Subroutine Sol
       if (outmode == 1) rhscalls = rhscalls + iwork(1)

       if (ifail /= 2) then
          write(outputfid,*) 'WARNING: Integrate >>> Rkf45 returns ',ifail
       end if


    case (6)
       if (mode == 2) then
          write(0,*) 'ERROR: Integrate >>> Full Variational System integration for IND not compatible with integmode...',integmode
          stop
       end if
       !Dormand Prince 8th order (dop853)
       itol = 0
       lw = 11*neq + 8*neq + 21
       liw = neq + 21
       allocate(work(lw), iwork(liw))
       work(1:20) = 0d0
       iwork(1:20) = 0
       iwork(1) = maxsteps
       ifail = 0
       iwork(5)=neq
       iout=2
       call dop853(neq,RHS,tin,y,tout,rtol,atol,itol,Solout,iout,work,lw,iwork,liw,lambda,ipar,ifail)
       !NB: initializations at 0 are in Subroutine Sol
       if (outmode == 1) then
          rhscalls = rhscalls + iwork(17)
          nbsteps = nbsteps + iwork(18)
          accsteps = accsteps + iwork(19)
          rejsteps = rejsteps + iwork(20)
       end if


       if (ifail /= 1) then
          if (shootverbose > 0) write(outputfid,*) 'WARNING: Integrate >>> dop853 returns ',ifail
       end if
       deallocate(work,iwork)


    case (7)
       if (mode == 2) then
          write(0,*) 'ERROR: Integrate >>> Full Variational System integration for IND not compatible with integmode...',integmode
          stop
       end if
       !Bulirsch Gragg Stoer extrapolation (odex)
       itol = 0
       iout = outmode
       km = 9
       lw = neq*(km+5) + 5*km +20 + (2*km*(km+2)+5)*neq
       liw = neq + 21 + 2*km
       allocate(work(lw), iwork(liw))
       work(1:20) = 0d0
       iwork(1:20) = 0
       iwork(1) = maxsteps
       ifail = 0
       iwork(8)=neq
       step = 1d-3
       call odex(neq,RHS,tin,y,tout,step,rtol,atol,itol,Solout,iout,work,lw,iwork,liw,lambda,ipar,ifail)
       !NB: initializations at 0 are in Subroutine Sol
       if (outmode == 1) then
          rhscalls = rhscalls + iwork(17)
          nbsteps = nbsteps + iwork(18)
          accsteps = accsteps + iwork(19)
          rejsteps = rejsteps + iwork(20)
       end if

       if (ifail /= 1) then
          if (shootverbose > 0) write(outputfid,*) 'WARNING: Integrate >>> odex returns ',ifail
       end if
       deallocate(work,iwork)


    case (8)
       !Dormand Prince 5th order (dopri5)
       itol = 0
       lw = 8*neq + 5*neq + 21
       liw = neq + 21
       allocate(work(lw), iwork(liw))
       work(1:20) = 0d0
       iwork(1:20) = 0
       iwork(1) = maxsteps
       !disable output messages for file output
       if (outmode /= 0) iwork(3) = -1
       ifail = 0
       iwork(5)=neq !restreindre le dense output a xpdim ??
       iout=2
       !if (discshoot == 1) allocate(ycrossing(neq),precrossing(neq),postcrossing(neq),yehyb(neq))

       !use vector tolerances to force step control to use only y
       allocate(atolv(neq),rtolv(neq))
       atolv = 1d0
       rtolv = 1d0
       atolv(1:xpdim) = atol
       rtolv(1:xpdim) = rtol
       itol = 1

       if (mode < 2) then   
          call dopri5(neq,RHS,tin,y,tout,rtolv,atolv,itol,Solout,iout,work,lw,iwork,liw,lambda,ipar,ifail)
       else
          vareqinteg = 1
          call dopri5(neq,RHS2,tin,y,tout,rtolv,atolv,itol,Solout,iout,work,lw,iwork,liw,lambda,ipar,ifail)
       end if

       !NB: initializations at 0 are in Subroutine Sol
       if (outmode == 1) then
          rhscalls = rhscalls + iwork(17)
          nbsteps = nbsteps + iwork(18)
          accsteps = accsteps + iwork(19)
          rejsteps = rejsteps + iwork(20)
       end if

       
       !write(0,*) 'DOPRI5: mode',mode,'rhscalls',rhscalls,'steps',nbsteps,'acc',accsteps,'rej',rejsteps

       if (ifail /= 1) then
          if (shootverbose > 0) write(outputfid,*) 'WARNING: Integrate >>> dopri5 returns ',ifail
       end if
       deallocate(work,iwork,atolv,rtolv)
       !if (discshoot == 1) deallocate(ycrossing,precrossing,postcrossing,yehyb)


    case (9)        
       if (mode == 2) then
          write(0,*) 'ERROR: Integrate >>> Full Variational System integration for IND not compatible with integmode...',integmode
          stop
       end if
       !Radau5 for stiff problems
       lw =  neq * (4*neq + 12) + 20
       liw = 3*neq + 20 
       allocate(work(lw), iwork(liw))
       work(1:20) = 0d0
       iwork(1:20) = 0
       iwork(2) = maxsteps
       iout = 1
       ifail = 0

       !use vector tolerances to force step control to use only y
       allocate(atolv(neq),rtolv(neq))
       atolv = 1d-1
       rtolv = 1d-1
       atolv(1:xpdim) = atol
       rtolv(1:xpdim) = rtol
       itol = 1

       call  radau5(neq,RHS,tin,y,tout,0d0,rtolv,atolv,itol,DummyFun,0,neq,neq,DummyFun,0,neq,neq,&
            &       Solout,iout,work,lw,iwork,liw,lambda,ipar,ifail)

       !NB: initializations at 0 are in Subroutine Sol
       if (outmode == 1) then
          rhscalls = rhscalls + iwork(14)
          nbsteps = nbsteps + iwork(16)
          accsteps = accsteps + iwork(17)
          rejsteps = rejsteps + iwork(18)
       end if

       if (ifail /= 1) then
          if (shootverbose > 0) write(outputfid,*) 'WARNING: Integrate >>> radau5 returns ',ifail
       end if
       deallocate(work,iwork,atolv,rtolv)


    case default
       write(outputfid,*) 'ERROR: Integrate >>> Unknown integrator...',integmode
       stop

    end select


  end Subroutine Integrate


end Module Integration