integrators.f90

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

    icomp(1) = 1
    lcon = neqn+1

    order = 4

    h = ( tout - tin ) / nbsteps
    !save current stepsize
    stepsize = h

    t = tin
    crossed = 0

    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 + 0.5d0 * h

       call f(neqn, t, y, k1, lambda)
       y1 = y + 0.5d0 * h * k1
       call f(neqn, t1, y1, k2, lambda)
       y2 = y + 0.5d0 * h * k2
       call f(neqn, t1, y2, k3, lambda)
       y3 = y + h * k3
       call f(neqn, t + h, y3, k4, lambda)

       y = y + h / 6d0 * (k1 + 2d0 * k2 + 2d0 * k3 + k4)
       t = t + h

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

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

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

          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 Rk4





!!$  !***************************!
!!$  ! Implicit Euler integrator !
!!$  !***************************!
!!$  Subroutine EulerI(lambda,neqn,y,tin,tout,nbsteps)
!!$    implicit none
!!$
!!$    integer, intent(in) :: neqn, nbsteps
!!$    real(kind=8), intent(in) :: lambda
!!$    real(kind=8), intent(inout) :: tin, tout
!!$    real(kind=8), intent(inout), dimension(neqn) :: y
!!$
!!$    integer :: i,j
!!$    real(kind=8) :: t,h,t1
!!$    real(kind=8), dimension(neqn) :: k,k1,phi1,z1
!!$
!!$
!!$    h = ( tout - tin ) / nbsteps
!!$    t = tin
!!$
!!$    do i = 1, nbsteps 
!!$
!!$       !Initial value
!!$       call RHS(neqn, t, y, k, lambda)
!!$       t1 = t + h
!!$       z1 = h * k
!!$
!!$       !Fixed point iterations
!!$       fpiter = 0
!!$       do j = 1, 10
!!$          call RHS(neqn, t1, y + z1, k1, lambda)
!!$          z1 = h * k1
!!$          fpiter = fpiter + 1
!!$       end do
!!$
!!$       !Integration
!!$       y = y + h * k1
!!$       t = t + h
!!$       totalfpiter = totalfpiter + fpiter
!!$       totalfpcount = totalfpcount + 1
!!$
!!$       if (outmode == 1) call Solout2(t,y,neqn,lambda)
!!$
!!$    end do
!!$
!!$    tin = t
!!$
!!$  end Subroutine EulerI



  !!************************************************
  !!* Symplectic Euler integrator (partitioned RK) *
  !!************************************************
  Subroutine EulerS(lambda,neqn,f,y,tin,tout,nbsteps)
    implicit none

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

    interface
       Subroutine f(dim,time,y,phi,lambda)
         implicit none
         integer, intent(in) :: dim
         real(kind=8), intent(in) :: time, lambda
         real(kind=8), intent(in), dimension(dim) :: y
         real(kind=8), intent(out), dimension(dim) :: phi
       end Subroutine f
    end interface

    !Local
    integer :: i
    real(kind=8) :: h, t, normprogress
    real(kind=8), dimension(neqn) :: k, kl1, z1, newz1, progress
    real(kind=8), dimension(neqn) :: z1prev, z1prev2, z1prev3

    !Partitioned RK: implicit Euler + explicit Euler
    !                  1 | 1             0 | 0
    !                  -----             -----
    !                      1                 1

    h = ( tout - tin ) / nbsteps
    t = tin

    newz1 = 0d0
    call f(neqn, t, y, k, lambda)
    z1 = 0d0
    z1(1:ns) = h * k(1:ns)
    z1prev = z1
    z1prev2 = z1
    z1prev3 = z1

    do i = 1, nbsteps 

       !Initial value
       !call f(neqn, t, y, k, lambda)
       !z1 = 0d0
       !z1(1:ns) = h * k(1:ns)   

       !Initial value: equistage approximation k=2
       !z1 = 2d0 * z1prev - z1prev2
       z1 = 3d0 * z1prev - 3d0 * z1prev2 + z1prev3

       !Fixed point iterations
       fpiter = 0
       normprogress = 1d0
       !do j = 1, 5
       do while (normprogress > fpminprog .and. fpiter < fpmaxiter)
          !NB: ici la formule n'est juste qu'en autonome
          !sinon il faudrait 2 appels de f en t+h et t pour avoir k1 et l1... 
          call f(neqn, t, y + z1, kl1, lambda)

          newz1(1:ns) = h * kl1(1:ns)

          progress(1:neqn) = newz1 - z1         
          normprogress = sqrt(sum(progress*progress)) / neqn

          z1 = newz1
          fpiter = fpiter + 1
       end do

       !write(0,*) fpiter

       ! Integration
       y = y + h * kl1
       t = t + h
       z1prev3 = z1prev2
       z1prev2 = z1prev
       z1prev = z1
       totalfpiter = totalfpiter + fpiter
       totalfpcount = totalfpcount + 1

       if (outmode == 1) call Solout2(t,y,neqn,lambda)

    end do

    tin = t

  end Subroutine EulerS



  !!*******************************************
  !!* Symplectic Implicit Midpoint integrator *
  !!*******************************************
  Subroutine MidpointI(lambda,neqn,f,y,tin,tout,nbsteps)
    implicit none

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

    interface
       Subroutine f(dim,time,y,phi,lambda)
         implicit none
         integer, intent(in) :: dim
         real(kind=8), intent(in) :: time, lambda
         real(kind=8), intent(in), dimension(dim) :: y
         real(kind=8), intent(out), dimension(dim) :: phi
       end Subroutine f
    end interface

    !Local
    integer :: i, crossed, irtrn, order
    real(kind=8) :: h, t, normprogress, told
    real(kind=8), dimension(neqn) :: yold, k, k1, z1, newz1, progress
    !real(kind=8), dimension(neqn) :: z1prev, z1prev2, z1prev3

    !dummy variables for Solout
    integer :: icomp(1), licomp, lcon
    real(kind=8) :: con(neqn+1)
    licomp = 1
    icomp(1) = 1
    lcon = neqn+1

    !Implicit Midpoint rule
    !                         1/2 | 1/2            
    !                         ---------            
    !                             |  1 


    order = 2

    h = ( tout - tin ) / nbsteps
    t = tin
    crossed = 0

    newz1 = 0d0
!!$    !Prepare equistage approximation for initial values
!!$    call f(neqn, t, y, k, lambda)
!!$    z1 = 0d0
!!$    z1 = h / 2d0 * k
!!$    z1prev = z1
!!$    z1prev2 = z1
!!$    z1prev3 = z1

    i = 0
    !do i = 1, nbsteps     
    !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

       !Initial value (basic) 
       call f(neqn, t, y, k, lambda)
       z1 = h * 0.5d0 * k

       !Initial value: equistage approximation k=2
       !z1 = 3d0 * z1prev - 3d0 * z1prev2 + z1prev3

       !Fixed point iterations
       fpiter = 0
       normprogress = 1d0
       do while (normprogress > fpminprog .and. fpiter < fpmaxiter)
          call f(neqn, t + h / 2d0, y + z1, k1, lambda)
          newz1 = h / 2d0 * k1

          progress(1:neqn) = newz1 - z1         
          normprogress = sqrt(sum(progress*progress)) / neqn

          z1 = newz1
          fpiter = fpiter + 1
       end do

       ! Integration
       y = y + h * k1
       t = t + h

!!$       !Equistage approximation update for next step
!!$       z1prev3 = z1prev2
!!$       z1prev2 = z1prev
!!$       z1prev = z1

       totalfpiter = totalfpiter + fpiter
       totalfpcount = totalfpcount + 1

       !Break integration for Switching detection
       if (controlmode == 1) then
          !Prepare Midpoint interpolation
          midy0 = yold
          midf0 = k1

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

          call Solout(i,told,t,y,neqn,con,lcon,ICOMP,LICOMP,lambda,order,irtrn)
          !Switching detected
          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 MidpointI





  !!****************************************************
  !!* Symplectic Stormer-Verlet (2nd order) integrator *
  !!****************************************************
  !NOTE: a reecrire avec un seul point intermediaire cf Geometric Num. Integ. 
  Subroutine StormerVerlet(lambda,neqn,f,y,tin,tout,nbsteps)
    implicit none

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

    interface
       Subroutine f(dim,time,y,phi,lambda)
         implicit none
         integer, intent(in) :: dim
         real(kind=8), intent(in) :: time, lambda
         real(kind=8), intent(in), dimension(dim) :: y
         real(kind=8), intent(out), dimension(dim) :: phi
       end Subroutine f
    end interface

    !Local
    integer :: i
    real(kind=8) :: h, t, normprogress
    real(kind=8), dimension(neqn) :: kl1, kl2, z1, z2, newz1, newz2, k    
    real(kind=8), dimension(neqn) :: z1prev, z1prev2, z1prev3, z2prev, z2prev2, z2prev3
    real(kind=8), dimension(2*neqn) :: progress

    !Partitioned RK:
    ! 0 |  0    0         1/2 | 1/2   0
    ! 1 | 1/2  1/2    +   1/2 | 1/2   0
    ! -------------       --------------
    !   | 1/2  1/2            | 1/2  1/2


    h = ( tout - tin ) / nbsteps
    t = tin

    z1 = 0d0
    z2 = 0d0
    newz1 = 0d0
    newz2 = 0d0
    call f(neqn, t, y, k, lambda)
    z1(ns+1:ns+nc) = h / 2d0 * k(ns+1:ns+nc)
    z2(1:ns) = h * k(1:ns)
    z2(ns+1:ns+nc) = h / 2d0 * k(ns+1:ns+nc)

    z1prev = z1
    z1prev2 = z1
    z1prev3 = z1
    z2prev = z2
    z2prev2 = z2
    z2prev3 = z2

    do i = 1, nbsteps 

       !Initial value
       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)
          !NB: ici la formule n'est juste qu'en autonome
          !sinon il faudrait 4 appels de f en t, t+h et t+h/2 pour avoir k1,k2 et l1,l2... 
          call f(neqn, t, y + z1, kl1, lambda)  
          call f(neqn, t, y + z2, kl2, lambda) 

          newz1(ns+1:ns+nc) = h / 2d0 * kl1(ns+1:ns+nc)
          newz2(1:ns) = h / 2d0 * (kl1(1:ns) + kl2(1:ns))
          newz2(ns+1:ns+nc) = h / 2d0 * kl1(ns+1:ns+nc)

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

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

       !Integration
       y = y + h / 2d0 * (kl1 + kl2)
       t = t + h

       z1prev3 = z1prev2
       z1prev2 = z1prev
       z1prev = z1
       z2prev3 = z2prev2
       z2prev2 = z2prev
       z2prev = z2

       totalfpiter = totalfpiter + fpiter
       totalfpcount = totalfpcount + 1

       if (outmode == 1) call Solout2(t,y,neqn,lambda)

    end do

    tin = t

  end Subroutine StormerVerlet



  !!***************************************************
  !!* Symplectic Gauss 2-stage (4th order) integrator *
  !!***************************************************
  Subroutine Gauss2(lambda,neqn,f,y,tin,tout,nbsteps)
    implicit none

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

    interface
       Subroutine f(dim,time,y,phi,lambda)
         implicit none
         integer, intent(in) :: dim
         real(kind=8), intent(in) :: time, lambda
         real(kind=8), intent(in), dimension(dim) :: y
         real(kind=8), intent(out), dimension(dim) :: phi
       end Subroutine f
    end interface

    !Local
    integer :: i, irtrn, crossed, order
    real(kind=8) :: h, t, told, t1, t2, normprogress
    real(kind=8), dimension(neqn) :: yold, k, k1, k2, z1, z2, newz1, newz2
    !real(kind=8), dimension(neqn) :: z1prev, z1prev2, z1prev3, z2prev, z2prev2, z2prev3
    real(kind=8), dimension(2*neqn) :: progress 
    real(kind=8), parameter :: c1 = 0.5d0 - sqrt(3d0) / 6d0, c2 = 0.5d0 + sqrt(3d0) / 6d0
    real(kind=8), parameter :: a11 = 0.25d0, a12 = 0.25d0 - sqrt(3d0) / 6d0
    real(kind=8), parameter :: a21 = 0.25d0 + sqrt(3d0) / 6d0, a22 = 0.25d0

    !dummy variables for Solout
    integer :: icomp(1), licomp, lcon
    real(kind=8) :: con(neqn+1), phi(xpdim)
    licomp = 1
    icomp(1) = 1
    lcon = neqn+1

    order = 4

    h = ( tout - tin ) / nbsteps