rhs.f90

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

!!****************************************
!!* Simplicial package v1.4              *
!!* Module for IVP right hand side       *
!!* Author: Pierre Martinon              *
!!* ENSEEIHT-IRIT, UMR CNRS 5505         *
!!* CMAP-INRIA FUTURS, UMR CNRS 7641     *
!!* 12/2006                              *
!!****************************************

Module RHSmod
  use shootdefs
  use SpecFuns
  implicit none

  integer, save :: switchcount, truerhs
  integer, dimension(:), allocatable, save :: swdetect 
  real(kind=8), save :: timeswitch

  real(kind=8), dimension(:), allocatable, save :: initdydt0
  real(kind=8), dimension(:,:), allocatable, save :: jac2, tempjac2

  !data for midpoint dense output
  real(kind=8), dimension(:), allocatable, save :: midy0, midf0
  !coefficients for Gauss2 collocation polynom
  real(kind=8), dimension(:), allocatable, save :: ag2, bg2, cg2
  !coefficients for RK4 dense output
  real(kind=8), dimension(:), allocatable, save :: kk1, kk2, kk3, kk4, yyold
  !data for Hermite interpolation
  real(kind=8), dimension(:), allocatable, save :: yy0, yy1, ff0, ff1
  !coefficients for bootstrapping method for dense output
  real(kind=8), dimension(:), allocatable, save :: aa, bb, cc, dd, ee, ffalpha
  integer, save :: hermiteorder  

  real(kind=8), dimension(:), allocatable, save :: utry

contains

  !!**********************************
  !!* IVP Right Hand Side evaluation *
  !!**********************************
  Subroutine RHS(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


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

    !out init
    phi = 0d0

    truerhs = truerhs + 1
    currenttime = time
    currentpoint = y(1:ns+nc)

    !Pre processing
    call PreProcess(time,y,x,p)

    !Control (and switching function)
    call ControlSwitch(lambda,x,p,u,psi)

    !Dynamics
    call Dynamics(dim,lambda,x,p,u,phi)

    !Post processing
    call PostProcess(dim,phi)

  end subroutine RHS



  Subroutine ControlSwitch(lambda,x,p,u,psi)
    implicit none
    real(kind=8), intent(in) ::  lambda
    real(kind=8), intent(in), dimension(ns) :: x
    real(kind=8), intent(in), dimension(nc) :: p
    real(kind=8), intent(out), dimension(m) :: u
    real(kind=8), intent(out), dimension(dimpsi) :: psi


    !out init
    u = 0d0
    psi = 0d0

    !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)
    select case (controlmode)
    case (-1)
       u = utry
    case (0:2,4)
       call Control(lambda,x,p,u)
    case (3)
       call AlternateControl(lambda,x,p,u)
    case default 
       write(0,*) 'ERROR: Control2 >>> Unknown controlmode...',controlmode
       stop
    end select

    !compute switching function
    call Switch(lambda,x,p,psi(1))
    if (dimpsi > 1) call Switchdot(lambda,x,p,u,psi(2))

    !if (controlmode /= 0 .and. outmode*shootdebug > 0) write(0,*) 'Controlmode,u,psi',controlmode,u,psi

  end Subroutine ControlSwitch



  !!*********************************************************
  !!* Variational System Right Hand Side evaluation for IND *
  !!*********************************************************
  Subroutine RHS2(dim,time,yy,phi,lambda)
    implicit none
    integer, intent(in) :: dim
    real(kind=8), intent(in) :: time, lambda
    real(kind=8), intent(in), dimension(dim) :: yy
    real(kind=8), intent(out), dimension(dim) :: phi

    !local declarations
    integer :: j, vardim
    real(kind=8) :: h
    real(kind=8) :: y0(xpdim), y1(xpdim), phi0(xpdim), phi1(xpdim)
    real(kind=8) :: dydy(xpdim,n), yyprime(xpdim,n)

    !out init
    phi = 0d0

    !Usual derivatives phi for y
    y0 = yy(1:xpdim)
    call RHS(xpdim,time,y0,phi0,lambda)
    phi(1:xpdim) = phi0

    !set unknown dimension (n)
    vardim = n !et non nz

    do j = 1, vardim
       dydy(1:xpdim,j) = yy(xpdim + (j-1)*xpdim + 1 : xpdim + j*xpdim)
    end do


    select case (vareqmode)

    case (1)
       !!First method: finite differences (cf Hairer, Solving ODEs I, p201)

       do j = 1, vardim
          !h = sqrt(macheps*(1d0+abs(y0(free0(j))))) pb: faudrait le z ici en fait au lieu de y0
          !h = sqrt(macheps)
          h = 1d-6
          y1 = y0 + h * dydy(1:xpdim,j)
          call RHS(xpdim,time,y1,phi1,lambda)
          yyprime(1:xpdim,j) = (phi1 - phi0) / h
       end do

       !Store result for rhs (by column after usual y')
       do j = 1, vardim
          phi(xpdim + (j-1)*xpdim + 1 : xpdim + j*xpdim ) = yyprime(1:xpdim,j)
       end do

    case default
       write(0,*) 'ERROR: RHS2 >>> Unknown vareqmode...',vareqmode
       stop
    end select !vareqmode


  end Subroutine RHS2




  Subroutine AlternateControl(lambda,x,p,u)
    implicit none

    real(kind=8), intent(in) ::  lambda                   
    real(kind=8), intent(in), dimension(ns) :: x         
    real(kind=8), intent(in), dimension(nc) :: p        
    real(kind=8), intent(out), dimension(m) :: u         

    !Local
    integer :: fail, continuous
    real(kind=8) :: alpha, newalpha
    
    !out init
    u = 0d0

    !use control continuity for init   *** SEMBLE IMPORTANT ***
    continuous = 1

    !initialize alternate control
    call InitAltControl(lambda,alpha,x,p,u)
    
    !compute alternate control
    call ControlCorrection(lambda,alpha,newalpha,fail)
    !xwrite(0,*) newalpha

    !rebuild alternate control
    call UpdateAltControl(lambda,newalpha,x,p,u)

    !save alpha for next step init (do not disable for outmode = 1 !)
    if (continuous * update == 1) prevalpha = newalpha

  end Subroutine AlternateControl
 


  !*************************************
  ! Control Correction on singular arc *
  !*************************************
  Subroutine ControlCorrection(lambda,alpha,newalpha,fail)
    implicit none
    integer, intent(inout) :: fail
    real(kind=8), intent(in) :: lambda,alpha
    real(kind=8), intent(out) :: newalpha
    
    !Local
    integer :: opt
    real(kind=8) :: d0, d, x(1), g(1)
    real(kind=8) :: t, y(ns+nc)

    !tnbc
    integer :: ierror, lw, ipivot(1)
    real(kind=8) :: w(14)

    !lbfgs
    character(len=60) :: csave, task
    logical :: lsave(4)
    integer :: dimn, dimm, iprint, nbd(1), iwa(3), isave(44) !ici n=m=1
    real(kind=8) :: factr, pgtol, dsave(29), wa(2*1*1+4*1+12*1*1+12*1) !n=m=1
    

    !out init
    newalpha = 0d0

    !opt: optimization routine choice
    !1: tnbc (truncated newton)       
    !2: lbfgs (limited memory bfgs) *** SEMBLE BUGGUER *** 
    opt = 1

    d = 666d0

    fail = 0
    !NB. set in RHS
    t = currenttime
    y = currentpoint
    !NB. stepsize is set in Solout or fixed step integrator
    !for first integration step stepsize is set to 0 by solout
    !reset it to correct initial value 1d-6
    if (stepsize == 0d0) stepsize = 1d-6

    !initial drift
    newalpha = alpha
    call Drift(lambda,t,stepsize,newalpha,y,d0)
    d = d0
    !write(0,*) 'intial drift for alpha,y=',alpha,y,':',d

    !find alpha that minimizes drift norm
    x(1) = newalpha
    
    select case (opt)

    case (1)
       !tnbc
       ierror = 0
       dimn = 1
       lw = 14  
       !write(0,*) 'Call tnbc',x,d
       call TNBC(ierror, dimn, x(1:dimn), d, g(1:dimn), w, lw, DriftFun, lower(1:dimn), upper(1:dimn), ipivot)
       !write(0,*) 'Exit tnbc',x,d,lower,upper


    case (2)
       !lbfgs
       dimn = 1
       dimm = 1
       iprint = -1
       factr=1.0d+7
       pgtol=1.0d-5
       nbd(1) = 2
       task = 'START'
       call setulb(dimn, dimm, x(1:dimn), lower(1:dimn), upper(1:dimn), nbd(1:dimn), d, g(1:dimn),&
            & factr, pgtol, wa, iwa, task, iprint, csave, lsave, isave, dsave)
         
       do while (task(1:2) .eq. 'FG' .or. task(1:5) .eq. 'NEW_X')
          if (task(1:2) .eq. 'FG') then
             call DriftFun(dimn,x(1:dimn),d,g(1:dimn))
          end if       
          call setulb(dimn, dimm, x(1:dimn), lower(1:dimn), upper(1:dimn), nbd(1:dimn), d, g(1:dimn),&
               & factr, pgtol, wa, iwa, task, iprint, csave, lsave, isave, dsave)  
       end do


    case default
       write(0,*) 'ERROR: ControlCorrection >>> Unknown value for opt...',opt
       stop

    end select

    !check drift for computed control
    newalpha = x(1)
    if (newalpha .gt. upper(1)) then
       if (shootdebug > 0) write(0,*) 'WARNING: ControlCorrection2 >>> Newalpha out of bounds...',lower(1),newalpha,upper(1)
       newalpha = upper(1)
    elseif (newalpha .lt. lower(1)) then
       if (shootdebug > 0) write(0,*) 'WARNING: ControlCorrection2 >>> Newalpha out of bounds...',lower(1),newalpha,upper(1)
       newalpha = lower(1)
    end if



  end Subroutine ControlCorrection


 !************************
  ! Drift on singular arc *
  !************************
  Subroutine Drift(lambda,t,h,alpha,y,d)
    implicit none
    real(kind=8), intent(in) :: lambda, t, h, alpha, y(ns+nc)
    real(kind=8), intent(out) ::d !drift

    !Local
    integer :: controlmodebackup
    real(kind=8) :: tin, psi, psidot
    real(kind=8) :: k1(ns+nc), ynext(ns+nc), x(ns), p(nc)

    !out init
    d = 0d0

    !drift mode
    !0: minimize H**2 at current step > BAAAAH -_-
    !1: minimize psi and psi' at next (Euler) step > LE MIEUX APPAREMMENT
    !2: minimize psi and psi' at next (Rk4) step


    !Build utry from alpha
    call PreProcess(t,y,x,p)
    call UpdateAltControl(lambda,alpha,x,p,utry)
  
    !switch to the use of control utry instead of computing u
    controlmodebackup = controlmode
    controlmode = -1

    !Perform Euler basic step (h was set externally)
    if (h <= 0d0) then
       write(0,*) 'ERROR: Drift >>> Something wrong with integration stepsize...',h
       stop
    end if

    tin = t
    ynext = y
    call RHS(ns+nc, tin, ynext, k1, lambda)
    ynext = ynext + h * k1
    tin = tin + h
    
    !compute drift          
    !singularcontrolmode
    !0: use exact singular control in Control    >>> you should not be here ;-)
    !1: use alternate singular control minimizing psi^2
    !2: use alternate singular control minimizing psi^2 + psi'^2

    call PreProcess(tin,ynext,x,p)
    call Switch(lambda,x,p,psi)

    select case (singularcontrolmode)

    case (1)
       d = psi**2 

    case (2)
       call Switchdot(lambda,x,p,utry,psidot)
       d = psi**2 + psidot**2

    case default
       write(0,*) 'ERROR: Drift >>> Unknown singularcontrolmode...',singularcontrolmode
       stop
    end select

    !back to singular control computation mode
    controlmode = controlmodebackup

  end Subroutine Drift



  !***************************
  ! Drift value and gradient *
  ! for TNBC and lbfgs       *
  !***************************
  Subroutine DriftFun(n,x,f,g)
    implicit none
    integer, intent(in) :: n
    real(kind=8), intent(inout) :: f
    real(kind=8), intent(inout), dimension(n) :: x, g

    !Local
    real(kind=8) :: t, h, f1, x1
    real(kind=8) :: y(ns+nc)

    !set in RHS
    t = currenttime
    y = currentpoint
   
    !write(0,*) 'DriftFun x,y',x,y

    !stepsize is set in Solout or fixed step integrator
    call Drift(lambda,t,stepsize,x(1),y,f)

    !gradient: finite differences
    h = 1d-6

    if (x(1) <= upper(1)-h) then
       !forward difference
       x1 = x(1) + h
       call Drift(lambda,t,stepsize,x1,y,f1)
       g = (f1 - f) / h
    else    
       !OK, on valide ca (ca a l'air d'amelio la CV dans certains cas...)
       !backward difference
       x1 = x(1) - h
       call Drift(lambda,t,stepsize,x1,y,f1)
       g = (f - f1) / h
    end if

    !if (shootdebug > 0) write(0,*) 'DriftFun f g',f,g


  end Subroutine DriftFun




  !!***********************************
  !!* Switching condition for control *
  !!***********************************
  Subroutine SwitchCond(dim,time,y,lambda,val)
    implicit none
    integer, intent(in) :: dim
    real(kind=8), intent(in) :: time, lambda
    real(kind=8), dimension(dim), intent(in) :: y
    real(kind=8), intent(out) :: val

    !Local
    integer :: aux, aux2
    real(kind=8) :: h1, h2
    real(kind=8) :: phi(dim), x(ns), p(nc)