c COPYRIGHT c William W. Hager c 1999 c This code solves the orbit transfer problem found in the paper c ``Runge-Kutta Methods in Optimal Control and c the Transformed Adjoint System'' c The solution is obtained using the subroutine ``optcon_xrk'' for c solving unconstrained control problems using explicit Runge-Kutta c discretizations. The terminal constraints in the control problem c are handled using penalty/multiplier techniques. The number of c mesh intervals used in the discretization is n. The integration c scheme is that given by c - - - - c | 0 0 0 | | 1/6 | c A = | .5 0 0 | , b = | 2/3 | c | -1 2 0 | | 1/6 | c - - - - parameter (nwork = 100000, nc = 1, ns = 3, nx = 3, n = 500 ) implicit real*8 (a-h,o-z) real*8 work(nwork) real*8 control(nc,ns,n) real*8 final(nx), state(nx,ns,n), state0(nx) real*8 costate(nx,ns,n) real*8 a(ns,ns), b(ns) real*8 r0, u0, v0, t0, tf real*8 m0, mdot, mu, t, lam1, lam2, p common m0, mdot, mu, t, lam1, lam2, p common /gp/step, scut, sigma, beta, ngp, ngrow, tolgp common /cg/ncg, nri, tolcg data a/0.d0, .5d0, -1.d0, 0.d0, 0.d0, 2.d0, 0.d0, 0.d0, 0.d0/ b(1) = 1.d0/6.d0 b(2) = 2.d0/3.d0 b(3) = 1.d0/6.d0 c gravitational constant for Sun (meters^3/sec^2) mu = 1.327330000000000e+20 t0 = 0 c final time in seconds (193 days) tf = 193.d0*24.d0*3600.d0 c fuel consumption of 12.9 kg/day, or 12.9/(24*3600) kg/sec mdot = 12.9d0/(24.d0*3600.d0) c thrust of .85d0*9.807 Newtons t = .85d0*9.807 c initial weight of space craft 10000 kg m0 = 10000 c initial radius 149.6e9 meters (distance from Earth to Sun) r0 = 149.6e9 c initial radial component of velocity u0 = 0 c initial tangential component of velocity, assuming fixed orbit v0 = dsqrt(mu/r0) state0(1) = r0 state0(2) = u0 state0(3) = v0 pi2 = .5*dacos(-1.d0) c Starting guess is to point rocket to Sun for the first half of the c flight and then point the rocket away from the Sun for the c second half of the flight m = n/2 do 10 k = 1, m do 10 j = 1, ns do 10 i = 1, nc control(i,j,k) = pi2 control(i,j,k+m) = -pi2 10 continue m1 = m - n/20 m2 = m + n/20 c Set the thrust angle to zero in the transition region where the c rocket switches direction do 20 k = m1, m2 do 20 j = 1, ns do 20 i = 1, nc control(i,j,k) = 0 20 continue c Penalty factor associated with the quadratic penalty term added to c cost function p = 10000 c We first solved the problem on the coarse mesh to get an estimate c for the multipliers associated with the terminal constraints lam1 = 7155756.1019616 lam2 = -6348152.2785148 c Parameters for gradient projection method step = .00000001 scut = 10000 sigma = .5 beta = .5 ngrow = 3 ngp = n tolgp = 1.e-2 c Parameters for conjugate gradient method ncg = 5*n nri = n tolcg = 1.e-5 olderr = 1.e50 do 30 it = 1, 100000 call optcon_xrk ( control, final, state, state0, costate, 1 n, nx, nc, t0, tf, a, b, ns, work, 2 gperror, cgerror, itgp, itcg ) write(6,*) 'final(1):',final(1) write(6,*) 'final(2):',final(2) write(6,*) 'final(3):',final(3) write(6,*) 'error in bc 1:',dabs(final(2)) write(6,*) 'error in bc 2:',dabs(final(3)-dsqrt(mu/final(1))) error = dabs(final(2)) + dabs(final(3)-dsqrt(mu/final(1))) write(6,*) 'gp iterations:',itgp write(6,*) 'cg iterations:',itcg c Update the multipliers using the first-order formula lam1 = lam1 + 2*p*final(2) lam2 = lam2 + 2*p*(final(3)-dsqrt(mu/final(1))) write(6,*) 'lam1',lam1 write(6,*) 'lam2',lam2 write(6,*) c If the error in the terminal boundary conditions increases, then stop if ( error .ge. olderr ) goto 40 olderr = error 30 continue 40 do 60 k = 1, n do 60 j = 1, ns do 60 i = 1, nc write(6,50) i, j, k, control(i,j,k) 50 format(3i5,e25.16) 60 continue stop end c ===================================== c User supplied functions c evaluate the right side of the differential equation at given c x, u, and time, storing the result in the output vector f subroutine de (f, x, c, time) implicit real*8 (a-h,o-z) real*8 x(*), f(*), c real*8 m0, mdot, mu, t, lam1, lam2, p common m0, mdot, mu, t, lam1, lam2, p r = x(1) u = x(2) v = x(3) d = m0 - mdot*time f(1) = u f(2) = (v**2 - mu/r)/r + t*dsin(c)/d f(3) = -u*v/r + t*dcos(c)/d return end c evaluate the gradient with respect to x of f subroutine xgrad(g, x, c, time ) implicit real*8 (a-h,o-z) real*8 g(*), x(*), c, time real*8 m0, mdot, mu, t, lam1, lam2, p common m0, mdot, mu, t, lam1, lam2, p r = x(1) u = x(2) v = x(3) g(1) = 0 g(2) = (2*mu/r - v*v)/(r*r) g(3) = u*v/(r*r) g(4) = 1 g(5) = 0 g(6) = -v/r g(7) = 0 g(8) = 2*v/r g(9) = -u/r return end c evaluate the gradient with respect to the control of f subroutine ugrad(g, x, c, time) implicit real*8 (a-h,o-z) real*8 g(*), x(*), c real*8 m0, mdot, mu, t, lam1, lam2, p common m0, mdot, mu, t, lam1, lam2, p d = m0 - mdot*time g(1) = 0 g(2) = t*dcos(c)/d g(3) = -t*dsin(c)/d return end c evaluate terminal cost function, including the penalty/multiplier term double precision function tcost( x ) implicit real*8 (a-h,o-z) real*8 x(*), s, tcost real*8 m0, mdot, mu, t, lam1, lam2, p common m0, mdot, mu, t, lam1, lam2, p r = x(1) u = x(2) v = x(3) s = v - dsqrt(mu/r) tcost = -r + p*(u*u + s*s) + lam1*u + lam2*s return end c evaluate gradient of cost subroutine gcost (g, x) implicit real*8 (a-h,o-z) real*8 g(*), x(*), s real*8 m0, mdot, mu, t, lam1, lam2, p common m0, mdot, mu, t, lam1, lam2, p r = x(1) u = x(2) v = x(3) s = v - dsqrt(mu/r) g(1) = -1 + (p*s + .5*lam2)*dsqrt(mu/(r**3)) g(2) = 2*p*u + lam1 g(3) = 2*p*s + lam2 return end c ===================================== c Routines in optcon_xrk c COPYRIGHT c William W. Hager c 1999 c Solve an unconstrained control problem of the form c c min C(x(t_f)) c . c subject to x = f(x,u), x(t_0) = x_0 c The continuous problem is discretized by an eXplicit Runge-Kutta c scheme of the form c x_{k+1} = x_k + h \sum_{i=1}^s b_i f(y_i,u_{ki}) c c y_i = x_k + h \sum_{j=1}^s a_{ij} f(y_j,u_{kj}) c The optimization is performed using the gradient method with an C Armijo type step rule and then the conjugate gradient method with c Polak-Ribiere update of the search direction. All computations c use double precision. c Input/Output arguments: c n - number of time levels in the discretization c ns - number of stages (s) in the Runge-Kutta scheme c nx - number of state variables c nc - number of controls c t0 - start time c tf - final time c a - array containing the Runge-Kutta coefficients (by columns) c b - array containing the coefficients of the final stage c state0 - array containing state variable (x_0) at start time t_0 c state - nx by ns by n array containing the state variables c final - array containing state variables at final time c costate - nx by ns by n array containing the costate variables c control - nc by ns by n array containing the controls c work - work array of length at least c 4*nc*ns*n + nx*nx + nc*nx + 2ns*ns + 2*ns + 2*nx c c control - array of size nc by ns by n c gperror - error after termination of gradient scheme c gpit - number of gradient steps that were performed c cgerror - error after termination of conjugate gradient scheme c cgit - number of conjugate gradient steps that were performed c The parameters for the gradient method and for the c conjugate gradient method should be placed in two common blocks: c common /gp/step, scut, sigma, beta, ngp, ngrow, tolgp c common /cg/ncg, nri, tolcg c The variables in these blocks have the following meanings: c step - starting step size c scut - maximum allowed step size c sigma - parameter in Armijo size rule (typically sigma = .5) c beta - if step too big, successively multiply it by beta c if step too small, successively divide it by beta c ngp - maximum number of gradient steps c ngrow - maximum number of divides by beta in each step c tolgp - terminate when gradient infinity norm <= this value c ncg - maximum number of conjugate gradient steps c nri - number iterations until reinitialization c tolcg - terminate when gradient infinity norm <= this value c The user must provide the following routines c Evaluation of terminal cost: c double precision function tcost( x ) c Evaluation of the gradient of the terminal cost: c subroutine gcost (g, x) c g - array containing gradient at x c x - state variables at final time c Evaluation of f: c subroutine de (f, x, c, t) c x - array containing the current state variables c c - array containing the current controls c t - current time c Evaluation of grad_x f: c subroutine xgrad(g, x, c, t ) c g - array containing the gradient with respect to x c x - array containing the current state variables c c - array containing the current controls c t - current time c Evaluation of grad_u f: c subroutine ugrad(g, x, c, t) c g - array containing the gradient with respect to u c x - array containing the current state variables c c - array containing the current controls c t - current time c Note: This version of the code uses the following rule for c choosing how to evaluate the difference in two function c values: Let v_i be the value of the cost function corresponding c to control c_i, i = 1, 2, and let d be the cost gradient c at c_0 times the control difference c_1 - c_0. c if ( dabs(d) .ge. 1.e-12*dabs(v0) ), then v_1 - v_0 c is gotten by simply subtracting the two values. c Otherwise, v_1 - v_0 is estimated by d. This approximation c d to the change in values allows us to obtain high c accuracy solutions to the discrete problem since c v1 - v0 is always zero near a local minimum of the cost. c On the other hand, if c_1 - c_0 points along the c gradient of the cost, then d only vanishes when the c cost gradient vanishes. c subroutine optcon_xrk ( control, final, state, state0, costate, 1 n, nx, nc, t0, tf, a, b, ns, work, 2 gperror, cgerror, itgp, itcg ) implicit real*8 (a-h, o-z) external valf, gradf real*8 control(*) real*8 final(*), state(*), state0(*) real*8 costate(*) real*8 work(*), h, t0, tf common /alg/ h, ti, n1, nx1, nc1, ns1, loc_gx, loc_gu, loc_d, & loc_d1, loc_p, loc_q, loc_y, loc_z, loc_a, loc_b, loc_c, loc_t common /cg/ncg, nri, tolcg ti = t0 n1 = n nx1 = nx nc1 = nc ns1 = ns call init ( a, b, ns, tf, work) m = nc*ns*n call gradproj( gperror, itgp, stepf, control, final, state, 1 state0, costate, work(loc_d), work(loc_d1), work(loc_p), 2 work(loc_q), m, work ) call conjgrad ( cgerror, itcg, stepf, control, final, state, 1 state0, costate, work(loc_d), work(loc_d1), work(loc_p), 2 work(loc_q), m, work ) return end subroutine init ( a, b, ns1, tf, work) implicit real*8 (a-h,o-z) real*8 a(*), b(*), work(*), h, t0, tf integer n, ns common /alg/ h, t0, n, nx, nc, ns, loc_gx, loc_gu, loc_d, & loc_d1, loc_p, loc_q, loc_y, loc_z, loc_a, loc_b, loc_c, loc_t loc_d = 1 loc_d1= loc_d + nc*ns*n loc_p = loc_d1 + nc*ns*n loc_q = loc_p + nc*ns*n loc_gx = loc_q + nc*ns*n loc_gu = loc_gx + nx*nx loc_a = loc_gu + nc*nx loc_b = loc_a + ns*ns loc_c = loc_b + ns loc_t = loc_c + ns*ns loc_y = loc_t + ns loc_z = loc_y + nx h = (tf-t0)/n call setup(work(loc_a), a, work(loc_b), b, work(loc_c), & work(loc_t), ns ) return end subroutine setup( a, a0, b, b0, c, tau, ns ) implicit real*8 (a-h,o-z) real*8 a(ns,*), a0(ns,*), b(*), b0(*), c(ns,*), tau(*) do 10 i = 1, ns b(i) = b0(i) 10 continue do 30 i = 1,ns t = 0 if ( b0(i) .eq. 0 ) then write(6,*) 'b(i) vanishes for i =',i,'-- this code only' write(6,*) 'handles the case of all b(i) nonzero.' stop endif do 20 j = 1,ns a(j,i) = a0(j,i) if ( i .ge. j ) then if ( a0(j,i) .ne. 0 ) then write(6,*) 'This code treats explicit Runge-Kutta' write(6,*) 'schemes only. The given scheme is implicit.' stop endif endif t = t + a0(i,j) c(i,j) = b(j)*a(j,i)/b(i) 20 continue tau(i) = t 30 continue return end subroutine gradproj( error, it, s, control, final, state, state0, & costate, g, g1, p, q, n, work ) implicit real*8 (a-h, o-z) real*8 control(*), final(*), state(*), state0(*) real*8 costate(*), g(*), g1(*), p(*), q(*), work(*) common /gp/step, scut, sigma, beta, ngp, ngrow, tol sbar = step do 90 it = 1, ngp call gradf(g, v0, control, final, state, state0, costate, & work ) error = 0 do 20 i = 1, n if ( error .lt. dabs(g(i)) ) error = dabs(g(i)) 20 continue if ( error .le. tol ) return s = sbar/beta ratio = 0 niters = 0 dowhile ( ratio .lt. sigma ) niters = niters + 1 if ( niters .gt. 40 ) then write(6,*) 'gradproj infinite loop' stop endif s = beta*s d = 0 do 30 i = 1, n p(i) = control(i) - s*g(i) d = d - g(i)**2 30 continue d = d*s if ( dabs(d) .ge. 1.e-12*dabs(v0) ) then v1 = valf ( p, final, state, state0, work ) df = v1 - v0 else call gradf(g1, v1, p, final, state, state0, & costate, work ) d1 = 0 do 40 i = 1, n d1 = d1 - g1(i)*g(i) 40 continue d1 = d1*s df = .5*(d+d1) endif ratio = -1 if ( df .le. 0 ) then ratio = df/d endif enddo if ( s .eq. sbar ) then if ( sbar .lt. scut ) then s1 = sbar*(1/beta)**ngrow dowhile ( s .lt. s1 ) s = s/beta d = 0 v2 = v1 do 50 i = 1, n q(i) = p(i) p(i) = control(i) - s*g(i) d = d - g(i)**2 50 continue d = d*s if ( dabs(d) .ge. 1.e-12*dabs(v0) ) then v1 = valf ( p, final, state, state0, & work ) df1 = v1 - v0 else call gradf(g1, v1, p, final, state, & state0, costate, work ) d1 = 0 do 60 i = 1, n d1 = d1 - g1(i)*g(i) 60 continue d1 = d1*s df1 = .5*(d+d1) endif if ( df1 .ge. df ) then do 70 i = 1, n p(i) = q(i) 70 continue s = s*beta v1 = v2 goto 75 else df = df1 endif enddo endif endif 75 v0 = v1 do 80 i = 1, n control(i) = p(i) 80 continue sbar = s 90 continue return end subroutine conjgrad ( error, it, s, control, final, state, state0, 1 costate, d, g, g0, p, n, work ) implicit real*8 (a-h, o-z) real*8 control(*), final(*), state(*), state0(*) real*8 costate(*), d(*), g(*), g0(*), p(*), work(*) common /cg/ncg, nri, tol it = 0 s1 = s dowhile ( it .lt. ncg ) call gradf(g0, v0, control, final, state, state0, & costate, work ) error = 0 do 10 i = 1, n d(i) = -g0(i) if ( dabs(d(i)) .gt. error ) error = dabs(d(i)) 10 continue if ( error .le. tol ) return eold = error vold = v0 do 110 is = 1, nri vbar = v0 io = 0 it = it + 1 if ( it .gt. ncg ) return d0 = 0 s0 = 0 do 20 i = 1, n d0 = d0 + d(i)*g0(i) p(i) = control(i) + s1*d(i) 20 continue dbar = d0 call gradf(g, v1, p, final, state, state0, costate, & work ) d1 = 0 do 30 i = 1, n d1 = d1 + d(i)*g(i) 30 continue if ( dabs(d0*s1) .ge. 1.e-12*dabs(v1) ) then df = v1 - v0 else df = .5*s1*(d0+d1) endif it1 = 0 dowhile ( df .gt. 0 ) s1 = .2d0*s1 do 40 i = 1, n p(i) = control(i) + s1*d(i) 40 continue call gradf(g, v1, p, final, state, state0, & costate, work ) it1 = it1 + 1 if ( it1 .gt. 20 ) then io = 1 goto 120 endif d1 = 0 do 50 i = 1, n d1 = d1 + d(i)*g(i) 50 continue if ( dabs(d0*s1) .ge. 1.e-12*dabs(v1) ) then df = v1 - v0 else df = .5*s1*(d0+d1) endif enddo it2 = 0 dowhile ( d1 .lt. 0 ) it2 = it2 + 1 if ( it2 .gt. 60 ) then io = 2 write(6,*) 'cost function appears unbounded' goto 120 endif s0 = s1 s1 = 2*s1 d0 = d1 do 60 i = 1, n p(i) = control(i) + s1*d(i) 60 continue call gradf(g, v1, p, final, state, state0, & costate, work ) d1 = 0 do 70 i = 1, n d1 = d1 + d(i)*g(i) 70 continue enddo info = 0 df = 0 it3 = 0 dowhile ( df .ge. 0 ) slope = 0 dowhile ( slope .ge. 0 ) if ( info .eq. 0 ) then if ( d1 .gt. 0 ) then s2 = .5*(s1+s0) else s2 = s0 - d0*(s1-s0)/(d1-d0) endif else s2 = s3 info = 0 endif do 80 i = 1, n p(i) = control(i) + s2*d(i) 80 continue call gradf(g, v2, p, final, state, state0, & costate, work ) it3 = it3 + 1 if ( it3 .gt. 30 ) then io = 3 goto 120 endif d2 = 0 g2 = 0 bnum = 0 bden = 0 do 90 i = 1, n d2 = d2 + d(i)*g(i) g2 = g2 + g(i)*g(i) bnum = bnum + (g(i)-g0(i))*g(i) bden = bden + g0(i)*g0(i) 90 continue beta = bnum/bden slope = beta*d2 - g2 if ( d2 .gt. 0 ) then if ( d1 .ne. d2 ) then s3 = s2 - d2*(s1-s2)/(d1-d2) if ( s3 .gt. s0 ) then if ( s3 .lt. s1 ) then info = 1 endif endif endif if ( s1 .eq. s2 ) then io = 4 goto 120 endif s1 = s2 d1 = d2 else if ( d0 .ne. d2 ) then s3 = s2 - d2*(s0-s2)/(d0-d2) if ( s3 .gt. s0 ) then if ( s3 .lt. s1 ) then info = 1 endif endif endif if ( s0 .eq. s2 ) then io = 4 goto 120 endif if ( dabs(dbar*s2) .ge. 1.e-12*dabs(v2) ) & then df = v2 - vbar else df = .5*s2*(dbar+d2) endif if ( df .le. 0 ) then s0 = s2 d0 = d2 else s1 = s2 d1 = d2 info = 0 endif endif enddo if ( dabs(dbar*s2) .ge. 1.e-12*dabs(v2) ) then df = v2 - vbar else df = .5*s2*(dbar+d2) endif enddo s1 = s2 v0 = v2 error = 0 do 100 i = 1, n control(i) = control(i) + s1*d(i) d(i) = -g(i) + beta*d(i) g0(i) = g(i) if ( error .lt. dabs(g(i)) ) error = dabs(g(i)) 100 continue if ( error .le. tol ) return 110 continue 120 if ( io .eq. 2 ) return if ( io .gt. 0 ) then error = 0 do 130 i = 1, n if ( error .lt. dabs(g(i)) ) error = dabs(g(i)) 130 continue endif if ( error .ge. .95*eold ) then write(6,*) write(6,*) 'Although the computing tolerance =',tol write(6,*) 'for conjgrad has not been achieved,' write(6,*) 'the routine is terminating anyway since' write(6,*) 'it appears that this tolerance is not' write(6,*) 'going to be achieved' write(6,*) return endif enddo return end c state(i,j,k) c k = time level c j = intermediate level c i = component at that level subroutine state_eqn (control, final, state, state0, & h, t0, n, nx, nc, ns, y, z, a, b, tau ) implicit real*8 (a-h, o-z) real*8 control(nc,ns,*), final(*), state(nx,ns,*), state0(*) real*8 h, t0, time, y(*), z(*), a(ns,*), b(*), tau(*) do 10 l = 1, nx z(l) = state0(l) 10 continue do 60 k = 1, n time = t0 + h*(k-1) do 20 i = 1,ns do 20 l = 1,nx state(l,i,k) = z(l) 20 continue do 50 j = 1, ns call de(y, state(1,j,k), control(1,j,k), & time + h*tau(j)) do 30 i = j+1, ns do 30 l = 1, nx state(l,i,k) = state(l,i,k) + h*a(i,j)*y(l) 30 continue do 40 l = 1, nx z(l) = z(l) + h*b(j)*y(l) 40 continue 50 continue 60 continue do 70 l = 1, nx final(l) = z(l) 70 continue return end subroutine costate_eqn( control, final, state, costate, & h, t0, n, nx, nc, ns, y, z, b, c, tau, g ) implicit real*8 (a-h,o-z) real*8 control(nc,ns,*), final(*), state(nx,ns,*) real*8 costate(nx,ns,*) real*8 h, t0, s, y(*), z(*), g(nx,*) real*8 b(*), c(ns,*), tau(*) call gcost (z, final ) do 70 k = n, 1, -1 do 10 i = 1,ns do 10 l = 1,nx costate(l,i,k) = z(l) 10 continue time = t0 + h*(k-1) do 60 j = ns, 1, -1 call xgrad(g, state(1,j,k), control(1,j,k), & time + h*tau(j) ) do 30 l = 1, nx s = 0 do 20 m = 1, nx s = s + costate(m,j,k)*g(m,l) 20 continue y(l) = s 30 continue do 40 i = 1, j-1 do 40 l = 1,nx costate(l,i,k) = costate(l,i,k) + h*c(i,j)*y(l) 40 continue do 50 l = 1, nx z(l) = z(l) + h*b(j)*y(l) 50 continue 60 continue 70 continue return end double precision function valf ( control, final, state, state0, & work ) implicit real*8 (a-h,o-z) real*8 control(*), final(*), state(*), state0(*), work(*) real*8 h, t0, valf common /alg/ h, t0, n, nx, nc, ns, loc_gx, loc_gu, loc_d, & loc_d1, loc_p, loc_q, loc_y, loc_z, loc_a, loc_b, loc_c, loc_t call state_eqn (control, final, state, state0, 1 h, t0, n, nx, nc, ns, work(loc_y), work(loc_z), 2 work(loc_a), work(loc_b), work(loc_t) ) valf = tcost( final ) return end subroutine gradf(df, valf, control, final, state, state0, costate, & work ) implicit real*8 (a-h,o-z) real*8 df(*), valf, control(*) real*8 final(*), state(*), state0(*), costate(*), work(*) common /alg/ h, t0, n, nx, nc, ns, loc_gx, loc_gu, loc_d, & loc_d1, loc_p, loc_q, loc_y, loc_z, loc_a, loc_b, loc_c, loc_t call state_eqn (control, final, state, state0, 1 h, t0, n, nx, nc, ns, work(loc_y), work(loc_z), 2 work(loc_a), work(loc_b), work(loc_t) ) valf = tcost( final ) call costate_eqn ( control, final, state, costate, 1 h, t0, n, nx, nc, ns, work(loc_y), work(loc_z), 2 work(loc_b), work(loc_c), work(loc_t), work(loc_gx) ) call combine ( df, control, state, costate, 1 h, t0, n, nx, nc, ns, 2 work(loc_b), work(loc_t), work(loc_gu) ) return end subroutine combine ( df, control, state, costate, & h, t0, n, nx, nc, ns, b, tau, g ) implicit real*8 (a-h, o-z) real*8 df(nc,ns,*), control(nc,ns,*), state(nx,ns,*) real*8 costate(nx,ns,*), h, t0, s, b(*), tau(*), g(nx, *) do 30 k = 1, n time = t0 + h*(k-1) do 20 j = 1,ns call ugrad(g, state(1,j,k), control(1,j,k), & time + h*tau(j) ) do 20 i = 1, nc s = 0 do 10 l = 1,nx s = s + costate(l,j,k)*g(l,i) 10 continue df(i,j,k) = h*b(j)*s 20 continue 30 continue return end