C ALGORITHM 733, COLLECTED ALGORITHMS FROM ACM. C THIS WORK PUBLISHED IN TRANSACTIONS ON MATHEMATICAL SOFTWARE, C VOL. 20, NO. 3, SEPTEMBER, 1994, PP. 262-281. File TOMP.TXT ============= The file TOMP.FOR contains the following groups of subroutines for solving optimal control problems as described in [1]. 1. A SIMULATOR, in which initial-value problems are solved for given parameter values. Cost function and constraint vector together with gradient vector and Jacobi matrix are evaluated. 2. An OPTIMIZER, in which new parameter values are proposed as a result of the SIMULATOR output. 3. A test driver which solves the state-constrained brachistochrone problem. Test results for a couple of other problems can be found in [1]. Only a double precision version exists at the moment. Creating of a single precision version should not be to difficult, as ALL variables are explicitely declared in TYPE statements. All major subroutines are carefully commented The heading comments (parameter description, dimensioning, etc.) are also given in the appendices to [1]. The optimizer makes havy use of some level 1 Blas [2] routines which are included. Also modified versions of the routines HFTI, LDP and NNLS from [3], RKF45 from [4], LEFT from [5], and FMIN from [6] are included. I acknowledge the influence of these examplary software projects on my programming style. The sample data for the test driver are included by parameter, data, or assignment statements within the driver. The execution output from file TOMP.OUT follows at the end of this file. Also included is the output from file TOMP.M for graphical postprocessing with MATLAB [7]. If MATLAB is available to you, just type (at the DOS prompt): MATLAB and then (at the Matlab prompt) >tomp >plot(x1,-x2), grid (and you should see fig.2 of accompanying VDI-report on screen) This output has been produced on a Compaq DeskPro 486/33L with the integrated Intel Co-Processor, using version 2.5 of the 32-bit compiler FTN77 of the University of Salford. The program has been run under a series of other compilers including FORTRAN/2 Version 1.04, LAHEY F77L32 Version 3.0, MICROSOFT FORTRAN Version 5.0, MICROWAY NDPF486 Version 3.2, WATCOM WATFOR78 Version 3.1, WATCOM WFL386 Version 8.0. [1] D. Kraft: TOMP -- FORTRAN Modules for Optimal Control Calculations. Fortschritt-Berichte VDI Reihe 8 Nr. 254. VDI-Verlag, D€sseldorf, 1991. [2] C.L. Lawson, R.J. Hanson, D.R. Kincaid, F.T. Krogh: Basic Linear Algebra Subprograms for FORTRAN Usage. Sandia Laboratories Tech. Rept. SAND77-0898, and ACM Trans. Math. Softw. 5 (1979) 324-325. [3] C.L. Lawson, R.J. Hanson: Solving Least Squares Problems. Prentice Hall, Englewood Cliffs, New Jersey, 1974. [4] L.F. Shampine, H.A. Watts: The Art of Writing a Runge-Kutta Code Appl. Math. Comp. 5 (1979) 93-121. [5] C. de Boor: A Practical Guide to Splines. Springer, New York, 1978. [6] R.P.Brent: Algorithms for Minimization without Derivatives. Prentice-Hall, Englewood Cliffs, 1973. [7] J. Little, C. Moler: 386-MATLAB User's Guide. The MathWorks, Natick, MA, 1989. File TOMP.OUT: ============== constrained brachistochrone problem iteration value of cost constraint violations 1 0.10000000D+01 0.7771D+00 2 0.20000000D+01 0.1547D+01 3 0.16999844D+01 0.2312D+00 4 0.18906318D+01 0.3518D-01 5 0.18909095D+01 0.2619D-02 6 0.18586003D+01 0.5257D-02 7 0.18265213D+01 0.1007D-01 8 0.17859895D+01 0.2417D-01 9 0.18029380D+01 0.2344D-03 10 0.18002306D+01 0.3732D-03 11 0.17971807D+01 0.9757D-03 12 0.17967373D+01 0.1809D-03 13 0.17957954D+01 0.4562D-03 14 0.17958396D+01 0.8919D-04 15 0.17956336D+01 0.7395D-04 16 0.17956215D+01 0.4949D-05 17 0.17954695D+01 0.3955D-04 18 0.17953546D+01 0.7071D-04 19 0.17953767D+01 0.7151D-05 20 0.17952737D+01 0.4281D-04 21 0.17952743D+01 0.6285D-05 22 0.17952700D+01 0.1907D-05 23 0.17952490D+01 0.8774D-05 24 0.17952443D+01 0.3778D-05 25 0.17952374D+01 0.4448D-05 26 0.17952392D+01 0.8371D-06 27 0.17952392D+01 0.8371D-06 mode = 0 after 27 iterations File TOMP.M: ============ o1=[ 0.000000E+00 4.014142E-08 2.107546E-07 6.122118E-07 1.344870E-06 2.509064E-06 4.205103E-06 6.533262E-06 9.593776E-06 1.348684E-05 1.831258E-05 2.417109E-05 3.116238E-05 3.938640E-05 4.894303E-05 5.993205E-05 7.245318E-05 8.660603E-05 1.024901E-04 1.202049E-04 1.398500E-04 1.615248E-04 1.853301E-04 2.113686E-04 2.397509E-04 2.706081E-04 3.041078E-04 3.404217E-04 3.796926E-04 4.220463E-04 4.676016E-04 5.164745E-04 5.687802E-04 6.246328E-04 6.841461E-04 7.474338E-04 8.146091E-04 8.857846E-04 9.610732E-04 1.040587E-03 1.124438E-03 1.212737E-03 1.305596E-03 1.403126E-03 1.505436E-03 1.612637E-03 1.724836E-03 1.842139E-03 1.964644E-03 2.092430E-03 2.225522E-03 2.363864E-03 2.507401E-03 2.656157E-03 2.810192E-03 2.969585E-03 3.134418E-03 3.304776E-03 3.480745E-03 3.662408E-03 3.849852E-03 4.043160E-03 4.242417E-03 4.447708E-03 4.659115E-03 4.876722E-03 5.100612E-03 5.330867E-03 5.567570E-03 5.810803E-03 6.060647E-03 6.317186E-03 6.580505E-03 6.850696E-03 7.127874E-03 7.412212E-03 7.703973E-03 8.003399E-03 8.310653E-03 8.625850E-03 8.949090E-03 9.280465E-03 9.620066E-03 9.967980E-03 1.032430E-02 1.068910E-02 1.106248E-02 1.144452E-02 1.183531E-02 1.223493E-02 1.264346E-02 1.306098E-02 1.348759E-02 1.392335E-02 1.436836E-02 1.482269E-02 1.528641E-02 1.575961E-02 1.624236E-02 1.673468E-02 1.723660E-02 1.774802E-02 1.826889E-02 1.879922E-02 1.933906E-02 1.988848E-02 2.044752E-02 2.101627E-02 2.159478E-02 2.218311E-02 2.278134E-02 2.338952E-02 2.400772E-02 2.463600E-02 2.527442E-02 2.592305E-02 2.658193E-02 2.725113E-02 2.793072E-02 2.862074E-02 2.932126E-02 3.003234E-02 3.075403E-02 3.148639E-02 3.222950E-02 3.298345E-02 3.374838E-02 3.452441E-02 3.531161E-02 3.611006E-02 3.691980E-02 3.774090E-02 3.857340E-02 3.941735E-02 4.027280E-02 4.113981E-02 4.201841E-02 4.290866E-02 4.381061E-02 4.472429E-02 4.564976E-02 4.658705E-02 4.753621E-02 4.849729E-02 4.947032E-02 5.045534E-02 5.145240E-02 5.246153E-02 5.348278E-02 5.451619E-02 5.556180E-02 5.661969E-02 5.768990E-02 5.877248E-02 5.986746E-02 6.097488E-02 6.209477E-02 6.322716E-02 6.437208E-02 6.552956E-02 6.669963E-02 6.788231E-02 6.907764E-02 7.028564E-02 7.150633E-02 7.273973E-02 7.398587E-02 7.524478E-02 7.651647E-02 7.780097E-02 7.909828E-02 8.040843E-02 8.173145E-02 8.306733E-02 8.441608E-02 8.577769E-02 8.715211E-02 8.853931E-02 8.993930E-02 9.135206E-02 9.277762E-02 9.421598E-02 9.566715E-02 9.713114E-02 9.860796E-02 1.000976E-01 1.016001E-01 1.031154E-01 1.046436E-01 1.061847E-01 1.077386E-01 1.093053E-01 1.108849E-01 1.124774E-01 1.140827E-01 1.157009E-01 1.173319E-01 1.189758E-01 1.206326E-01 1.223022E-01 1.239847E-01 1.256803E-01 1.273888E-01 1.291105E-01 1.308452E-01 1.325930E-01 1.343538E-01 1.361276E-01 1.379145E-01 1.397143E-01 1.415272E-01 1.433530E-01 1.451917E-01 1.470434E-01 1.489080E-01 1.507855E-01 1.526758E-01 1.545790E-01 1.564950E-01 1.584238E-01 1.603653E-01 1.623196E-01 1.642866E-01 1.662663E-01 1.682585E-01 1.702634E-01 1.722806E-01 1.743102E-01 1.763521E-01 1.784062E-01 1.804726E-01 1.825511E-01 1.846418E-01 1.867446E-01 1.888594E-01 1.909863E-01 1.931252E-01 1.952761E-01 1.974389E-01 1.996136E-01 2.018001E-01 2.039985E-01 2.062086E-01 2.084304E-01 2.106640E-01 2.129091E-01 2.151659E-01 2.174342E-01 2.197141E-01 2.220055E-01 2.243085E-01 2.266233E-01 2.289499E-01 2.312883E-01 2.336386E-01 2.360006E-01 2.383743E-01 2.407595E-01 2.431564E-01 2.455647E-01 2.479844E-01 2.504155E-01 2.528578E-01 2.553114E-01 2.577761E-01 2.602519E-01 2.627387E-01 2.652364E-01 2.677449E-01 2.702643E-01 2.727943E-01 2.753349E-01 2.778861E-01 2.804475E-01 2.830190E-01 2.856000E-01 2.881894E-01 2.907870E-01 2.933922E-01 2.960052E-01 2.986257E-01 3.012539E-01 3.038895E-01 3.065327E-01 3.091834E-01 3.118415E-01 3.145070E-01 3.171799E-01 3.198602E-01 3.225477E-01 3.252426E-01 3.279447E-01 3.306540E-01 3.333706E-01 3.360943E-01 3.388250E-01 3.415629E-01 3.443078E-01 3.470595E-01 3.498177E-01 3.525817E-01 3.553503E-01 3.581231E-01 3.608996E-01 3.636799E-01 3.664639E-01 3.692515E-01 3.720429E-01 3.748379E-01 3.776366E-01 3.804390E-01 3.832452E-01 3.860550E-01 3.888685E-01 3.916858E-01 3.945068E-01 3.973314E-01 4.001598E-01 4.029920E-01 4.058278E-01 4.086674E-01 4.115108E-01 4.143579E-01 4.172089E-01 4.200638E-01 4.229231E-01 4.257872E-01 4.286565E-01 4.315310E-01 4.344107E-01 4.372958E-01 4.401861E-01 4.430816E-01 4.459825E-01 4.488886E-01 4.518000E-01 4.547166E-01 4.576384E-01 4.605655E-01 4.634978E-01 4.664353E-01 4.693781E-01 4.723261E-01 4.752792E-01 4.782376E-01 4.812011E-01 4.841698E-01 4.871437E-01 4.901226E-01 4.931065E-01 4.960949E-01 4.990874E-01 5.020837E-01 5.050837E-01 5.080873E-01 5.110946E-01 5.141055E-01 5.171201E-01 5.201383E-01 5.231602E-01 5.261858E-01 5.292150E-01 5.322479E-01 5.352845E-01 5.383247E-01 5.413686E-01 5.444162E-01 5.474675E-01 5.505224E-01 5.535811E-01 5.566435E-01 5.597095E-01 5.627794E-01 5.658531E-01 5.689307E-01 5.720129E-01 5.751002E-01 5.781929E-01 5.812911E-01 5.843948E-01 5.875041E-01 5.906190E-01 5.937394E-01 5.968654E-01 5.999969E-01 6.031339E-01 6.062765E-01 6.094245E-01 6.125781E-01 6.157371E-01 6.189017E-01 6.220717E-01 6.252472E-01 6.284281E-01 6.316145E-01 6.348064E-01 6.380037E-01 6.412065E-01 6.444151E-01 6.476297E-01 6.508516E-01 6.540819E-01 6.573213E-01 6.605698E-01 6.638277E-01 6.670946E-01 6.703706E-01 6.736556E-01 6.769494E-01 6.802521E-01 6.835634E-01 6.868833E-01 6.902118E-01 6.935486E-01 6.968936E-01 7.002469E-01 7.036083E-01 7.069776E-01 7.103548E-01 7.137398E-01 7.171325E-01 7.205327E-01 7.239404E-01 7.273554E-01 7.307774E-01 7.342062E-01 7.376414E-01 7.410828E-01 7.445302E-01 7.479836E-01 7.514428E-01 7.549080E-01 7.583787E-01 7.618552E-01 7.653373E-01 7.688249E-01 7.723179E-01 7.758163E-01 7.793200E-01 7.828289E-01 7.863430E-01 7.898621E-01 7.933862E-01 7.969152E-01 8.004491E-01 8.039878E-01 8.075312E-01 8.110791E-01 8.146316E-01 8.181887E-01 8.217502E-01 8.253161E-01 8.288864E-01 8.324611E-01 8.360400E-01 8.396230E-01 8.432100E-01 8.468010E-01 8.503959E-01 8.539946E-01 8.575969E-01 8.612029E-01 8.648124E-01 8.684253E-01 8.720415E-01 8.756610E-01 8.792837E-01 8.829094E-01 8.865380E-01 8.901696E-01 8.938039E-01 8.974410E-01 9.010806E-01 9.047228E-01 9.083674E-01 9.120143E-01 9.156635E-01 9.193148E-01 9.229681E-01 9.266235E-01 9.302807E-01 9.339398E-01 9.376004E-01 9.412627E-01 9.449265E-01 9.485917E-01 9.522582E-01 9.559259E-01 9.595948E-01 9.632648E-01 9.669356E-01 9.706074E-01 9.742799E-01 9.779530E-01 9.816267E-01 9.853010E-01 9.889756E-01 9.926505E-01 9.963256E-01 1.000001E+00 ]'; x1=o1(:); o2=[ 0.000000E+00 6.471486E-06 2.588532E-05 5.823999E-05 1.035332E-04 1.617619E-04 2.329221E-04 3.170094E-04 4.140182E-04 5.239424E-04 6.467751E-04 7.825083E-04 9.311339E-04 1.092642E-03 1.267023E-03 1.454267E-03 1.654360E-03 1.867292E-03 2.093048E-03 2.331615E-03 2.582978E-03 2.847122E-03 3.124031E-03 3.413686E-03 3.716069E-03 4.031158E-03 4.358926E-03 4.699343E-03 5.052381E-03 5.418013E-03 5.796209E-03 6.186942E-03 6.590180E-03 7.005894E-03 7.434050E-03 7.874615E-03 8.327558E-03 8.792840E-03 9.270427E-03 9.760281E-03 1.026236E-02 1.077664E-02 1.130306E-02 1.184159E-02 1.239219E-02 1.295481E-02 1.352941E-02 1.411594E-02 1.471437E-02 1.532464E-02 1.594674E-02 1.658064E-02 1.722634E-02 1.788382E-02 1.855303E-02 1.923395E-02 1.992652E-02 2.063071E-02 2.134647E-02 2.207377E-02 2.281255E-02 2.356278E-02 2.432441E-02 2.509739E-02 2.588168E-02 2.667722E-02 2.748398E-02 2.830191E-02 2.913094E-02 2.997105E-02 3.082217E-02 3.168425E-02 3.255725E-02 3.344110E-02 3.433575E-02 3.524110E-02 3.615705E-02 3.708349E-02 3.802032E-02 3.896746E-02 3.992486E-02 4.089243E-02 4.187011E-02 4.285783E-02 4.385550E-02 4.486307E-02 4.588046E-02 4.690758E-02 4.794438E-02 4.899077E-02 5.004667E-02 5.111202E-02 5.218673E-02 5.327072E-02 5.436393E-02 5.546625E-02 5.657762E-02 5.769795E-02 5.882717E-02 5.996522E-02 6.111204E-02 6.226761E-02 6.343193E-02 6.460493E-02 6.578655E-02 6.697672E-02 6.817537E-02 6.938241E-02 7.059777E-02 7.182137E-02 7.305313E-02 7.429298E-02 7.554084E-02 7.679661E-02 7.806024E-02 7.933163E-02 8.061070E-02 8.189737E-02 8.319157E-02 8.449319E-02 8.580217E-02 8.711842E-02 8.844184E-02 8.977237E-02 9.110989E-02 9.245431E-02 9.380548E-02 9.516329E-02 9.652761E-02 9.789837E-02 9.927545E-02 1.006588E-01 1.020482E-01 1.034438E-01 1.048452E-01 1.062526E-01 1.076657E-01 1.090844E-01 1.105087E-01 1.119386E-01 1.133737E-01 1.148142E-01 1.162599E-01 1.177106E-01 1.191664E-01 1.206270E-01 1.220924E-01 1.235625E-01 1.250372E-01 1.265164E-01 1.279999E-01 1.294877E-01 1.309797E-01 1.324757E-01 1.339756E-01 1.354793E-01 1.369868E-01 1.384979E-01 1.400125E-01 1.415305E-01 1.430518E-01 1.445763E-01 1.461038E-01 1.476344E-01 1.491679E-01 1.507041E-01 1.522430E-01 1.537845E-01 1.553284E-01 1.568747E-01 1.584232E-01 1.599739E-01 1.615266E-01 1.630813E-01 1.646377E-01 1.661960E-01 1.677559E-01 1.693175E-01 1.708806E-01 1.724451E-01 1.740110E-01 1.755782E-01 1.771465E-01 1.787158E-01 1.802860E-01 1.818571E-01 1.834289E-01 1.850013E-01 1.865742E-01 1.881475E-01 1.897211E-01 1.912949E-01 1.928688E-01 1.944427E-01 1.960165E-01 1.975900E-01 1.991632E-01 2.007360E-01 2.023082E-01 2.038798E-01 2.054505E-01 2.070202E-01 2.085886E-01 2.101558E-01 2.117214E-01 2.132854E-01 2.148478E-01 2.164084E-01 2.179670E-01 2.195237E-01 2.210781E-01 2.226304E-01 2.241803E-01 2.257277E-01 2.272726E-01 2.288148E-01 2.303542E-01 2.318907E-01 2.334242E-01 2.349546E-01 2.364818E-01 2.380057E-01 2.395261E-01 2.410431E-01 2.425564E-01 2.440661E-01 2.455721E-01 2.470746E-01 2.485732E-01 2.500681E-01 2.515590E-01 2.530460E-01 2.545288E-01 2.560074E-01 2.574817E-01 2.589516E-01 2.604170E-01 2.618779E-01 2.633340E-01 2.647853E-01 2.662317E-01 2.676732E-01 2.691095E-01 2.705407E-01 2.719666E-01 2.733871E-01 2.748022E-01 2.762116E-01 2.776154E-01 2.790133E-01 2.804050E-01 2.817901E-01 2.831682E-01 2.845392E-01 2.859029E-01 2.872593E-01 2.886081E-01 2.899493E-01 2.912828E-01 2.926085E-01 2.939263E-01 2.952361E-01 2.965378E-01 2.978313E-01 2.991165E-01 3.003933E-01 3.016615E-01 3.029212E-01 3.041722E-01 3.054143E-01 3.066477E-01 3.078721E-01 3.090875E-01 3.102940E-01 3.114920E-01 3.126827E-01 3.138677E-01 3.150480E-01 3.162242E-01 3.173962E-01 3.185641E-01 3.197280E-01 3.208877E-01 3.220432E-01 3.231945E-01 3.243416E-01 3.254843E-01 3.266228E-01 3.277568E-01 3.288864E-01 3.300116E-01 3.311323E-01 3.322485E-01 3.333601E-01 3.344671E-01 3.355694E-01 3.366672E-01 3.377604E-01 3.388492E-01 3.399346E-01 3.410182E-01 3.421027E-01 3.431899E-01 3.442802E-01 3.453740E-01 3.464714E-01 3.475724E-01 3.486769E-01 3.497852E-01 3.508970E-01 3.520125E-01 3.531317E-01 3.542545E-01 3.553810E-01 3.565112E-01 3.576451E-01 3.587826E-01 3.599239E-01 3.610689E-01 3.622176E-01 3.633700E-01 3.645262E-01 3.656860E-01 3.668494E-01 3.680159E-01 3.691848E-01 3.703547E-01 3.715251E-01 3.726955E-01 3.738660E-01 3.750364E-01 3.762067E-01 3.773769E-01 3.785471E-01 3.797171E-01 3.808870E-01 3.820568E-01 3.832264E-01 3.843959E-01 3.855653E-01 3.867344E-01 3.879034E-01 3.890722E-01 3.902409E-01 3.914093E-01 3.925775E-01 3.937455E-01 3.949134E-01 3.960813E-01 3.972496E-01 3.984192E-01 3.995913E-01 4.007667E-01 4.019456E-01 4.031282E-01 4.043144E-01 4.055043E-01 4.066980E-01 4.078954E-01 4.090965E-01 4.103014E-01 4.115100E-01 4.127223E-01 4.139385E-01 4.151584E-01 4.163820E-01 4.176095E-01 4.188408E-01 4.200759E-01 4.213148E-01 4.225575E-01 4.238040E-01 4.250543E-01 4.263081E-01 4.275649E-01 4.288237E-01 4.300830E-01 4.313420E-01 4.326004E-01 4.338580E-01 4.351148E-01 4.363708E-01 4.376259E-01 4.388801E-01 4.401335E-01 4.413860E-01 4.426377E-01 4.438884E-01 4.451382E-01 4.463870E-01 4.476349E-01 4.488818E-01 4.501278E-01 4.513727E-01 4.526167E-01 4.538596E-01 4.551014E-01 4.563419E-01 4.575807E-01 4.588164E-01 4.600463E-01 4.612669E-01 4.624764E-01 4.636740E-01 4.648594E-01 4.660324E-01 4.671929E-01 4.683408E-01 4.694761E-01 4.705987E-01 4.717085E-01 4.728054E-01 4.738894E-01 4.749604E-01 4.760183E-01 4.770631E-01 4.780946E-01 4.791130E-01 4.801179E-01 4.811095E-01 4.820876E-01 4.830523E-01 4.840035E-01 4.849412E-01 4.858659E-01 4.867783E-01 4.876795E-01 4.885699E-01 4.894497E-01 4.903189E-01 4.911775E-01 4.920255E-01 4.928629E-01 4.936895E-01 4.945054E-01 4.953105E-01 4.961048E-01 4.968883E-01 4.976609E-01 4.984225E-01 4.991733E-01 4.999130E-01 5.006418E-01 5.013594E-01 5.020660E-01 5.027615E-01 5.034458E-01 5.041189E-01 5.047808E-01 5.054311E-01 5.060698E-01 5.066963E-01 5.073106E-01 5.079125E-01 5.085019E-01 5.090790E-01 5.096435E-01 5.101955E-01 5.107350E-01 5.112618E-01 5.117761E-01 5.122778E-01 5.127668E-01 5.132431E-01 5.137067E-01 5.141577E-01 5.145958E-01 5.150212E-01 5.154338E-01 5.158336E-01 5.162205E-01 5.165946E-01 5.169559E-01 5.173042E-01 5.176396E-01 5.179621E-01 5.182717E-01 5.185683E-01 5.188520E-01 5.191227E-01 5.193804E-01 5.196251E-01 5.198568E-01 5.200755E-01 5.202811E-01 5.204737E-01 5.206532E-01 5.208197E-01 5.209731E-01 5.211134E-01 5.212407E-01 5.213547E-01 5.214558E-01 5.215437E-01 5.216185E-01 5.216802E-01 5.217288E-01 5.217642E-01 5.217866E-01 5.217957E-01 ]'; x2=o2(:); o3=[ 0.000000E+00 3.597634E-03 7.195182E-03 1.079259E-02 1.438980E-02 1.798677E-02 2.158342E-02 2.517973E-02 2.877562E-02 3.237105E-02 3.596596E-02 3.956029E-02 4.315400E-02 4.674703E-02 5.033932E-02 5.393082E-02 5.752148E-02 6.111123E-02 6.470004E-02 6.828785E-02 7.187459E-02 7.546022E-02 7.904468E-02 8.262791E-02 8.620985E-02 8.979040E-02 9.336944E-02 9.694682E-02 1.005225E-01 1.040962E-01 1.076681E-01 1.112380E-01 1.148058E-01 1.183714E-01 1.219348E-01 1.254959E-01 1.290547E-01 1.326110E-01 1.361648E-01 1.397160E-01 1.432645E-01 1.468103E-01 1.503533E-01 1.538934E-01 1.574305E-01 1.609647E-01 1.644956E-01 1.680235E-01 1.715481E-01 1.750694E-01 1.785874E-01 1.821024E-01 1.856143E-01 1.891233E-01 1.926293E-01 1.961323E-01 1.996323E-01 2.031291E-01 2.066227E-01 2.101132E-01 2.136004E-01 2.170842E-01 2.205648E-01 2.240419E-01 2.275156E-01 2.309858E-01 2.344525E-01 2.379156E-01 2.413750E-01 2.448307E-01 2.482828E-01 2.517310E-01 2.551754E-01 2.586159E-01 2.620525E-01 2.654848E-01 2.689128E-01 2.723362E-01 2.757547E-01 2.791683E-01 2.825769E-01 2.859805E-01 2.893790E-01 2.927724E-01 2.961604E-01 2.995432E-01 3.029206E-01 3.062926E-01 3.096591E-01 3.130200E-01 3.163753E-01 3.197249E-01 3.230688E-01 3.264069E-01 3.297391E-01 3.330653E-01 3.363855E-01 3.396997E-01 3.430078E-01 3.463098E-01 3.496056E-01 3.528955E-01 3.561795E-01 3.594577E-01 3.627301E-01 3.659965E-01 3.692570E-01 3.725115E-01 3.757599E-01 3.790023E-01 3.822385E-01 3.854685E-01 3.886923E-01 3.919097E-01 3.951209E-01 3.983256E-01 4.015239E-01 4.047156E-01 4.079009E-01 4.110795E-01 4.142515E-01 4.174168E-01 4.205754E-01 4.237272E-01 4.268721E-01 4.300100E-01 4.331408E-01 4.362643E-01 4.393805E-01 4.424892E-01 4.455905E-01 4.486842E-01 4.517704E-01 4.548489E-01 4.579197E-01 4.609828E-01 4.640380E-01 4.670855E-01 4.701250E-01 4.731565E-01 4.761801E-01 4.791956E-01 4.822030E-01 4.852023E-01 4.881933E-01 4.911761E-01 4.941506E-01 4.971167E-01 5.000744E-01 5.030236E-01 5.059643E-01 5.088964E-01 5.118197E-01 5.147343E-01 5.176401E-01 5.205369E-01 5.234249E-01 5.263039E-01 5.291739E-01 5.320347E-01 5.348865E-01 5.377290E-01 5.405624E-01 5.433865E-01 5.462012E-01 5.490066E-01 5.518026E-01 5.545890E-01 5.573660E-01 5.601334E-01 5.628912E-01 5.656393E-01 5.683777E-01 5.711064E-01 5.738253E-01 5.765345E-01 5.792338E-01 5.819235E-01 5.846034E-01 5.872736E-01 5.899340E-01 5.925845E-01 5.952251E-01 5.978558E-01 6.004765E-01 6.030872E-01 6.056878E-01 6.082783E-01 6.108587E-01 6.134288E-01 6.159888E-01 6.185384E-01 6.210778E-01 6.236067E-01 6.261253E-01 6.286334E-01 6.311311E-01 6.336182E-01 6.360947E-01 6.385605E-01 6.410156E-01 6.434597E-01 6.458926E-01 6.483144E-01 6.507248E-01 6.531240E-01 6.555117E-01 6.578881E-01 6.602530E-01 6.626064E-01 6.649483E-01 6.672786E-01 6.695973E-01 6.719043E-01 6.741996E-01 6.764832E-01 6.787550E-01 6.810150E-01 6.832631E-01 6.854993E-01 6.877235E-01 6.899358E-01 6.921360E-01 6.943243E-01 6.965004E-01 6.986645E-01 7.008169E-01 7.029574E-01 7.050861E-01 7.072031E-01 7.093081E-01 7.114014E-01 7.134827E-01 7.155521E-01 7.176095E-01 7.196550E-01 7.216884E-01 7.237097E-01 7.257189E-01 7.277160E-01 7.297009E-01 7.316737E-01 7.336341E-01 7.355824E-01 7.375183E-01 7.394419E-01 7.413530E-01 7.432519E-01 7.451382E-01 7.470118E-01 7.488725E-01 7.507198E-01 7.525533E-01 7.543729E-01 7.561784E-01 7.579700E-01 7.597474E-01 7.615107E-01 7.632599E-01 7.649948E-01 7.667155E-01 7.684219E-01 7.701141E-01 7.717918E-01 7.734552E-01 7.751042E-01 7.767387E-01 7.783588E-01 7.799643E-01 7.815553E-01 7.831318E-01 7.846937E-01 7.862411E-01 7.877741E-01 7.892934E-01 7.908005E-01 7.922975E-01 7.937859E-01 7.952662E-01 7.967386E-01 7.982032E-01 7.996599E-01 8.011088E-01 8.025500E-01 8.039832E-01 8.054087E-01 8.068263E-01 8.082361E-01 8.096380E-01 8.110320E-01 8.124182E-01 8.137964E-01 8.151668E-01 8.165293E-01 8.178840E-01 8.192307E-01 8.205696E-01 8.219007E-01 8.232245E-01 8.245417E-01 8.258550E-01 8.271672E-01 8.284804E-01 8.297954E-01 8.311126E-01 8.324319E-01 8.337534E-01 8.350772E-01 8.364032E-01 8.377315E-01 8.390620E-01 8.403948E-01 8.417298E-01 8.430670E-01 8.444065E-01 8.457482E-01 8.470922E-01 8.484384E-01 8.497869E-01 8.511376E-01 8.524905E-01 8.538457E-01 8.552029E-01 8.565622E-01 8.579230E-01 8.592843E-01 8.606448E-01 8.620036E-01 8.633603E-01 8.647150E-01 8.660674E-01 8.674177E-01 8.687657E-01 8.701116E-01 8.714553E-01 8.727967E-01 8.741359E-01 8.754730E-01 8.768077E-01 8.781404E-01 8.794708E-01 8.807990E-01 8.821250E-01 8.834488E-01 8.847704E-01 8.860897E-01 8.874069E-01 8.887221E-01 8.900352E-01 8.913468E-01 8.926580E-01 8.939701E-01 8.952840E-01 8.965998E-01 8.979178E-01 8.992379E-01 9.005602E-01 9.018847E-01 9.032114E-01 9.045402E-01 9.058713E-01 9.072044E-01 9.085398E-01 9.098774E-01 9.112172E-01 9.125591E-01 9.139032E-01 9.152495E-01 9.165980E-01 9.179486E-01 9.193014E-01 9.206563E-01 9.220133E-01 9.233721E-01 9.247323E-01 9.260926E-01 9.274514E-01 9.288079E-01 9.301617E-01 9.315127E-01 9.328610E-01 9.342064E-01 9.355489E-01 9.368886E-01 9.382255E-01 9.395595E-01 9.408907E-01 9.422191E-01 9.435446E-01 9.448672E-01 9.461870E-01 9.475039E-01 9.488180E-01 9.501292E-01 9.514375E-01 9.527430E-01 9.540455E-01 9.553449E-01 9.566407E-01 9.579316E-01 9.592146E-01 9.604862E-01 9.617447E-01 9.629891E-01 9.642192E-01 9.654350E-01 9.666363E-01 9.678231E-01 9.689955E-01 9.701533E-01 9.712965E-01 9.724252E-01 9.735393E-01 9.746388E-01 9.757236E-01 9.767938E-01 9.778493E-01 9.788901E-01 9.799163E-01 9.809276E-01 9.819243E-01 9.829062E-01 9.838734E-01 9.848261E-01 9.857646E-01 9.866897E-01 9.876027E-01 9.885038E-01 9.893935E-01 9.902716E-01 9.911383E-01 9.919935E-01 9.928372E-01 9.936695E-01 9.944902E-01 9.952995E-01 9.960972E-01 9.968834E-01 9.976581E-01 9.984213E-01 9.991729E-01 9.999130E-01 1.000642E+00 1.001359E+00 1.002064E+00 1.002758E+00 1.003440E+00 1.004110E+00 1.004769E+00 1.005417E+00 1.006051E+00 1.006674E+00 1.007284E+00 1.007881E+00 1.008466E+00 1.009038E+00 1.009597E+00 1.010144E+00 1.010678E+00 1.011199E+00 1.011708E+00 1.012203E+00 1.012686E+00 1.013157E+00 1.013614E+00 1.014059E+00 1.014491E+00 1.014910E+00 1.015316E+00 1.015710E+00 1.016091E+00 1.016459E+00 1.016814E+00 1.017157E+00 1.017487E+00 1.017804E+00 1.018108E+00 1.018399E+00 1.018678E+00 1.018943E+00 1.019196E+00 1.019436E+00 1.019663E+00 1.019878E+00 1.020079E+00 1.020268E+00 1.020444E+00 1.020607E+00 1.020758E+00 1.020895E+00 1.021020E+00 1.021132E+00 1.021230E+00 1.021317E+00 1.021390E+00 1.021450E+00 1.021498E+00 1.021532E+00 1.021554E+00 1.021563E+00 ]'; x3=o3(:); o4=[ 1.568472E+00 1.564594E+00 1.560716E+00 1.556838E+00 1.552960E+00 1.549082E+00 1.545204E+00 1.541326E+00 1.537448E+00 1.533570E+00 1.529692E+00 1.525815E+00 1.521937E+00 1.518059E+00 1.514181E+00 1.510303E+00 1.506425E+00 1.502547E+00 1.498669E+00 1.494790E+00 1.490911E+00 1.487029E+00 1.483140E+00 1.479231E+00 1.475270E+00 1.471165E+00 1.466809E+00 1.462323E+00 1.457789E+00 1.453237E+00 1.448679E+00 1.444119E+00 1.439558E+00 1.434997E+00 1.430435E+00 1.425874E+00 1.421312E+00 1.416750E+00 1.412189E+00 1.407627E+00 1.403066E+00 1.398504E+00 1.393942E+00 1.389381E+00 1.384820E+00 1.380261E+00 1.375704E+00 1.371158E+00 1.366636E+00 1.362185E+00 1.357922E+00 1.353972E+00 1.350177E+00 1.346439E+00 1.342721E+00 1.339012E+00 1.335305E+00 1.331599E+00 1.327894E+00 1.324188E+00 1.320483E+00 1.316778E+00 1.313073E+00 1.309368E+00 1.305663E+00 1.301957E+00 1.298252E+00 1.294547E+00 1.290841E+00 1.287136E+00 1.283429E+00 1.279719E+00 1.276002E+00 1.272265E+00 1.268472E+00 1.264529E+00 1.260349E+00 1.256058E+00 1.251726E+00 1.247379E+00 1.243026E+00 1.238671E+00 1.234316E+00 1.229960E+00 1.225604E+00 1.221248E+00 1.216892E+00 1.212536E+00 1.208180E+00 1.203824E+00 1.199468E+00 1.195112E+00 1.190756E+00 1.186400E+00 1.182045E+00 1.177690E+00 1.173337E+00 1.168989E+00 1.164655E+00 1.160360E+00 1.156169E+00 1.152131E+00 1.148164E+00 1.144222E+00 1.140290E+00 1.136361E+00 1.132433E+00 1.128506E+00 1.124579E+00 1.120653E+00 1.116726E+00 1.112799E+00 1.108872E+00 1.104945E+00 1.101019E+00 1.097092E+00 1.093165E+00 1.089238E+00 1.085311E+00 1.081385E+00 1.077457E+00 1.073529E+00 1.069598E+00 1.065661E+00 1.061706E+00 1.057702E+00 1.053631E+00 1.049530E+00 1.045419E+00 1.041303E+00 1.037186E+00 1.033068E+00 1.028950E+00 1.024832E+00 1.020714E+00 1.016596E+00 1.012478E+00 1.008360E+00 1.004242E+00 1.000124E+00 9.960060E-01 9.918880E-01 9.877700E-01 9.836519E-01 9.795339E-01 9.754157E-01 9.712973E-01 9.671781E-01 9.630570E-01 9.589304E-01 9.547895E-01 9.506299E-01 9.464622E-01 9.422916E-01 9.381199E-01 9.339478E-01 9.297755E-01 9.256032E-01 9.214309E-01 9.172586E-01 9.130862E-01 9.089139E-01 9.047416E-01 9.005693E-01 8.963969E-01 8.922246E-01 8.880523E-01 8.838800E-01 8.797077E-01 8.755355E-01 8.713635E-01 8.671921E-01 8.630223E-01 8.588569E-01 8.547035E-01 8.505818E-01 8.464987E-01 8.424318E-01 8.383708E-01 8.343120E-01 8.302540E-01 8.261963E-01 8.221386E-01 8.180810E-01 8.140235E-01 8.099659E-01 8.059084E-01 8.018508E-01 7.977933E-01 7.937357E-01 7.896782E-01 7.856206E-01 7.815630E-01 7.775053E-01 7.734476E-01 7.693895E-01 7.653304E-01 7.612688E-01 7.572002E-01 7.531124E-01 7.489746E-01 7.447795E-01 7.405611E-01 7.363341E-01 7.321040E-01 7.278727E-01 7.236410E-01 7.194092E-01 7.151772E-01 7.109453E-01 7.067134E-01 7.024814E-01 6.982495E-01 6.940175E-01 6.897856E-01 6.855536E-01 6.813217E-01 6.770898E-01 6.728579E-01 6.686262E-01 6.643948E-01 6.601644E-01 6.559366E-01 6.517161E-01 6.475153E-01 6.433656E-01 6.392704E-01 6.351969E-01 6.311315E-01 6.270690E-01 6.230076E-01 6.189466E-01 6.148857E-01 6.108249E-01 6.067641E-01 6.027033E-01 5.986425E-01 5.945817E-01 5.905209E-01 5.864601E-01 5.823993E-01 5.783385E-01 5.742776E-01 5.702166E-01 5.661553E-01 5.620929E-01 5.580278E-01 5.539554E-01 5.498629E-01 5.457156E-01 5.414290E-01 5.370029E-01 5.325221E-01 5.280212E-01 5.235130E-01 5.190020E-01 5.144901E-01 5.099778E-01 5.054653E-01 5.009528E-01 4.964403E-01 4.919278E-01 4.874153E-01 4.829027E-01 4.783902E-01 4.738778E-01 4.693654E-01 4.648532E-01 4.603418E-01 4.558321E-01 4.513274E-01 4.468362E-01 4.423817E-01 4.380272E-01 4.339452E-01 4.305448E-01 4.277828E-01 4.252673E-01 4.228422E-01 4.204504E-01 4.180708E-01 4.156956E-01 4.133221E-01 4.109492E-01 4.085765E-01 4.062039E-01 4.038314E-01 4.014588E-01 3.990863E-01 3.967137E-01 3.943413E-01 3.919689E-01 3.895969E-01 3.872260E-01 3.848577E-01 3.824969E-01 3.801562E-01 3.778704E-01 3.757343E-01 3.740056E-01 3.732748E-01 3.734196E-01 3.738978E-01 3.744984E-01 3.751441E-01 3.758062E-01 3.764744E-01 3.771448E-01 3.778160E-01 3.784875E-01 3.791591E-01 3.798308E-01 3.805024E-01 3.811741E-01 3.818458E-01 3.825175E-01 3.831891E-01 3.838605E-01 3.845314E-01 3.852012E-01 3.858675E-01 3.865245E-01 3.871562E-01 3.877192E-01 3.880946E-01 3.880219E-01 3.875808E-01 3.870008E-01 3.863699E-01 3.857202E-01 3.850637E-01 3.844047E-01 3.837447E-01 3.830844E-01 3.824240E-01 3.817635E-01 3.811031E-01 3.804426E-01 3.797821E-01 3.791216E-01 3.784612E-01 3.778008E-01 3.771406E-01 3.764808E-01 3.758224E-01 3.751674E-01 3.745221E-01 3.739030E-01 3.733553E-01 3.730018E-01 3.731004E-01 3.735474E-01 3.741247E-01 3.747497E-01 3.753923E-01 3.760412E-01 3.766926E-01 3.773448E-01 3.779974E-01 3.786500E-01 3.793027E-01 3.799555E-01 3.806082E-01 3.812609E-01 3.819136E-01 3.825663E-01 3.832189E-01 3.838713E-01 3.845232E-01 3.851734E-01 3.858194E-01 3.864539E-01 3.870568E-01 3.875737E-01 3.878566E-01 3.876112E-01 3.869836E-01 3.862141E-01 3.853925E-01 3.845518E-01 3.837041E-01 3.828538E-01 3.820026E-01 3.811511E-01 3.802994E-01 3.794476E-01 3.785958E-01 3.777441E-01 3.768923E-01 3.760405E-01 3.751886E-01 3.743365E-01 3.734840E-01 3.726300E-01 3.717723E-01 3.709045E-01 3.700091E-01 3.690383E-01 3.678622E-01 3.661273E-01 3.631724E-01 3.593876E-01 3.552965E-01 3.510928E-01 3.468479E-01 3.425878E-01 3.383221E-01 3.340544E-01 3.297860E-01 3.255173E-01 3.212484E-01 3.169796E-01 3.127107E-01 3.084418E-01 3.041729E-01 2.999041E-01 2.956353E-01 2.913666E-01 2.870984E-01 2.828313E-01 2.785673E-01 2.743117E-01 2.700790E-01 2.659086E-01 2.619080E-01 2.582646E-01 2.548501E-01 2.515197E-01 2.482203E-01 2.449322E-01 2.416483E-01 2.383659E-01 2.350840E-01 2.318024E-01 2.285209E-01 2.252393E-01 2.219578E-01 2.186763E-01 2.153948E-01 2.121133E-01 2.088318E-01 2.055502E-01 2.022686E-01 1.989869E-01 1.957049E-01 1.924218E-01 1.891362E-01 1.858435E-01 1.825313E-01 1.791664E-01 1.756948E-01 1.721587E-01 1.685989E-01 1.650305E-01 1.614588E-01 1.578860E-01 1.543128E-01 1.507393E-01 1.471659E-01 1.435924E-01 1.400189E-01 1.364454E-01 1.328719E-01 1.292984E-01 1.257249E-01 1.221514E-01 1.185779E-01 1.150044E-01 1.114309E-01 1.078574E-01 1.042839E-01 1.007104E-01 9.713706E-02 9.356385E-02 8.999117E-02 8.641950E-02 8.284841E-02 7.927752E-02 7.570671E-02 7.213593E-02 6.856515E-02 6.499438E-02 6.142362E-02 5.785285E-02 5.428208E-02 5.071132E-02 4.714055E-02 4.356979E-02 3.999902E-02 3.642825E-02 3.285749E-02 2.928672E-02 2.571596E-02 2.214519E-02 1.857443E-02 1.500366E-02 1.143289E-02 7.862128E-03 4.291363E-03 7.205969E-04 ]'; x4=o4(:); t= 0.0000000E+00: 3.5976737E-03: 1.7952392E+00; i=length(t); j=length(x1); if i < j t(j) = t(i) + 3.5976737E-03; end clear o1 clear o2 clear o3 clear o4 ==================== End of File TOMP.TXT C TOMP: FORTRAN MODULES for OPTIMAL CONTROL CALCULATIONS c you will find the test driver at the end of this file c corresponding results have been written to the accompanying c file tomp.txt. In that file also some remarks concerning c TOMP have been included. C Dieter Kraft C Fachhochschule M€nchen ************************************************************************ * SIMULATOR * ************************************************************************ SUBROUTINE d_TOMP (SLV, RHS, VALUES, OUTPUT, IV, X, Y, * Z, Z0, Z1, ACC, F, C, DF, DC, LC) C d_TOMP EVALUATION OF COST FUNCTION F(X) AND CONSTRAINT VECTOR C C(X), TOGETHER WITH GRADIENT DF(X) AND JACOBIAN DC(X). C USAGE : C CALL d_TOMP (SLV, RHS, VALUES, OUTPUT, IV, X, Y, C * Z, Z0, Z1, ACC, F, C, DF, DC, LC) C PURPOSE : C SIMULATES SYSTEM GIVEN IN RHS C FOR GIVEN INITIAL CONDITIONS Z0 AND GIVEN CONTROLS X C EVALUATES DEVIATIONS FROM FINAL CONDITIONS Z1 AND FROM TRAJEC- C TORY CONSTRAINTS SPECIFIED IN USER SUPPLIED SUBROUTINE VALUES; C ALSO PARTIAL DERIVATIVES OF Z WITH RESPECT TO X ARE GENERATED. C INPUT ARGUMENTS : C FIRST FOUR ARGUMENTS ARE SUBROUTINES WHICH HAVE TO BE DECLARED C EXTERNAL IN THE CALLING PROGRAM. C THE CALLING SEQUENCE CAN BE FOUND AT THE RESPECTIVE LINES IN TOMP. C SLV : INITIAL VALUE SOLVER C USAGE: C CALL SLV(RHS, NZ, Z, T0, T1, RE, AE, IFLAG, W, IW, X, IV) C PARAMETER DESCRIPTION TO BE FOUND IN THE DESCRIPTION OF C SUBROUTINE SLV later in this PACKAGE C RHS : RHS(T,Z,ZDOT,X,IV) DESCRIBES THE SIMULATED SYSTEM OF C ORDINARY DIFFERENTIAL EQUATIONS ZDOT(T)=PHI(T,Z,X,IV). C VALUES : USER SUPPLIED SUBROUTINE WHICH C EVALUATES BOUNDARY CONDITIONS, TRAJECTORY CONSTRAINTS, C AND COST. FURTHER INFORMATION CAN BE TAKEN FROM THE C EXAMPLE PROBLEM. C USAGE: C CALL VALUES (RHS, IV, T0, X, Y, Z, Z0, Z1, W, F, C, j) C PARAMETERS ARE AS DESCRIBED IN THE PRESENT DESCRIPTION C INPUT: RHS, IV, T0, X, Y, Z, j C WHERE T0 IS THE CURRENT TIME IN d_TOMP, C j MUST BE ZERO FOR THE FIRST CALL AFTERWARDS C j IS THE INDEX OF THE COMMUNICATION INTERVAL C SET BY d_TOMP. C OUTPUT: Z0, Z1 MUST BE GIVEN BY THE USER FOR j=0 C COST F and BOUNDARY VALUES C MUST BE FORMUL- C ATED FOR THE LAST COMMUNICATION POINT C INTERMEDIATE VALUES OF C CAN BE FORMULATED C FOR POINTWISE SATISFACTION OF STATE CONSTRAINTS C IN/OUT: W WORK ARRAY of DIMENSION as in SLV C OUTPUT : PRINT-OUT SUBROUTINE TO BE FURNISHED BY USER. C PARAMETERS ARE AS DESCRIBED IN THE PRESENT DESCRIPTION C USAGE: C CALL OUTPUT (T, K, G, LONG, NO, Z, X, IV) C INPUT: T, K, G, LONG, Z, X, IV C WHERE T IS THE CURRENT TIME set by d_TOMP, C K IS THE INDEX OF THE INTEGRATION INTERVAL C set by d_TOMP, G(LONG, BROAD) is a REAL*4 C ARRAY to store VALUES FOR PLOTTING, see later C COLUMNS NO-(NU+NZ) CAN BE ATTACHED TO G by USER C OUTPUT: NO see last line of INPUT C IV : INTEGER ARRAY OF LENGTH 20, CONTAINING: C NI(1) = IV(1) C . C . NUMBER OF BREAKPOINTS OF AT MOST 6 CONTROLS, C . C NI(NU) = IV(NU) C NI(NU+1) = IV(NU+1) C . C . INTERPOLATION MODEs OF AT MOST 6 CONTROLS, C . C NI(2*NU) = IV(2*NU) C MODE : INDICATES ORDER OF INTERPOLATION: C 1 = PIECEWISE CONSTANT C 2 = PIECEWISE LINEAR C 3 = PIECEWISE CUBIC SPLINE C 4 = PIECEWISE EXPONENTIAL SPLINE C TENSIONS ARE BEING CALCULATED C 5 = PIECEWISE EXPONENTIAL SPLINE C TENSIONS ARE GIVEN BY USER C 6 = AKIMA'S INTERPOLANT C NP = IV(13) NUMBER OF DESIGN PARAMETERS PI, C NU = IV(14) ACTUAL NUMBER OF CONTROL VARIABLES U, C NY = IV(15) NUMBER OF COMMUNICATION POINTS Y, C NZ = IV(16) NUMBER OF STATE VARIABLES Z, C M = IV(17) TOTAL NUMBER OF CONSTRAINTS C, C EQUALITY CONSTRAINTS (BOUNDARY VALUES) C MUST BE PLACED FIRST IN C. C MODE = IV(18) OPTIMISATION CHOICE: C MODE = 1 : DATA ARE FREE FOR OPTIMIZATION, c including np design parameters, C MODE = 2 : ALSO BREAKPOINTS ARE FREE FOR C OPTIMIZATION, C MODE = 3 : ALSO TENSIONS ARE FREE FOR C OPTIMIZATION, C ISW = IV(19) FLAG TO CONTROL CALCULATIONS: C ISW = 0 : CALCULATES FUNCTIONS C AND DERIVATIVES, C ISW = 1 : CALCULATES FUNCTIONS ONLY, C ISW =-1 : CALCULATES DERIVATIVES ONLY, C ISW =-2 : PRINTS TRAJECTORY C VIA OUTPUT AND STORES DATA FOR C PLOTTING VIA R A S P FORMAT C OR VIA M A T L A B FORMAT C ISW =-3 : INTEGRATES IN ONE-STEP-MODE C TO DISPLAY HARD INTEGRATION WORK C IN FUNCTION GENERATION C ISW =-4 : INTEGRATES IN ONE-STEP-MODE C TO DISPLAY HARD INTEGRATION WORK C IN GRADIENT GENERATION C IP = IV(20) ON ENTRY UNIT-NUMBER for STORING PLOT DATA C ON EXIT C COUNTER FOR RIGHT-HAND-SIDE EVALUATION. C X : DOUBLE PRECISION ARRAY OF LENGTH NP + SUM OVER J (6*NI(J)) C J=1, ... , NU, WHERE NU IS AT MOST 6, C THE ORDER OF WHICH IS THE FOLLOWING: C CONTAINS DATA C X(1), ... , X(NI(1)), OF THE FIRST C X(NI(1)+1), ... , X(NI(1)+NI(2)), SECOND C X(NI(1)+NI(2)+1), ..., X(NI(1)+NI(2)+ ... +NI(NU)) C NU-th CONTROL, C X(... +NI(NU)+1, ... ,NI(NU)+NP) CONTAINS FREE FINAL TIME, C AND OTHER DESIGN PARAMETERS NOT DEPENDING ON TIME, C FREE FINAL TIME must BE THE FIRST DESIGN PARAMETER C X(NI(1)+NI(2)+ ... +NI(NU)+NP+1) , ... , C X(2*(NI(1)+NI(2)+ ... +NI(NU))+NP) , ... , C CONTAINS BREAKPOINT SEQUENCE 1, ... , NU, C OF SPLINE FUNCTION WHERE THE DATA ARE GIVEN, C X(2*(NI(1)+NI(2)+ ... +NI(NU))+NP+1) , ... , C X(3*(NI(1)+NI(2)+ ... +NI(NU))+NP) , ... , C CONTAINS TENSION PARAMETERS C FOR THE SPLINE FUNCTION, 1, ..., NU, C X(4*(NI(1)+NI(2)+ ... +NI(NU))+NP+1) , ... , C X(6*(NI(1)+NI(2)+ ... +NI(NU))+NP) , ... , C CONTAINS NU SETS OF COEFFICIENTS A, B, C, C TO BE EVALUATED IN SUBROUTINE SPLINE. C Y : DOUBLE PRECISION ARRAY OF LENGTH NY OF COMMUNICATION POINTS C AT WHICH THE USER 'COMMUNICATES' WITH THE PROBLEM VIA VALUES C Z : DOUBLE PRECISION ARRAY OF LENGTH NZ OF STATE VARIABLES. C Z0 : DOUBLE PRECISION ARRAY OF LENGTH NZ OF INITIAL VALUES. C Z1 : DOUBLE PRECISION ARRAY OF LENGTH NZ OF FINAL VALUES. C ACC : DOUBLE PRECISION VALUE OF RELATIVE AND ABSOLUTE ACCURACY OF C THE INITIAL VALUE SOLVER. C LC : INTEGER VARIABLE INDICATING LEADING DIMENSION OF DC(LC,N+1). C OUTPUT ARGUMENTS : C F : DOUBLE PRECISION VARIABLE CONTAINING THE CALCULATED COST. C C : DOUBLE PRECISION ARRAY OF LENGTH M CONTAINING THE C CONSTRAINTS. C DF : DOUBLE PRECISION ARRAY OF LENGTH N CONTAINING THE GRADIENT. C OF THE COST FUNCTION. C DC : DOUBLE PRECISION MATRIX OF LENGTH (LC,N) CONTAINING THE C JACOBIAN OF THE CONSTRAINTS. C IFC = IV(20) COUNTER FOR RIGHT-HAND-SIDE EVALUATION. C DUMMY ARGUMENTS : C W : DOUBLE PRECISION ARRAY USED FOR SLV, THE LENGTH LW OF WHICH C SHOULD AT LEAST BE 13*NZ + 3 + M FOR RK87 AS SLV, C AND 7*NZ + 3 + M FOR RK54 AS SLV, RESPECTIVELY. C IN THE PRESENT IMPLEMENTATION W IS FIXED AT 250, C See PARAMETER Statement. C W(13*NZ+4) ,..., W(13*NZ+3+M) D.P. ARRAY, STORING THE C PERTURBED CONSTRAINTS; IF YOUR PROBLEM GENERATES MORE C THEN 250-13*NZ+3 CONSTRAINTS ENLARGE W ACCORDINGLY. C G : SINGLE PRECISION DOUBLE INDEXED ARRAY OF FIXED LENGTH C (LONG, BROAD) IN WHICH TRAJECTORY INFORMATION FOR R A S P C PLODAT AND PLOTHP or M A T L A B IS STORED. C The Array DIMENSION can be taken from the C PARAMETER Statement. C ERROR NUMBERS : C NONE; THE PROGAM STOPS IF THERE ARE FAULT CONDITIONS IN SLV; C RELEVANT INFORMATION WILL BE GIVEN. C METHOD : C FUNCTION-GENERATOR FOR DIRECT SHOOTING DESCRIBED IN: C D. KRAFT: 'ON CONVERTING OPTIMAL CONTROL PROBLEMS INTO C MATHEMATICAL PROGRAMMING PROBLEMS'. C IN: K. SCHITTKOWSKI (ED.): COMPUTATIONAL MATHEMATICAL PROGRAMMING, C SPRINGER, BERLIN 1985. C D. KRAFT: 'TOMP - FORTRAN MODULES FOR OPTIMAL CONTROL CALCULATIONS' C FORTSCHRITT-BERICHT xxx, VDI-VERLAG, D€SSELDORF, 1991. C REMARKS : C TOMP HAS BEEN TESTED ON VARIOUS REAL-LIFE PROBLEMS C INCLUDING AEROSPACE & ROBOTICS PATH PLANNING C USING THE FOLLOWING COMPILERS: C FORTRAN/2 VERSION 1.04 C LAHEY F77L32 VERSION 3.0 C MICROSOFT FORTRAN VERSION 5.0 C MICROWAY NDPF486 VERSION 3.2 C SALFORD FTN77 VERSION 2.5 C WATCOM WATFOR78 VERSION 3.1 C WATCOM WFL386 VERSION 8.0 C AUTHOR : C DIETER KRAFT, C DEUTSCHE FORSCHUNGS- UND VERSUCHSANSTALT FUER LUFT- UND RAUMFAHRT, C OBERPFAFFENHOFEN, C INSTITUT FUER DYNAMIK DER FLUGSYSTEME, C M€NCHNER STRASSE 20 C D-8031 WESSLING C TEL. 08153 / 28443 C 26. 1. 1985 C now at: C FACHHOCHSCHULE M€NCHEN C FACHBEREICH MASCHINENBAU C DACHAUERSTRASSE 98 B C D-8000 M€NCHEN 2 C TEL. 089 / 1265 1108 C FAX. 089 / 1265 1490 C EMAIL: kraf@maschinenbau.fh-muenchen.dbp.de C COPYRIGHT : Dieter Kraft c/o C *************************************** C ************** D L R ************** C ************** 1 9 8 6 ************** C *************************************** C ************** F H M ************** C ************** 1 9 9 1 ************** C *************************************** C MODIFICATIONS : C ARRAY G CHANGED TO SINGLE PRECISION C IN THIS CASE PLODAT MUST READ SINGLE PRECISION ARRAY X C CHANGED ON 18/02/85 C PLODAT INCORPORATED INTO TOMP, NAMELIST AS LOCAL DUMPING C POSSIBILITY ESTABLISHED C CHANGED ON 09/03/85 C INCORPORATE HANDLING OF NP DESIGN PARAMETERS C AND HANDLING OF PIECEWISE CONSTANT CONTROLS C ADAPTATION TO LANGUAGE LEVEL 77 OF FORTRAN 77 COMPILERS C MAKES NECESSARY A MODIFICATION (AUGMENTATION) OF PARAMETER LIST C CHANGED ON 31/01/86 C CHANGED TO IBM PS/2 STANDARD WITH MICROSOFT FORTRAN VERSION 4.01 C CHANGED TO IBM PS/2 STANDARD WITH FORTRAN/2 RYAN-McFARLAND V.1.0 C NAMELIST STATEMENT ELIMINATED, AS NOT ALLOWED WITHIN MS-FORTRAN C ZTIME REPLACED BY GETTIM AND GETDAT C INTEGER VECTOR IV(20) RE-SORTED C CHANGED ON 24/01/89 & 9/04/89 C INTEGRATION OF MATLAB PLOTting FACILITY C CREATION OF TOMP.M FILE WITH TRAJECTORIES C COMMON /CXTOMP/ has been de-activated C CHANGED ON 14/02/89 C INTERRUPT INTEGRATION IF ERROR-FLAG CONTINUOUSLY SET C RETURN WITH F = FLARGE C CHANGED ON 20/05/89 C MAXIMUM NUMBER OF CONTROLS CHANGED TO SIX C BECAUSE OF APPLICATIONS IN ROBOTICS C CHANGED ON 07/04/91 C INCORPORATION OF CALCULATION OF VISUALLY PLEASING C EXPONENTIAL SPLINE INTERPOLANTS OF RENTROP & WEVER C CHANGED ON 16/06/91 C DATE : C 16/06/91 C REQD. COMMON BLOCKS : C COMMON /CXTOMP/ STORES THE PLOT-RELEVANT DATA G, TANF, TEND, HSP. C ALL RELEVANT INFORMATION FOR THE CONTROLS C IS GIVEN IN THE PAIR (X, IV). C COMMON /CXTOMP/ has been de-activated C REQD. RASP-ROUTINES : * = DIRECT CALL C *SLV,*SPLINE,*SPLINT C REQD. GRAPHIC ROUTINES : * = DIRECT CALL C NO GRAPHIC ROUTINES C REQD. USER-ROUTINES : * = DIRECT CALL C *OUTPUT,*VALUES,*RHS C REQD. LIBRARY-ROUTINES : * = DIRECT CALL C *SPLINE,*SLV,*GETDAT,*GETTIM C ALTERNATIVES FOR SLV : RK87, RK54, and respective CORE- C routines; SPLINE. Euler-Cauchy in-line coded. C GETDAT, GETTIM are compiler-depending date and time routines C both are available or are emulated with identical I/O in the C following compiler for microcomputers: FORTRAN/2, MS-FORTRAN, C WATCOM-WATFOR77 C date and time routines are de-activated C----------------------------------------------------------------------- C DECLARATION OF VARIABLES , ARRAYS , COMMON AND NAMELIST C IBM-MAINFRAME NAMELIST DEACTIVATED INTEGER*2 IHR, IMIN, ISEC, IHUN, IYR, IMON, IDAY INTEGER IV(20), IW(5) INTEGER I, MODE, IFLAG, IG, IP, IPL, IPLOT, IS, ISW, I1, IVI, * J, JSW, J1, J2, K, KP, K1, L, LC, LW, LEND, LONG, M, N, NI, * NJ, NK, NO, NP, NUMBER, NY, NU, NW, NZ, N1, N2, N3, N4, N5, * O, IDUMMY, IANF, BROAD, LONG1, JFLAG, IFC, IVJ PARAMETER (LW = 750, LONG = 501, LONG1 = LONG-1, BROAD = 18) DOUBLE PRECISION F, C(*), DF(*), DC(LC,*), W(LW), X(*), Y(*), * Z(*), Z0(*), Z1(*), E, H, T, T0, T1, TF, RE, AE, ACC, DEL, EPS, * EPMACH, RTEPS, ZERO, TEN, FLARGE, SPLINT, DFLOAT REAL G(LONG,BROAD), TANF, TEND, HSP LOGICAL ONESTP, MATLAB, RASP EXTERNAL RHS, SLV, VALUES, OUTPUT INTRINSIC ABS, MAX, MIN, SQRT * COMMON /CXTOMP/ G, TANF, TEND, HSP CONTROLS FORWARD DIFFERENCE INTERVAL; FOR TESTING PURPOSES ONLY * DOUBLE PRECISION DEL1 * COMMON /CDTOMP/ DEL1, IDEL1 * NAMELIST /NXTOMP/ IFLAG, IPLOT, K, KP, LEND, N, NUMBER, * * E, H, T, T0, T1, TF, DEL, EPS, W, G DATA EPMACH /2.22D-16/, ZERO /0.0D+00/, TEN /10.0D+00/ DATA FLARGE /1.00D+12/ DATA IS /06/, IP/110/, IANF /1/, MATLAB /.TRUE./ DATA IHR, IMIN, ISEC, IHUN, IYR, IMON, IDAY /7*0/ C----------------------------------------------------------------------- C EXTRACT PLOT INDICES FROM IP IF (IANF .EQ. 1) THEN IP = IV(20) IPL = IP/10 IPLOT = IP - IPL*10 IANF = 0 IV(20) = 0 OPEN (UNIT=IPL, FILE='TOMP.M') END IF C MATLAB or RASP GRAPHICS? RASP = .NOT. MATLAB C SET INTEGRATION ACCURACY RE = ACC AE = ACC RTEPS = SQRT(EPMACH) C LOWER BOUND FOR PERTURBATION EPS = MAX(RTEPS,ACC/TEN) C RECOVER FROM C INTEGER ARRAY IV(I), I=1, ..., 20, WHICH CONTAINS C NI(I), I=1, ... , 6, NUMBER OF BREAKPOINTS FOR MAXIMAL 6 CONTROLS C NI(I+NU), I=1, ... , 6, INTERPOLATION MODE OF MAXIMAL 6 CONTROLS C NP NUMBER OF DESIGN PARAMETERS (INCLUDING FREE FINAL TIME) C NU NUMBER OF CONTROLS C NY NUMBER OF COMMUNICATION POINTS C NZ NUMBER OF STATES C M NUMBER OF CONSTRAINTS(BOUNDARY VALUES + TRAJECTORY CONSTRAINTS) C ISW SIMULATION SWITCH C MODE OPTIMIZATION ALTERNATIVES (SEE COMMENT IMMEDIATELY AFTER C LABEL 220) C IFC NUMBER OF RHS-EVALUATIONS NP = IV(13) NU = IV(14) NY = IV(15) NZ = IV(16) M = IV(17) MODE = IV(18) ISW = IV(19) IFC = IV(20) IF (NP .LT. 0) THEN WRITE (IS, 9830) NP STOP END IF IF (NU .LT. 1 .OR. NU .GT. 6) THEN WRITE (IS, 9840) NU STOP END IF IF (NY .LT. 2) THEN WRITE (IS, 9850) NY STOP END IF IF (NZ .LT. 1) THEN WRITE (IS, 9860) NZ STOP END IF IF (MODE .LT. 1 .OR. MODE .GT. 3) THEN WRITE (IS, 9870) MODE STOP END IF IF (ISW .LT. -4 .OR. ISW .GT. 1) THEN WRITE (IS, 9880) ISW STOP END IF I = NU+NZ IF (I .GT. BROAD) THEN WRITE (IS, 9890) I PAUSE + 'PAUSE: YOU MAY INVOKE A DEBUGGER DISPLAY HERE BEFORE STOP' STOP END IF I = M + 13*NZ + 3 IF (I .GT. LW) THEN WRITE (IS, 9900) I PAUSE + 'PAUSE: YOU MAY INVOKE A DEBUGGER DISPLAY HERE BEFORE STOP' STOP END IF C SUM UP TOTAL NUMBER N OF SPLINE-BREAKPOINTS N = 0 DO 10 I=1,NU IVI = IV(I) IVJ = IV(NU+I) N = N + IVI IF (IVJ .EQ. 1 .AND. IVI .LT. 1 + .OR. IVJ .EQ. 2 .AND. IVI .LT. 2 + .OR. IVJ .GE. 3 .AND. IVI .LT. 4) THEN WRITE (IS, 9902) I PAUSE + 'PAUSE: YOU MAY INVOKE A DEBUGGER DISPLAY HERE BEFORE STOP' STOP END IF IVI = IV(NU+I) IF (IVI .LT. 1 .OR. IVI .GT. 6) THEN WRITE (IS, 9904) I PAUSE + 'PAUSE: YOU MAY INVOKE A DEBUGGER DISPLAY HERE BEFORE STOP' STOP END IF 10 CONTINUE N1 = N + NP N2 = N + N1 N3 = N + N2 N4 = N + N3 N5 = N + N4 NW = 13*NZ + 3 J = 1 DO 15 I=1,NU-1 IF (ABS(X(N1+J+IV(I))-X(N1+J)).GT.EPMACH) THEN WRITE (IS, 9906) PAUSE + 'PAUSE: YOU MAY INVOKE A DEBUGGER DISPLAY HERE BEFORE STOP' STOP END IF IF (ABS(X(N1+J+IV(I)+IV(I+1)-1)-X(N1+J+IV(I)-1)).GT.EPMACH) + THEN WRITE (IS, 9906) PAUSE + 'PAUSE: YOU MAY INVOKE A DEBUGGER DISPLAY HERE BEFORE STOP' STOP END IF J = J + IV(I) 15 CONTINUE IF (ISW.EQ.(-1)) GO TO 220 C ONE-STEP INTEGRATION MODE? ONESTP = .FALSE. ONESTP = ISW.LE.(-3) C----------------------------------------------------------------------- C FUNCTIONS BY TRAJECTORY SIMULATION WITH BASIS FUNCTIONS C FROM SPLINE AS CONTROLS C----------------------------------------------------------------------- C COMPUTE SPLINE COEFFICIENTS X(N1+N+N+J) .... X(N1+N+N+N+J) C FOR GIVEN BREAKPOINTS X(N1+J), DATA X(J), AND TENSIONS X(N1+N+J), C J=1, ... , IV(I), I=1, ... , NU. C EVALUATE SPLINES IN SUBROUTINE RHS FOR GIVEN TIME T. J = 1 DO 20 I=1,NU CALL SPLINE (IV(NU+I), IV(I), X(N1+J), X( J), * X(N2+J), X(N3+J), X(N4+J), X(N5+J)) J = J + IV(I) C TENSIONS HAVE BEEN CALCULATED; C NOW RESET INTERPOLATION MODE IF (IV(NU+I) .EQ. 4) IV(NU+I) = 5 20 CONTINUE C INITIAL CONDITIONS ARE IN Z0(I), I=1, ... , NZ, C OR ARE FREE PARAMETERS IN X AND WILL BE SWAPPED FROM C X TO Z0 IN VALUES FOR MODE = 0. C ALSO VECTOR Y OF COMMUNICATION POINTS MAY BE ASSIGNED IN VALUES C FOR MODE = 0. 30 CALL VALUES (RHS, IV, T0, X, Y, Z, Z0, Z1, W, F, C, 0) T0 = Y(1) DO 40 I=1,NZ Z(I) = Z0(I) 40 CONTINUE C IS GRAPH OF TRAJECTORIES WANTED ONLY? IF (ISW.EQ.(-2)) GO TO 530 IFLAG = 1 IF (.NOT.ONESTP) GO TO 50 C ONE-STEP-INTEGRATION-MODE IFLAG = -1 WRITE (IS, 9760) C INTEGRATE SYSTEM GIVEN IN RHS BY INITIAL-VALUE-SOLVER SLV C WITH SPLINE-BREAKPOINTS X(N1+J), J=1, ... , IV(I), I=1, ... , NU, C AND COMMUNICATION POINTS Y(I), I=1, ... , NY, AS OUTPUT INTERVALS. 50 DO 210 I=1,NY C FIND LEFTMOST BREAKPOINT LARGER THAN T0 60 K1 = N1 TF = Y(NY) DO 90 I1=1,NU J2 = IV(I1) DO 70 J1=1,J2 T = X(K1+J1) IF (T.GT.T0) GO TO 80 70 CONTINUE 80 IF (T.LT.TF) TF = T K1 = K1 + J2 90 CONTINUE T1 = Y(I) C IS NEXT COMMUNICATION POINT SMALLER THAN BREAKPOINT FOUND ? IF (TF.LT.T1) T1 = TF C RESET CONTINUATION FLAG FOR ERROR CONDITIONS IN SLV JFLAG = 0 C INITIAL VALUE SOLVER; TAKE HIGH ORDER FORMULAE (E.G. 8(7)) FOR C SPARSE OUTPUT, AND LOW ORDER FORMULAE (E.G. 5(4)) FOR DENSE OUTPUT. 100 CALL SLV(RHS, NZ, Z, T0, T1, RE, AE, IFLAG, W, IW, X, IV) IF (ONESTP) GO TO 180 C INTEGRATION OUTPUT FLAG OK FOR CONTINUATION ? IF (ABS(IFLAG).EQ.2) GO TO 200 J = 1 DO 110 K=1,NU W(LW-K) = SPLINT (IV(NU+K), IV(K), X(N1+J), X(J), * X(N2+J), X(N3+J), X(N4+J), X(N5+J), T0) J = J + IV(K) 110 CONTINUE JFLAG = JFLAG + 1 IF (JFLAG .GT. 2) THEN K = -1 CALL VALUES (RHS, IV, T0, X, Y, Z, Z0, Z1, W, F, C, K) CALL OUTPUT(T0, K, G, LONG, NO, Z, X, IV) F = FLARGE DO 115, J=1, M C(J) = -FLARGE 115 CONTINUE WRITE (IS, 9680) GO TO 600 END IF WRITE (IS, 9690) IFLAG WRITE (IS, 9780) T0, T1, T, TF, Y(I) WRITE (IS, 9780) (Z(J),J=1,NZ) WRITE (IS, 9780) (W(LW-J),J=1,NU) GO TO (170, 200, 120, 130, 140, 150, 160, 170), IFLAG 120 WRITE (IS, 9700) GO TO 100 130 WRITE (IS, 9710) GO TO 100 140 WRITE (IS, 9720) AE = RTEPS GO TO 100 150 WRITE (IS, 9730) AE = TEN*AE RE = TEN*RE IFLAG = 2 GO TO 100 160 WRITE (IS, 9740) ONESTP = .TRUE. GO TO 30 170 WRITE (IS, 9750) PAUSE * 'PAUSE: YOU MAY INVOKE A DEBUGGER DISPLAY HERE BEFORE STOP' STOP C IN ONE-STEP-INTEGRATION-MODE PRINT STATES AND CONTROLS 180 J = 1 DO 190 K=1,NU W(LW-K) = SPLINT (IV(NU+K), IV(K), X(N1+J), X(J), * X(N2+J), X(N3+J), X(N4+J), X(N5+J), T0) J = J + IV(K) 190 CONTINUE WRITE (IS, 9780) T0, T1, (Z(J),J=1,NZ), (W(LW-J),J=1,NU) IF (IFLAG.EQ.(-2)) GO TO 100 IFLAG = -2 C COMMUNICATION POINT REACHED ? 200 IF (T1.LT.Y(I)) GO TO 60 C CALLS BOUNDARY VALUES, TRAJECTORY CONSTRAINTS, AND COST FUNCTION CALL VALUES (RHS, IV, T0, X, Y, Z, Z0, Z1, W, F, C, I) 210 CONTINUE C THIS ENDS INTEGRATION LOOP; RHS-EVALUATIONS ARE ACCUMULATED IFC = IFC + IW(1) C IF FUNCTIONS ARE REQUIRED ONLY RETURN, ELSE CONTINUE. IF (ISW.EQ.(+1)) GO TO 600 IF (ISW.EQ.(-3)) GO TO 600 C----------------------------------------------------------------------- C GRADIENTS BY FORWARD DIFFERENCES C----------------------------------------------------------------------- C ALL COMMENTS FROM ABOVE ARE VALID HERE AS PERTURBED TRAJECTORIES C ARE INTEGRATED 220 DO 520 JSW = 1, MODE C MODE = 1 : GRADIENTS FOR OPTIMAL DATA, INCLUDING DESIGN PARAMETERS C = 2 : GRADIENTS FOR OPTIMAL BREAKPOINTS C = 3 : GRADIENTS FOR OPTIMAL TENSIONS GO TO (230, 240, 250), JSW C INITIALIZE PARAMETER INDEX KP 230 KP = 0 LEND = N1 GO TO 260 240 LEND = N GO TO 260 250 LEND = N 260 IG = 1 NJ = 1 ONESTP = .FALSE. ONESTP = ISW.EQ.(-4) C START LOOP OVER ALL PARAMETERS DO 510 L=1,LEND IF (IG.GT.NU) GO TO 270 NI = IV(IG) NK = IV(IG+NU) 270 O = NI + NJ - 1 KP = KP + 1 H = X(KP) C FIRST AND LAST BREAKPOINT OF A CONTROL REMAIN UNPERTURBED IF (JSW.EQ.2 .AND. (L.EQ.NJ .OR. L.EQ.O)) GO TO 480 C PERTURBATION OF PARAMETER X(KP) DEL = MAX(EPS,ABS(H)*EPS) ****TEST IF (IDEL1 .EQ. 1) DEL = DEL1 X(KP) = H + DEL C DO NOT EVALUATE SPLINE FOR FREE ENDTIME X(N+1), C AND OTHER FREE DESIGN PARAMETERS. IF (L.GT.N) GO TO 280 CALL SPLINE (NK, NI, X(N1+NJ), X( NJ), X(N2+NJ), * X(N3+NJ), X(N4+NJ), X(N5+NJ)) IF (NK .EQ. 4) IV(IG+NU) = 5 C INITIAL CONDITIONS ARE IN Z0(I), I=1, ... , NZ. 280 CALL VALUES (RHS, IV, T0, X, Y, Z, Z0, Z1, W, E, W(NW+1), 0) T0 = Y(1) DO 290 I=1,NZ Z(I) = Z0(I) 290 CONTINUE IFLAG = 1 IF (.NOT.ONESTP) GO TO 300 IFLAG = -1 WRITE (IS, 9770) C HERE LOOP 210 IS REPEATED EXACTLY IN LOOP 460 300 DO 460 I=1,NY C FIND LEFTMOST BREAKPOINT LARGER THAN T0 310 K1 = N1 TF = Y(NY) DO 340 I1=1,NU J2 = IV(I1) DO 320 J1=1,J2 T = X(K1+J1) IF (T.GT.T0) GO TO 330 320 CONTINUE 330 IF (T.LT.TF) TF = T K1 = K1 + J2 340 CONTINUE T1 = Y(I) C IS NEXT COMMUNICATION POINT SMALLER THAN BREAKPOINT FOUND ? IF (TF.LT.T1) T1 = TF C INITIAL VALUE SOLVER; TAKE HIGH ORDER FORMULAE (E.G. 8(7)) FOR C SPARSE OUTPUT, AND LOW ORDER FORMULAE (E.G. 5(4)) FOR DENSE OUTPUT. 350 CALL SLV(RHS, NZ, Z, T0, T1, RE, AE, IFLAG, W, IW, X, IV) IF (ONESTP) GO TO 430 C INTEGRATION OUTPUT FLAG OK FOR CONTINUATION ? IF (ABS(IFLAG).EQ.2) GO TO 450 J = 1 DO 360 K=1,NU W(LW-K) = SPLINT (IV(NU+K), IV(K), X(N1+J), X(J), * X(N2+J), X(N3+J), X(N4+J), X(N5+J), T0) J = J + IV(K) 360 CONTINUE J = -IFLAG WRITE (IS, 9690) J WRITE (IS, 9780) T0, T1, T, TF, Y(I) WRITE (IS, 9780) (Z(J),J=1,NZ) WRITE (IS, 9780) (W(LW-J),J=1,NU) GO TO (420, 450, 370, 380, 390, 400, 410, 420), IFLAG 370 WRITE (IS, 9700) GO TO 350 380 WRITE (IS, 9710) GO TO 350 390 WRITE (IS, 9720) AE = RTEPS GO TO 350 400 WRITE (IS, 9730) AE = TEN*AE RE = TEN*RE IFLAG = 2 GO TO 350 410 WRITE (IS, 9740) ONESTP = .TRUE. GO TO 280 420 WRITE (IS, 9750) PAUSE * 'PAUSE: YOU MAY INVOKE A DISPLAY HERE BEFORE STOP' STOP C IN ONE-STEP-INTEGRATION-MODE PRINT STATES AND CONTROLS 430 J = 1 DO 440 K=1,NU W(LW-K) = SPLINT (IV(NU+K), IV(K), X(N1+J), X(J), * X(N2+J), X(N3+J), X(N4+J), X(N5+J), T0) J = J + IV(K) 440 CONTINUE WRITE (IS, 9780) T0, T1, (Z(J),J=1,NZ), * (W(LW-J), J=1,NU) IF (IFLAG.EQ.(-2)) GO TO 350 IFLAG = -2 C COMMUNICATION POINT REACHED ? 450 IF (T1.LT.Y(I)) GO TO 310 C CALLS BOUNDARY VALUES, TRAJECTORY CONSTRAINTS, AND COST FUNCTION C OF PERTURBED TRAJECTORY CALL VALUES (RHS, IV, T0, X, Y, Z, Z0, Z1, W, E, W(NW+1), I) 460 CONTINUE IFC = IFC + IW(1) C FORWARD DIFFERENCES APPROXIMATE THE GRADIENT DF(KP) = (E-F)/DEL DO 470 I=1,M DC(I,KP) = (W(NW+I)-C(I))/DEL 470 CONTINUE GO TO 500 C GRADIENT FOR INITIAL AND FINAL BREAKPOINT OF A CONTROL SEQUENCE 480 DF(KP) = ZERO DO 490 I=1,M DC(I,KP) = ZERO 490 CONTINUE C RESET PERTURBATION 500 X(KP) = H IF (L.GT.N) GO TO 505 IF (L.NE.O) GO TO 505 C EVALUATE SPLINE AFTER RESETTING OF PERTURBATION FOR LAST C BREAKPOINT OF A CONTROL FUNCTION ONLY CALL SPLINE (NK, NI, X(N1+NJ), X( NJ), X(N2+NJ), * X(N3+NJ), X(N4+NJ), X(N5+NJ)) NJ = NJ + NI IG = IG + 1 505 IF (L.GT.N .AND. L.LE.N1) * CALL VALUES (RHS, IV, T0, X, Y, Z, Z0, Z1, W, F, C, 0) 510 CONTINUE 520 CONTINUE GO TO 600 C----------------------------------------------------------------------- C GRAPH VIA PLODAT & PLOTHP FROM R A S P C OR GRAPH VIA M A T L A B PLOTTING FACILITY C----------------------------------------------------------------------- 530 IF (MATLAB) L = LONG1 IF (RASP) L = LONG T1 = Y(NY) C INTEGRATION WITH FIXED STEP SIZE H H = (T1-T0)/DFLOAT(L-1) T = T0 TANF = SNGL(T0) TEND = SNGL(T1) HSP = SNGL(H) IF (NP .GT. 0) THEN TEND = SNGL(X(N1)*T1) HSP = SNGL(H)*TEND END IF K = 1 C STORE INITIAL STATES DO 535 I=1,NZ E = Z(I) G(K,I) = SNGL(E) 535 CONTINUE C STORE INITIAL CONTROLS J = 1 DO 540 I=1,NU E = X(J) G(K,NZ+I) = SNGL(E) J = J + IV(I) 540 CONTINUE NO = NU + NZ C THE TRAJECTORIES CAN BE DISPLAYED IN USER SUPPLIED SUBROUTINE C OUTPUT ON ANY DEVICE (HERE FOR INITIAL VALUES AT T0) C NO MAY BE CHANGED IN OUTPUT TO CATER FOR ADDITIONAL FUNCTIONS C WHICH ARE NEITHER STATES NOR CONTROLS CALL OUTPUT(T, K, G, LONG, NO, Z, X, IV) NO = MIN(NO, BROAD) C ENTRY POINT FOR INTEGRATION LOOP 545 K = K + 1 TF = DFLOAT(K)*EPMACH C EULER-CAUCHY (SECOND-ORDER RUNGE-KUTTA) FORMULAE C ARE USED FOR STATE INTEGRATION WITH C EXTREMELY DENSE OUTPUT (IN-LINE CODED) DO 550 I=1,NZ W(I) = Z(I) 550 CONTINUE CALL RHS (T, Z, W(NZ+1), X, IV) DO 555 I=1,NZ Z(I) = Z(I) + H*W(NZ+I) 555 CONTINUE T = T + H CALL RHS (T, Z, W(NZ+NZ+1), X, IV) E = 0.5D0*H DO 560 I=1,NZ Z(I) = W(I) + E*(W(NZ+I)+W(NZ+NZ+I)) 560 CONTINUE C STORE STATES DO 565 I=1,NZ E = Z(I) G(K,I) = SNGL(E) 565 CONTINUE C STORE CONTROLS J = 1 DO 570 I=1,NU E = SPLINT (IV(NU+I), IV(I), X(N1+J), X(J), * X(N2+J), X(N3+J), X(N4+J), X(N5+J), T) G(K,NZ+I) = SNGL(E) J = J + IV(I) 570 CONTINUE C THE TRAJECTORIES CAN BE DISPLAYED IN USER SUPPLIED SUBROUTINE C OUTPUT ON ANY DEVICE CALL OUTPUT(T, K, G, LONG, IDUMMY, Z, X, IV) IF ((T+H).GT.T1) H = T1 - T C HAS THE ENDPOINT BEEN REACHED? IF (K.LT.L) GO TO 545 C IN CASE OF UNCERTAINTY PRINT NAMELIST AS LOCAL DUMP * IF (IPL.LT.10) WRITE (IPL, NXTOMP) C IS R A S P AVAILABLE ? IF (IPL.LT.10) GO TO 600 C BY CALLING PLODAT C STORE DATA ON FILE IPL IN R A S P - FORMAT C FOR PLOTTING IN SEPERATE JOB USING PLOTHP C ARRAY G MUST BE DECLARED SINGLE PRECISION WITHIN PLODAT C MS-FORTRAN & RYAN-MCFARLAND TIME & DATE * CALL GETTIM (IHR, IMIN, ISEC, IHUN) * CALL GETDAT (IYR, IMON, IDAY) C OPEN ONLY ONCE AT THE BEGINNING * OPEN (UNIT=IPL, FILE='TOMP.DAT') IF (RASP) THEN J = IPLOT*NO DO 580 I=1,NO NUMBER = J + I WRITE (IPL, 9790) IDAY, IMON, IYR, IHR, IMIN, ISEC, IHUN WRITE (IPL, 9800) NUMBER WRITE (IPL, 9810) TANF, HSP, K WRITE (IPL, 9820) (G(L,I), L=1,K) 580 CONTINUE ELSE IF (MATLAB) THEN C MATLAB METACOMMANDS TO BE WRITTEN INTO TOMP.M-FILE J = IPLOT*NO DO 590 I=1,NO NUMBER = J + I IF (NUMBER .LE. 9) THEN WRITE (IPL, 9910) NUMBER ELSE WRITE (IPL, 9920) NUMBER END IF WRITE (IPL, 9930) (G(L,I), L=1,K) WRITE (IPL, 9940) IF (NUMBER .LE. 9) THEN WRITE (IPL, 9950) NUMBER, NUMBER ELSE WRITE (IPL, 9960) NUMBER, NUMBER END IF 590 CONTINUE WRITE (IPL, 9970) TANF, HSP, TEND WRITE (IPL, 9971) WRITE (IPL, 9972) WRITE (IPL, 9973) WRITE (IPL, 9974) HSP WRITE (IPL, 9975) DO 595 I=1,NO NUMBER = J + I IF (NUMBER .LE. 9) THEN WRITE (IPL, 9980) NUMBER ELSE WRITE (IPL, 9990) NUMBER END IF 595 CONTINUE ELSE WRITE (*,*) '==> NO PLOTTING PROVIDED IN TOMP' END IF C CLOSE (UNIT=IPL) C COUNT UP TRAJECTORY NUMBER INDICATOR FOR NEXT GRAPHING IPLOT = IPLOT + 1 C----------------------------------------------------------------------- 600 CONTINUE IV(20) = IFC C END TOMP C----------------------------------------------------------------------- C FORMAT STATEMENTS 9680 FORMAT (39H INTERRUPT INTEGRATION WITH F = FLARGE) 9690 FORMAT (25H0 PROBLEMS IN RK> FLAG IS, I2, 19H; T's, Z, U ARE :) 9700 FORMAT (41H TOLERANCES NOT APPROPRIATE; RESET BY RK) 9710 FORMAT (46H 500 STEPS TAKEN; TRY ANOTHER 500, THEN STOP.) 9720 FORMAT (46H VANISHING SOLUTION NEEDS ABSOLUTE ERROR TEST) 9730 FORMAT (43H TOLERANCES NOT APPROPRIATE; RESET BY TOMP) 9740 FORMAT (52H TOO MUCH OUTPUT RESTRICTS NATURAL STEP-SIZE; CONTI, * 24HNUATION IN ONE-STEP MODE) 9750 FORMAT (39H0 TOMP: IMPROPER INPUT PARAMETERS; STOP) 9760 FORMAT (52H1 IN ONE-STEP-MODE T, Z & U HAVE THE FOLLOWING VALUE, * 26HS FOR BASIC TRAJECTORIES :) 9770 FORMAT (52H1 IN ONE-STEP-MODE T, Z & U HAVE THE FOLLOWING VALUE, * 30HS FOR PERTURBED TRAJECTORIES :) 9780 FORMAT (3(D24.16)) 9790 FORMAT (16HC DATE: , I2.2, 1H/, I2.2, 1H/, I4.4, * 23H TIME: , * I2.2, 1H:, I2.2, 1H:, I2.2, 1H., I2.2) 9800 FORMAT (1HC/40HC INITIAL TIME STEP SIZE , * 20H NUMBER /1HT, I2) 9810 FORMAT (2(8X, 1PE12.5), 10X, I10) 9820 FORMAT (6(1PE12.5)) 9830 FORMAT (41H TOMP: WRONG NUMBER OF DESIGN PARAMETERS:, I3) 9840 FORMAT (41H TOMP: WRONG NUMBER OF CONTROL VARIABLES:, I3) 9850 FORMAT (44H TOMP: WRONG NUMBER OF COMMUNICATION POINTS:, I3) 9860 FORMAT (39H TOMP: WRONG NUMBER OF STATE VARIABLES:, I3) 9870 FORMAT (41H TOMP: WRONG NUMBER OF OPTIMIZATION MODE:, I3) 9880 FORMAT (42H TOMP: WRONG NUMBER OF INTEGRATION SWITCH:, I3) 9890 FORMAT (42H TOMP: ENLARGE AUXILIARY ARRAY G TO len_G:, I3) 9900 FORMAT (42H TOMP: ENLARGE AUXILIARY ARRAY W TO len_W:, I3) 9902 FORMAT (39H TOMP: INCREASE BREAKPOINTS OF CONTROL:, I3) 9904 FORMAT (43H TOMP: WRONG INTERPOLATION MODE OF CONTROL:, I3) 9906 FORMAT (41H TOMP: INITIAL AND/OR FINAL POINT DEVIATE) C FORMAT STATEMENTS FOR MATLAB META-COMMANDS 9910 FORMAT (1Ho,I1,2H=[) 9920 FORMAT (1Ho,I2,2H=[) 9930 FORMAT (5(1PE14.6)) 9940 FORMAT (3H]';) 9950 FORMAT (1Hx,I1,2H=o,I1,4H(:);) 9960 FORMAT (1Hx,I2,2H=o,I2,4H(:);) 9970 FORMAT (2Ht=,1PE14.7,1H:,1PE14.7,1H:,1PE14.7,1H;) 9971 FORMAT (12Hi=length(t);) 9972 FORMAT (13Hj=length(x1);) 9973 FORMAT (8Hif i < j) 9974 FORMAT (13Ht(j) = t(i) +,1PE14.7,1H;) 9975 FORMAT (3Hend) 9980 FORMAT (7Hclear o,I1) 9990 FORMAT (7Hclear o,I2) C----------------------------------------------------------------------- END SUBROUTINE CONTRL (T,X,IV,Y) C COMPLEMENT TO SUBROUTINE D_TOMP: C EVALUATES Y FOR GIVEN T,X,IV C USED TO EVALUATE BASIS FUNCTION C IN COMBINATION WITH D_TOMP AND SPLINE C CODED: DIETER KRAFT, 17.02.89 INTEGER IV(20),N,N1,N2,N3,N4,N5,NP,NU,I,J DOUBLE PRECISION T,X(*),Y(*),SPLINT C SUM UP TOTAL NUMBER N OF SPLINE-BREAKPOINTS N = 0 NP = IV(13) NU = IV(14) DO 10 I=1,NU N = N + IV(I) 10 CONTINUE C CALCULATE ADDRESSES IN ARRAY X N1 = N + NP N2 = N + N1 N3 = N + N2 N4 = N + N3 N5 = N + N4 C EVALUATE CONTROL VECTOR Y J = 1 DO 20 I=1,NU Y(I)=SPLINT (IV(NU+I), IV(I), X(N1+J), X( J), * X(N2+J), X(N3+J), X(N4+J), X(N5+J), T) J = J + IV(I) 20 CONTINUE END SUBROUTINE SPLINE (MODE, N, X, Y, P, A, B, C) C SPLINE CALCULATES COEFFICIENT SETS FOR LINEAR, CUBIC OR EXPONENTIAL C INTERPOLATING SPLINES C PURPOSE: C CALCULATION OF COEFFICIENT SETS FOR LINEAR, CUBIC, OR EXPONENTIAL C INTERPOLATING SPLINES. C ALSO TENSION PARAMETERS FOR VISUALLY PLEASING EXPONENTIAL SPLINE C INTERPOLANTS ARE CALCULATED. C TO BE USED FOR INTERPOLATION IN CONNECTION WITH FUNCTION SPLINT C INPUT ARGUMENTS: C MODE : INDICATES ORDER OF INTERPOLATION: C 1 = PIECEWISE CONSTANT C 2 = PIECEWISE LINEAR C 3 = PIECEWISE CUBIC SPLINE C 4 = PIECEWISE EXPONENTIAL SPLINE C WITH TENSIONS A PRIORI CALCULATED C 5 = PIECEWISE EXPONENTIAL SPLINE C WITH TENSIONS GIVEN BY USER C 6 = AKIMA'S INTERPOLANT C N : NUMBER OF DATA FOR WHICH THE COEFFICIENT SET IS TO BE C CALCULATED C X : VECTOR OF LENGTH N STORING THE ABSCISSAE C Y : VECTOR OF LENGTH N STORING THE DATA POINTS C P : VECTOR OF LENGTH N STORING THE STIFFNESS PARAMETERS C OUTPUT ARGUMENTS: C A,B,C : VECTOR OF LENGTH N EACH STORING THE COEFFICIENT SET C TO BE USED FOR INTERPOLATION IN FUNCTION SPLINT. C THESE TOGETHER WITH X,Y AND P ARE INPUT ARGUMENTS TO C SPLINT AND MAY NOT BE CHANGED BY THE USER BETWEEN CALLS. C REMARK: C A PRACTICAL WAY TO FIND SUITABLE STIFFNESS PARAMETERS C IS DESCRIBED IN /2/ AND IS IMPLEMENTED IN SUBROUTINE GENERA C METHOD: C THAT OF CHR. REINSCH AND P. RENTROP AS DESCRIBED IN: C /1/ BULIRSCH,R.,RUTISHAUSER,H.: INTERPOLATION UND GENAEHERTE C QUADRATUR. IN:SAUER,R.,SZABO,I.(EDS.) MATHEMATISCHE HILFS- C MITTEL DES INGENIEURS,VOL.III. BERLIN-HEIDELBERG-NEW YORK: C SPRINGER, 1968. C /2/ RENTROP,P.: AN ALGORITHM FOR THE COMPUTATION OF THE C EXPONENTIAL SPLINE. NUMER. MATH. 35 (1980) 81-93. C /3/ RENTROP,P.,WEVER,U.: THEORY AND APPLICATION OF THE C EXPONENTIAL SPLINE. REP. 282, MATH. INST. TU M€NCHEN, 1991. C /4/ AKIMA, H: A NEW METHOD OF INTERPOLATION AND SMOOTH CURVE FITTING C BASED ON LOCAL PROCEDURES. J. ACM 17 (1970) 589-602. C IMPLEMENTED BY: C KRAFT,D., DLR - INSTITUT FUER DYNAMIK DER FLUGSYSTEME C D-8031 OBERPFAFFENHOFEN C COPYRIGHT : C ************** D L R ************** C ************** 1 9 8 6 ************** C ************** F H M ************** C ************** 1 9 9 1 ************** C CHANGES: C INCORPORATION OF ZERO-ORDER SPLINES C TOGETHER WITH PARAMETER MODE FOR SPLINE SELECTION C 31. JANUAR 1986 C INCORPORATION OF AKIMA'S INTERPOLANT C 14. JUNI 1989 C INCORPORATION OF VISUALLY PLEASING INTERPOLANT C OF RENTROP & WEVER C 16. JUNI 1991 C STATUS: 16. JUNI 1991 C SUBROUTINES REQUIRED: NONE INTEGER .I,IBACK,IS,MODE,N DOUBLE PRECISION .X(N),Y(N),P(N),A(N),B(N),C(N),C1,C2,D1,D2,H,HP,U,V,W,Z, .PHI,ZERO,ONE,TWO,THREE,HALF,QUART,A1,A2,A3,A4,UMAX DATA .ZERO/0D0/,ONE/1D0/,TWO/2D0/,THREE/3D0/,HALF/.5D0/,QUART/.25D0/, .A1/.166666666657193D+0/, A2/.833333363787823D-2/, .A3/.198409277128940D-3/, A4/.277139911687000D-5/, .UMAX/2.5D+01/,IS/6/ C** AUXILIARY FUNCTION PHI WITH BEST APPROXIMATION FOR SINH ** PHI(Z) = A1+(A2+(A3+A4*Z)*Z)*Z C** Is MODE in it's range? ** if (MODE .lt. 1 .or. MODE .gt. 6) then write (IS,*) 'SPLINE: MODE not in its range; MODE=', MODE stop 'STOP SPLINE 01' endif C** DO YOU HAVE MONOTONE ABSCISSAE? ** DO 10 I=1,N-1 IF (X(I) .ge. X(I+1)) GOTO 20 10 CONTINUE GOTO 40 20 DO 30 I=1,N-1 IF (X(I) .le. X(I+1)) Then WRITE (IS,*) . 'SPLINE: non-monotonic abscissae:', X(I), X(I+1) STOP 'STOP SPLINE 02' ENDIF 30 CONTINUE C** CHOOSE INTERPOLATION ORDER ** 40 GO TO (50, 60, 80, 130, 138, 180), MODE 50 GOTO 220 C** LINEAR SPLINE COEFFICIENTS ** 60 DO 70 I=1,N-1 70 A(I) = (Y(I+1)-Y(I))/(X(I+1)-X(I)) GOTO 220 C** CUBIC SPLINE COEFFICIENTS ** C** COMPUTATION OF THE ELEMENTS OF THE TRIDIAGONAL SYSTEM ** 80 V = ZERO DO 90 I = 1,N-1 C(I) = X(I+1)-X(I) U = (Y(I+1)-Y(I))/C(I) B(I) = U-V 90 V = U U = ZERO V = U B(1) = V DO 100 I = 2,N-1 B(I) = B(I)+U*B(I-1) A(I) = TWO*(X(I-1)-X(I+1))-U*V V = C(I) 100 U = V/A(I) C** SOLUTION OF THE TRIDIAGONAL SYSTEM ** A(N) = ZERO B(N) = ZERO C(N) = ZERO DO 110 I = 2,N-1 IBACK = N+1-I 110 B(IBACK) = (C(IBACK)*B(IBACK+1)-B(IBACK))/A(IBACK) DO 120 I = 1,N-1 V = C(I) U = B(I+1)-B(I) C(I) = U/V B(I) = THREE*B(I) 120 A(I) = (Y(I+1)-Y(I))/V-(B(I)+U)*V GOTO 220 C** EXPONENTIAL SPLINE COEFFICIENTS ** C** A PRIORI COMPUTATION OF THE TENSION PARAMETERS ** 130 DO 132 I = 1,N-1 132 C(I) = X(I+1)-X(I) DO 134 I = 2,N-1 134 B(I) = (Y(I+1)-Y(I))/C(I)-(Y(I)-Y(I-1))/C(I-1) DO 135 I = 2,N-2 IF (B(I)*B(I+1) .EQ. ZERO) THEN C ABNORMAL CASE A(I) = UMAX ELSE IF (B(I)*B(I+1) .LT. ZERO) THEN C MONOTONICITY IF (Y(I+1) .EQ. Y(I)) THEN A(I) = UMAX ELSE A(I) = C(I)*ABS((B(I+1)-B(I))/(Y(I+1)-Y(I))) END IF ELSE C CONVEXITY H = ABS(B(I)/B(I+1)) A(I) = MAX(H,ONE/H) END IF A(I) = MIN(UMAX,A(I)) IF (A(I) .LT. THREE) A(I) = ZERO 135 CONTINUE A(1) = A(2) A(N-1) = A(N-2) DO 136 I = 1,N-1 136 P(I) = A(I)/C(I) C** COMPUTATION OF THE ELEMENTS OF THE TRIDIAGONAL SYSTEM ** 138 U = Y(1) DO 140 I = 2,N V = Y(I) H = X(I)-X(I-1) C(I) = (V-U)/H U = V HP = H*P(I-1) IF (HP.GT.HALF) THEN D1 = EXP(-HP) D2 = D1*D1 W = ONE-D2 C1 = W*HP C2 = W/HP W = H/C1 A(I) = (ONE-C2+D2)*W B(I) = (C2-TWO*D1)*W ELSE HP = HP*HP C1 = PHI(HP) W = H/(ONE+HP*C1) HP = QUART*HP C2 = ONE+HP*PHI(HP) A(I) = (HALF*C2*C2-C1)*W B(I) = C1*W ENDIF 140 CONTINUE C** GENERATE THE TRIDIAGONAL SYSTEM ** U = ZERO C(1) = U DO 150 I = 2,N-1 A(I) = A(I)+A(I+1)-U*B(I) C(I) = C(I+1)-C(I)-U*C(I-1) 150 U = B(I+1)/A(I) C** SOLUTION OF THE TRIDIAGONAL SYSTEM ** C(N) = ZERO DO 160 I = 2,N-1 IBACK = N+1-I 160 C(IBACK) = (C(IBACK)-B(IBACK+1)*C(IBACK+1))/A(IBACK) C** STORE AUXILIARY TERMS FOR SPLINT & DSPLNT IN A & B ** DO 170 I = 1,N-1 H = X(I+1)-X(I) HP = H*P(I) IF(HP.GT.HALF) THEN D1 = EXP(-HP) A(I) = ONE/(ONE-D1*D1) B(I) = ONE/(P(I)*P(I)) ELSE HP = HP*HP D1 = PHI(HP) D2 = H*H/(ONE+HP*D1) A(I) = D2 B(I) = D1 ENDIF 170 CONTINUE GOTO 220 C** AKIMA SPLINE COEFFICIENTS ** 180 DO 190 I=1,N-1 190 P(I) = (Y(I+1)-Y(I))/(X(I+1)-X(I)) C** SLOPES AT NODES ** DO 200 I=3,N-2 IF (P(I-2).EQ.P(I-1).AND.P(I).EQ.P(I+1)) THEN IF (P(I-1).NE.P(I)) THEN A(I) = HALF*(P(I-1)+P(I)) ELSE A(I) = P(I) ENDIF ELSE U=ABS(P(I+1)-P(I)) V=ABS(P(I-1)-P(I-2)) A(I) = (U*P(I-1)+V*P(I))/(U+V) ENDIF 200 CONTINUE C** BACKWARD EXTRAPOLATION ** U = X(1)+X(2)-X(3) H = U-X(1) C1 = Y(1)+H*(P(1)+P(1)-P(2)) D1 = (C1-Y(1))/H V = U+X(1)-X(2) H = V-U C2 = C1+H*(D1+D1-P(1)) D2 = (C2-C1)/H IF (D2.EQ.D1.AND.P(1).EQ.P(2)) THEN IF (D1.NE.P(1)) THEN A(1) = HALF*(D1+P(1)) ELSE A(1) = P(1) ENDIF ELSE U = ABS(P(2)-P(1)) V = ABS(D1-D2) A(1) = (U*D1+V*P(1))/(U+V) ENDIF IF (D1.EQ.P(1).AND.P(2).EQ.P(3)) THEN IF (P(1).NE.P(2)) THEN A(2) = HALF*(P(1)+P(2)) ELSE A(2) = P(2) ENDIF ELSE U = ABS(P(3)-P(2)) V = ABS(P(1)-D1) A(2) = (U*P(1)+V*P(2))/(U+V) ENDIF C** FORWARD EXTRAPOLATION ** U = X(N)+X(N-1)-X(N-2) H = U-X(N) C1 = Y(N)+H*(P(N-1)+P(N-1)-P(N-2)) D1 = (C1-Y(N))/H V = U+X(N)-X(N-1) H = V-U C2 = C1+H*(D1+D1-P(N-1)) D2 = (C2-C1)/H IF (P(N-3).EQ.P(N-2).AND.P(N-1).EQ.D1) THEN IF (P(N-2).NE.P(N-1)) THEN A(N-1) = HALF*(P(N-2)+P(N-1)) ELSE A(N-1) = P(N-1) ENDIF ELSE U = ABS(P(N-1)-D1) V = ABS(P(N-2)-P(N-3)) A(N-1) = (U*P(N-2)+V*P(N-1))/(U+V) ENDIF IF (P(N-2).EQ.P(N-1).AND.D1.EQ.D2) THEN IF (P(N-1).NE.D1) THEN A(N) = HALF*(P(N-1)+D1) ELSE A(N) = D1 ENDIF ELSE U = ABS(D1-D2) V = ABS(P(N-1)-P(N-2)) A(N) = (U*P(N-1)+V*D1)/(U+V) ENDIF DO 210 I=1,N-1 H = X(I+1)-X(I) B(I) = (P(I)+P(I)+P(I)-A(I)-A(I)-A(I+1))/H 210 C(I) = (A(I)+A(I+1)-P(I)-P(I))/(H*H) 220 RETURN C END OF SPLINE END DOUBLE PRECISION FUNCTION SPLINT (MODE,N,X,Y,P,A,B,C,XS) C SPLINT INTERPOLATES IN THE RANGE OF COEFFICIENT SETS FOR CUBIC C OR EXPONENTIAL SPLINES ESTABLISHED IN SUBROUTINE SPLINE C PURPOSE: C INTERPOLATION WITHIN COEFFICIENT SETS FOR CUBIC OR EXPONENTIAL C SPLINES C INPUT ARGUMENTS: C MODE : INDICATES ORDER OF INTERPOLATION: C 1 = PIECEWISE CONSTANT C 2 = PIECEWISE LINEAR C 3 = PIECEWISE CUBIC SPLINE C 4 = PIECEWISE EXPONENTIAL SPLINE C WITH TENSIONS A PRIORI CALCULATED C 5 = PIECEWISE EXPONENTIAL SPLINE C WITH TENSIONS GIVEN BY USER C 6 = AKIMA'S INTERPOLANT C N : NUMBER OF DATA FOR WHICH THE COEFFICIENT SET HAS BEEN C CALCULATED IN SUBROUTINE SPLINE PREVIOUSLY C X : VECTOR OF LENGTH N STORING THE ABSCISSAE C Y : VECTOR OF LENGTH N STORING THE DATA POINTS C P : VECTOR OF LENGTH N STORING THE STIFFNESS PARAMETERS C A,B,C : VECTOR OF LENGTH N EACH STORING THE COEFFICIENT SET C XS : ABSCISSA AT WHICH INTERPOLATION IS REQUIRED C OUTPUT ARGUMENTS: C SPLINT: VALUE OF THE INTERPOLATING ORDINATE AT ABSCISSA XS C METHOD: C THAT OF CHR. REINSCH, P. RENTROP & D. KRAFT AS DESCRIBED IN: C /1/ BULIRSCH,R.,RUTISHAUSER,H.: INTERPOLATION UND GENAEHERTE C QUADRATUR. IN:SAUER,R.,SZABO,I.(EDS.) MATHEMATISCHE HILFS- C MITTEL DES INGENIEURS,VOL.III. BERLIN-HEIDELBERG-NEW YORK: C SPRINGER, 1968. C /2/ RENTROP,P.: AN ALGORITHM FOR THE COMPUTATION OF THE C EXPONENTIAL SPLINE. NUMER. MATH. 35 (1980) 81-93. C /3/ KRAFT,D.: FIRST DERIVATIVES OF EXPONENTIAL SPLINES. C UNPUBLISHED MANUSCRIPT (1984). C /4/ AKIMA, H: A NEW METHOD OF INTERPOLATION AND SMOOTH CURVE FITTING C BASED ON LOCAL PROCEDURES. J. ACM 17 (1970) 589-602. C IMPLEMENTED BY: C KRAFT,D., DLR - INSTITUT FUER DYNAMIK DER FLUGSYSTEME C D-8031 OBERPFAFFENHOFEN C STATUS: 14. JUNI 1989 C SUBROUTINES REQUIRED: * = DIRECT CALL C * LEFT INTEGER .IP,IS,LEFT,MFLAG,MODE,N DOUBLE PRECISION .X(N),Y(N),P(N),A(N),B(N),C(N),H,U,V,Z,D1,D2,HP,XS,XT, .PHI,DPHI,ZERO,ONE,HALF,A1,A2,A3,A4,DSPLNT DATA ZERO/0.D0/,ONE/1.D0/,HALF/.5D0/, .A1/.166666666657193D+0/, A2/.833333363787823D-2/, .A3/.198409277128940D-3/, A4/.277139911687000D-5/ C** AUXILIARY FUNCTION PHI WITH BEST APPROXIMATION FOR SINH ** PHI(Z) = A1+(A2+(A3+A4*Z)*Z)*Z DPHI(Z) = (Z+Z)*(A2+Z*Z*(A3+A3+(A4+A4+A4)*Z*Z)) C** FIND INDEX OF ABSCISSA WHICH LIES IMMEDIATELY LEFT OF XS ** IS = LEFT(N,X,XS,MFLAG) XT = XS IF(MFLAG.EQ.-1) XT = X(1) IF(MFLAG.EQ. 1) XT = X(N) C** CHOOSE INTERPOLATION ORDER ** GO TO (10, 15, 20, 25, 25, 20), MODE C** CONSTANT SPLINE ** 10 IF(MFLAG.EQ. 1) IS = IS-1 SPLINT = Y(IS) GOTO 70 C** LINEAR SPLINE ** 15 SPLINT = Y(IS) U = XT-X(IS) IF(U.EQ.ZERO) GOTO 70 SPLINT = SPLINT+A(IS)*U GOTO 70 C** CUBIC SPLINE & AKIMA SPLINE ** 20 SPLINT = Y(IS) U = XT-X(IS) IF(U.EQ.ZERO) GOTO 70 SPLINT = SPLINT+(A(IS)+(B(IS)+C(IS)*U)*U)*U GOTO 70 C** EXPONENTIAL SPLINE ** 25 SPLINT = Y(IS) U = XT-X(IS) IF(U.EQ.ZERO) GOTO 70 IP = IS+1 H = X(IP)-X(IS) U = U/H V = ONE-U HP = H*P(IS) IF(HP.LE.HALF) GOTO 30 D1 = EXP(-HP*U) D2 = EXP(-HP*V) SPLINT = U*Y(IP)+(A(IS)*D2*(ONE-D1*D1)-U)*B(IS)*C(IP) . + V*Y(IS)+(A(IS)*D1*(ONE-D2*D2)-V)*B(IS)*C(IS) GOTO 70 30 HP = HP*HP D1 = U*U D2 = V*V SPLINT = U*(Y(IP)+A(IS)*C(IP)*(D1*PHI(HP*D1)-B(IS))) . + V*(Y(IS)+A(IS)*C(IS)*(D2*PHI(HP*D2)-B(IS))) GOTO 70 C END OF SPLINT ENTRY DSPLNT (N,X,Y,P,A,B,C,XS) C** FIRST DERIVATIVE OF SPLINE FUNCTION ** IS = LEFT(N,X,XS,MFLAG) XT = XS IF(MFLAG.EQ.-1) XT = X(1) IF(MFLAG.EQ. 1) XT = X(N) IF(MFLAG.EQ. 1) IS = IS-1 GO TO (40, 45, 50, 55, 55, 50), MODE C** CONSTANT SPLINE ** 40 DSPLNT = ZERO GOTO 70 C** LINEAR SPLINE ** 45 DSPLNT = A(IS) GOTO 70 C** CUBIC & AKIMA SPLINE ** 50 DSPLNT = A(IS) U = XT-X(IS) IF(U.EQ.ZERO) GOTO 70 DSPLNT = DSPLNT+(2*B(IS)+3*C(IS)*U)*U GOTO 70 C** EXPONENTIAL SPLINE ** 55 IP = IS+1 U = XT-X(IS) H = X(IP)-X(IS) U = U/H V = ONE-U HP = H*P(IS) IF(HP.LE.HALF) GOTO 60 D1 = EXP(-HP*U) D2 = EXP(-HP*V) DSPLNT = Y(IP)+(A(IS)*HP*D2*(ONE+D1*D1)-ONE)*B(IS)*C(IP) . - Y(IS)-(A(IS)*HP*D1*(ONE+D2*D2)-ONE)*B(IS)*C(IS) GOTO 65 60 D1 = U*U D2 = V*V U = HP*U V = HP*V HP = HP*HP DSPLNT = Y(IP)+A(IS)*C(IP)*(D1*(3*PHI(HP*D1)+U*DPHI(U))-B(IS)) . - Y(IS)-A(IS)*C(IS)*(D2*(3*PHI(HP*D2)+V*DPHI(V))-B(IS)) 65 DSPLNT = DSPLNT/H 70 RETURN C END OF DSPLNT END SUBROUTINE GENERA(N,X,Y,P,D) C GENERA GENERATES A POSTERIORI STIFFNES PARAMETERS C FOR EXPONENTIAL SPLINES C PURPOSE: C GENERATES STIFFNES PARAMETERS FOR EXPONENTIAL SPLINES C TO BE USED FOR INTERPOLATION IN CONNECTION WITH C SUBROUTINE SPLINE AND FUNCTION SPLINT C INPUT ARGUMENTS: C N : NUMBER OF DATA FOR WHICH THE STIFFNESS PARAMETERS C ARE TO BE CALCULATED C X : VECTOR OF LENGTH N STORING THE ABSCISSAE C Y : VECTOR OF LENGTH N STORING THE DATA POINTS C D : VECTOR OF LENGTH N STORING THE SECOND DERIVATIVES OF DATA C OUTPUT ARGUMENTS: C P : VECTOR OF LENGTH N STORING THE STIFFNESS PARAMETERS C METHOD: C THAT OF P. RENTROP AS DESCRIBED IN: C RENTROP,P. (1979) AN ALGORITHM FOR THE COMPUTATION OF THE C EXPONENTIAL SPLINE. NUMER. MATH. 35 (1980) 81-93. C IMPLEMENTED BY: C KRAFT,D., DLR - INSTITUT FUER DYNAMIK DER FLUGSYSTEME C D-8031 OBERPFAFFENHOFEN C STATUS: 15. JANUARY 1980 C SUBROUTINES REQUIRED: NONE INTEGER N,I DOUBLE PRECISION X(N),Y(N),P(N),D(N),A,C,HM,YM,HI,YI,HP,YP,EPMACH DATA EPMACH/2.22D-16/ DO 10 I=2,N-2 P(I)=EPMACH A=D(I)*D(I+1) IF(A.GT.0D0 .AND. ABS(A).GT.EPMACH) GOTO 10 HM=X(I)-X(I-1) YM=Y(I)-Y(I-1) HI=X(I+1)-X(I) YI=Y(I+1)-Y(I) HP=X(I+2)-X(I+1) YP=Y(I+2)-Y(I+1) A=(HM*YI-HI*YM)/HI A=A*(HI*YP-HP*YI)/HP IF(A.LT.0D0 .AND. ABS(A).GT.EPMACH) GOTO 10 IF(ABS(Y(I)).EQ.0D0 .AND. ABS(Y(I+1)).EQ.0D0) THEN P(I)=15D0/HI ELSE C=.1D0+ABS(YI)/DMAX1(ABS(Y(I+1)),ABS(Y(I))) P(I)=(4D0+1D0/C)/HI END IF 10 CONTINUE P(1)=P(2) P(N-1)=P(N-2) RETURN C END OF GENERA END INTEGER FUNCTION LEFT(LXT,XT,X,MFLAG) C LEFT FINDS INDEX LEFT OF AN ARRAY XT FOR WHICH XT(LEFT) C LIES IMMEDIATELY LEFT OF X C PURPOSE: C FINDS INDEX LEFT OF AN ARRAY XT FOR WHICH XT(LEFT) C LIES IMMEDIATELY LEFT OF X C INPUT ARGUMENTS: C LXT : NUMBER OF ELEMENTS IN VECTOR XT C XT : VECTOR OF LENGTH LXT STORING THE ABSCISSAE C X : X-VALUE FOR WHICH THE INDEX LEFT IS TO BE FOUND C OUTPUT ARGUMENTS: C LEFT : INDEX FOR WHICH XT(LEFT) LIES IMMEDIATELY LEFT OF X C MFLAG : FLAG SET IN THE FOLLOWING MANNER C LEFT MFLAG C 1 -1 IF X .LT. XT(1) C I 0 IF XT(I) .LE. X .LT. XT(I+1) C LXT 1 IF XT(LXT) .LE. X C METHOD: C THAT OF CARL DE BOOR AS DESCRIBED ON PAGE 91 FF. IN: C /1/ DE BOOR,C. (1978) A PRACTICAL GUIDE TO SPLINES. C APPLIED MATHEMATICAL SCIENCES, VOLUME 27. C NEW-YORK-HEIDELBERG-BERLIN: SPRINGER. C IMPLEMENTED BY: C KRAFT,D., DLR - INSTITUT FUER DYNAMIK DER FLUGSYSTEME C D-8031 OBERPFAFFENHOFEN C STATUS: 15. JANUARY 1980 C SUBROUTINES REQUIRED: NONE INTEGER LXT,MFLAG,IHI,ILO,ISTEP,MIDDLE DOUBLE PRECISION X,XT(LXT) SAVE ILO DATA ILO/1/ IHI=ILO+1 IF(IHI.LT.LXT) GOTO 10 IF(X.GE.XT(LXT)) GOTO 110 IF(LXT.LE.1) GOTO 90 ILO=LXT-1 IHI=LXT 10 IF(X.GE.XT(IHI)) GOTO 40 IF(X.GE.XT(ILO)) GOTO 100 ISTEP=1 20 IHI=ILO ILO=IHI-ISTEP IF(ILO.LE.1) GOTO 30 IF(X.GE.XT(ILO)) GOTO 70 ISTEP=ISTEP+ISTEP GOTO 20 30 ILO=1 IF(X.LT.XT(1)) GOTO 90 GOTO 70 40 ISTEP=1 50 ILO=IHI IHI=ILO+ISTEP IF(IHI.GE.LXT) GOTO 60 IF(X.LT.XT(IHI)) GOTO 70 ISTEP=ISTEP+ISTEP GOTO 50 60 IF(X.GE.XT(LXT)) GOTO 110 IHI=LXT 70 MIDDLE=(ILO+IHI)/2 IF(MIDDLE.EQ.ILO) GOTO 100 IF(X.LT.XT(MIDDLE)) GOTO 80 ILO=MIDDLE GOTO 70 80 IHI=MIDDLE GOTO 70 90 MFLAG=-1 LEFT=1 GOTO 120 100 MFLAG=0 LEFT=ILO GOTO 120 110 MFLAG=1 LEFT=LXT 120 RETURN C END OF LEFT END SUBROUTINE RK54 (F,N,Y,T,TOUT,RELERR,ABSERR,IFLAG,WORK,IWORK,W,IW) C RK54 INITIAL VALUE SOLVER BASED ON HIGH ORDER RUNGE-KUTTA FORMULAE C ADAPTATION OF THE IMPLEMENTATION OF C FEHLBERG'S FOURTH-FIFTH ORDER RUNGE-KUTTA METHOD C WRITTEN BY C L.F.SHAMPINE & H.A.WATTS C SANDIA LABORATORIES C ALBUQUERQUE, NEW MEXICO C TO THE MORE ACCURATE PRINCE/DORMAND FOURTH-FIFTH ORDER FORMULAE C BY C D.KRAFT C INSTITUT FUER DYNAMIK DER FLUGSYSTEME C DLR OBERPFAFFENHOFEN C STATUS: 27. JANUARY 1985 C RK54 IS PRIMARILY DESIGNED TO SOLVE NON-STIFF AND MILDLY STIFF C DIFFERENTIAL EQUATIONS WITH MEDIUM TO HIGH ACCURACY & LOW OUTPUT C----------------------------------------------------------------------- C ABSTRACT C----------------------------------------------------------------------- C SUBROUTINE RK54 INTEGRATES A SYSTEM OF N FIRST ORDER C ORDINARY DIFFERENTIAL EQUATIONS OF THE FORM C DY(I)/DT = F(T,Y(1),Y(2),...,Y(N)) C WHERE THE Y(I) ARE GIVEN AT T . C TYPICALLY THE SUBROUTINE IS USED TO INTEGRATE FROM T TO TOUT BUT C IT CAN BE USED AS A ONE-STEP INTEGRATOR TO ADVANCE THE SOLUTION A C SINGLE STEP IN THE DIRECTION OF TOUT. ON RETURN THE PARAMETERS IN C THE CALL LIST ARE SET FOR CONTINUING THE INTEGRATION. THE USER HAS C ONLY TO CALL RK54 AGAIN (AND PERHAPS DEFINE A NEW VALUE FOR TOUT). C ACTUALLY, RK54 IS AN INTERFACING ROUTINE WHICH CALLS SUBROUTINE C RKC54 FOR THE SOLUTION. RKC54 IN TURN CALLS SUBROUTINE CORE WHICH C COMPUTES AN APPROXIMATE SOLUTION OVER ONE STEP. C RKF45 USES THE RUNGE-KUTTA-FEHLBERG (4,5) METHOD DESCRIBED IN C THE REFERENCES C E. FEHLBERG, LOW-ORDER CLASSICAL RUNGE-KUTTA FORMULAS WITH STEPSIZE C CONTROL, NASA TR R-315. C ALSO IN COMPUTING 6 (1970) 61-71. C L.F. SHAMPINE AND H.A. WATTS, PRACTICAL SOLUTION OF ORDINARY C DIFFERENTIAL EQUATIONS BY RUNGE-KUTTA METHODS. C SANDIA LABORATORIES REPORT SAND76-0585. C L.F. SHAMPINE AND H.A. WATTS, THE ART OF WRITING A RUNGE-KUTTA CODE C APPL. MATH. COMP. 5 (1979) 93-121. C THE PERFORMANCE OF RKF45 IS ILLUSTRATED IN THE REFERENCE C L.F. SHAMPINE, H.A. WATTS, S. DAVENPORT, SOLVING NON-STIFF ORDINARY C DIFFERENTIAL EQUATIONS-THE STATE OF THE ART. C SANDIA LABORATORIES REPORT SAND78-0182 . ALSO IN C SIAM REVIEW 18 (1976) 376-411. C RK54 USES THE RUNGE-KUTTA (5,4) METHOD DESCRIBED IN C P.J. PRINCE, J.R. DORMAND 'A FAMILY OF EMBEDED RUNGE-KUTTA FORMULAE' C J. COMP. APPL. MATH. 6 (1980) 19-26. C THE PARAMETERS REPRESENT C F -- SUBROUTINE F(T,Y,YP) TO EVALUATE DERIVATIVES YP(I)=DY(I)/DT C N -- NUMBER OF EQUATIONS TO BE INTEGRATED C Y( ) -- SOLUTION VECTOR AT T C T -- INDEPENDENT VARIABLE C TOUT -- OUTPUT POINT AT WHICH SOLUTION IS DESIRED C RELERR,ABSERR -- RELATIVE AND ABSOULTE ERROR TOLERANCES FOR LOCAL C ERROR TEST. AT EACH STEP THE CODE REQUIRES THAT C ABS(LOCAL ERROR) .LE. RELERR*ABS(Y) + ABSERR C FOR EACH COMPONENT OF THE LOCAL ERROR AND SOLUTION VECTORS C IFLAG -- INDICATOR FOR STATUS OF INTEGRATION C WORK( ) -- ARRAY TO HOLD INFORMATION INTERNAL TO RK54 WHICH IS C NECESSARY FOR SUBSEQUENT CALLS. MUST BE DIMENSIONED C AT LEAST 7*N + 3 C IWORK( ) -- INTEGER ARRAY USED TO HOLD INFORMATION INTERNAL TO C RK54 WHICH IS NECESSARY FOR SUBSEQUENT CALLS. MUST BE C DIMENSIONED AT LEAST 5 C---------------------------------------------------------------------- C FIRST CALL TO RK54 C---------------------------------------------------------------------- C THE USER MUST PROVIDE STORAGE IN HIS CALLING PROGRAM FOR THE ARRAY C IN THE CALL LIST - Y(N), WORK(3+12*N), IWORK(5), C DECLARE F IN AN EXTERNAL STATEMENT, SUPPLY SUBROUTINE F(T,Y,YP) C AND INITIALIZE THE FOLLOWING PARAMETERS C N -- NUMBER OF EQUATIONS TO BE INTEGRATED. (N .GE. 1) C Y( ) -- VECTOR OF INITIAL CONDITIONS C T -- STARTING POINT OF INTEGRATION, MUST BE A VARIABLE C T=TOUT IS ALLOWED ON THE FIRST CALL ONLY, IN WHICH CASE C RK54 RETURNS WITH IFLAG=2 IF CONTINUATION IS POSSIBLE. C RELERR,ABSERR -- RELATIVE AND ABSOLUTE LOCAL ERROR TOLERANCES C WHICH MUST BE NON-NEGATIVE. RELERR MUST BE A VARIABLE WHILE C ABSERR MAY BE A CONSTANT. THE CODE SHOULD NORMALLY NOT BE C USED WITH RELATIVE ERROR CONTROL SMALLER THAN ABOUT 1.D-11. C TO AVOID LIMITING PRECISION DIFFICULTIES THE CODE REQUIRES C RELERR TO BE LARGER THAN AN INTERNALLY COMPUTED RELATIVE C ERROR PARAMETER WHICH IS MACHINE DEPENDENT. IN PARTICULAR, C PURE ABSOLUTE ERROR IS NOT PERMITTED. IF A SMALLER THAN C ALLOWABLE VALUE OF RELERR IS ATTEMPTED, RK54 INCREASES C RELERR APPROPRIATELY AND RETURNS CONTROL TO THE USER BEFORE C CONTINUING THE INTEGRATION. C IFLAG -- +1,-1 INDICATOR TO INITIALIZE THE CODE FOR EACH NEW C PROBLEM. NORMAL INPUT IS +1. THE USER SHOULD SET IFLAG=-1 C ONLY WHEN ONE-STEP INTEGRATOR CONTROL IS ESSENTIAL. IN THIS C CASE, RK54 ATTEMPTS TO ADVANCE THE SOLUTION A SINGLE STEP C IN THE DIRECTION OF TOUT EACH TIME IT IS CALLED. SINCE THIS C MODE OF OPERATION RESULTS IN EXTRA COMPUTING OVERHEAD, IT C SHOULD BE AVOIDED UNLESS NEEDED. C----------------------------------------------------------------------- C OUTPUT FROM RK54 C----------------------------------------------------------------------- C Y( ) -- SOLUTION AT T C T -- LAST POINT REACHED IN INTEGRATION. C IFLAG = 2 -- INTEGRATION REACHED TOUT. INDICATES SUCCESSFUL RETURN C AND IS THE NORMAL MODE FOR CONTINUING INTEGRATION. C =-2 -- A SINGLE SUCCESSFUL STEP IN THE DIRECTION OF TOUT C HAS BEEN TAKEN. NORMAL MODE FOR CONTINUING C INTEGRATION ONE STEP AT A TIME. C = 3 -- INTEGRATION WAS NOT COMPLETED BECAUSE RELATIVE ERROR C TOLERANCE WAS TOO SMALL. RELERR HAS BEEN INCREASED C APPROPRIATELY FOR CONTINUING. C = 4 -- INTEGRATION WAS NOT COMPLETED BECAUSE MORE THAN C 3000 DERIVATIVE EVALUATIONS WERE NEEDED. THIS C IS APPROXIMATELY 500 STEPS C = 5 -- INTEGRATION WAS NOT COMPLETED BECAUSE SOLUTION C VANISHED MAKING A PURE RELATIVE ERROR TEST C IMPOSSIBLE. MUST USE NON-ZERO ABSERR TO CONTINUE. C USING THE ONE-STEP INTEGRATION MODE FOR STEP C IS A GOOD WAY TO PROCEED. C = 6 -- INTEGRATION WAS NOT COMPLETED BECAUSE REQUESTED C ACCURACY COULD NOT BE ACHIEVED USING SMALLEST C ALLOWABLE STEPSIZE. USER MUST INCREASE THE ERROR C TOLERANCE BEFORE CONTINUED INTEGRATION CAN BE C ATTEMPTED. C = 7 -- IT IS LIKELY THAT RK54 IS INEFFICIENT FOR SOLVING C THIS PROBLEM. TOO MUCH OUTPUT IS RESTRICTING THE C NATURAL STEPSIZE CHOICE. USE THE ONE-STEP INTEGRATOR C MODE. C = 8 -- INVALID INPUT PARAMETERS C THIS INDICATOR OCCURS IF ANY OF THE FOLLOWING IS C SATISFIED - N .LE. 0 C T=TOUT AND IFLAG .NE. +1 OR -1 C RELERR OR ABSERR .LT. 0. C IFLAG .EQ. 0 OR .LT. -2 OR .GT.8 C WORK( ),IWORK( ) -- INFORMATION WHICH IS USUALLY OF NO INTEREST C TO THE USER BUT NECESSARY FOR SUBSEQUENT CALLS C WORK(1),...,WORK(N) CONTAINS THE FIRST DERIVATIVES C OF THE SOLUTION VECTOR Y AT T. WORK(N+1) CONTAINS C THE STEP SIZE H TO BE ATTEMPTED ON THE NEXT STEP. C IWORK(1) CONTAINS THE DERIVATIVE EVALUATION COUNTER. C----------------------------------------------------------------------- C SUBSEQUENT CALLS TO RK54 C----------------------------------------------------------------------- C SUBROUTINE RK54 RETURNS WITH ALL INFORMATION NEEDED TO CONTINUE C THE INTEGRATION. IF THE INTEGRATION REACHED TOUT, THE USER NEED ONLY C DEFINE A NEW TOUT AND CALL RK54 AGAIN. IN THE ONE-STEP INTEGRATOR C MODE (IFLAG=-2) THE USER MUST KEEP IN MIND THAT EACH STEP TAKEN IS C IN THE DIRECTION OF THE CURRENT TOUT. UPON REACHING TOUT (INDICATED C BY CHANGING IFLAG TO 2), THE USER MUST THEN DEFINE A NEW TOUT AND C RESET IFLAG TO -2 TO CONTINUE IN THE ONE STEP INTEGRATOR MODE. C IF THE INTEGRATION WAS NOT COMPLETED BUT THE USER STILL WANTS TO C CONTINUE (IFLAG=3,4 CASES), HE JUST CALLS RK54 AGAIN. WITH IFLAG=3 C THE RELERR PARAMETER HAS BEEN ADJUSTED APPROPTIATELY FOR CONTINUING C THE INTEGRATION. IN THE CASE OF IFLAG=4 THE FUNCTION COUNTER WILL C BE RESET TO 0 AND ANOTHER 6000 FUNCTION EVALUATIONS ARE ALLOWED. C HOWEVER, IN THE CASE IFLAG=5, THE USER MUST FIRST ALTER THE ERROR C CRITERION TO USE A POSITIVE VALUE OF ABSERR BEFORE INTEGRATION CAN C PROCEED. IF HE DOES NOT, EXECUTION IS TERMINATED. C ALSO, IN THE CASE IFLAG=6, IT IS NECESSARY FOR THE USER TO RESET C IFLAG TO 2 (OR -2 WHEN THE ONE-STEP INTEGRATION MODE IS BEING USED) C AS WELL AS INCREASING EITHER ABSERR, RELERR OR BOTH BEFORE THE C INTEGRATION CAN BE CONTINUED. IF THIS IS NOT DONE, EXECUTION WILL C BE TERMINATED. THE OCCURRENCE OF IFLAG=6 INDICATES A TROUBLE SPOT C (SOLUTION IS CHANGING RAPIDLY, SINGULARITY MAY BE PRESENT) AND IT C OFTEN IS INADVISABLE TO CONTINUE. C IF IFLAG=7 IS ENCOUNTERED, THE USER SHOULD USE THE ONE-STEP C INTEGRATION MODE WITH THE STEPSIZE DETERMINED BY THE CODE OR C CONSIDER SWITCHING TO THE ADAMS CODES DE,STEP,INTRP. IF THE USER C INSISTS UPON CONTINUING THE INTEGRATION WITH RK54, HE MUST RESET C IFLAG TO 2 BEFORE CALLING RK54 AGAIN. OTHERWISE, EXECUTION WILL C BE TERMINATED. C IF IFLAG=8 IS OBTAINED, INTEGRATION CANNOT BE CONTINUED UNLESS C THE INVALID INPUT PARAMETERS ARE CORRECTED. C IT SHOULD BE NOTED THAT THE ARRAYS WORK,IWORK CONTAIN INFORMATION C REQUIRED FOR SUBSEQUENT INTEGRATION. ACCORDINGLY, WORK AND IWORK C SHOULD NOT BE ALTERED. C----------------------------------------------------------------------- INTEGER IFLAG, K1, K2, K3, K4, K5, K6, K7, K8, K1M, N INTEGER IWORK(5), IW(*) DOUBLE PRECISION Y(N), WORK(*), W(*), T, TOUT, ABSERR, RELERR EXTERNAL F C COMPUTE INDICES FOR THE SPLITTING OF THE WORK ARRAY K1M = N + 1 K1 = K1M + 1 K2 = K1 + N K3 = K2 + N K4 = K3 + N K5 = K4 + N K6 = K5 + N K7 = K6 + N K8 = K7 + N C----------------------------------------------------------------------- C THIS INTERFACING ROUTINE MERELY RELIEVES THE USER OF A LONG C CALLING LIST VIA THE SPLITTING APART OF TWO WORKING STORAGE C ARRAYS. IF THIS IS NOT COMPATIBLE WITH THE USERS COMPILER, C HE MUST USE RKC54 DIRECTLY. C----------------------------------------------------------------------- CALL RKC54(F,N,Y,T,TOUT,RELERR,ABSERR,IFLAG,WORK(1),WORK(K1M),WORK 1(K1),WORK(K2),WORK(K3),WORK(K4),WORK(K5),WORK(K6),WORK(K7),WORK 3(K8),WORK(K8+1),IWORK(1),IWORK(2),IWORK(3),IWORK(4),IWORK(5),W,IW) RETURN END SUBROUTINE RKC54(F,N,Y,T,TOUT,RELERR,ABSERR,IFLAG,YP,H,F1,F2,F3, 1 F4,F5,F6,F7,SAVRE,SAVAE,NFE,KOP,INIT,JFLAG,KFLAG,W,IW) C PRINCE/DORMAND 5(4) ORDER RUNGE-KUTTA METHOD C RKC54 INTEGRATES A SYSTEM OF FIRST ORDER ORDINARY DIFFERENTIAL C EQUATIONS AS DESCRIBED IN THE COMMENTS FOR RK54 . C THE ARRAYS YP,F1,....,F10 AND F11 (OF DIMENSION AT LEAST N) AND C THE VARIABLES H,SAVRE,SAVAE,NFE,KOP,INIT,JFLAG,AND KFLAG ARE USED C INTERNALLY BY THE CODE AND APPEAR IN THE CALL LIST TO ELIMINATE C LOCAL RETENTION OF VARIABLES BETWEEN CALLS. ACCORDINGLY, THEY C SHOULD NOT BE ALTERED. ITEMS OF POSSIBLE INTEREST ARE : C YP - DERIVATIVE OF SOLUTION VECTOR AT T C H - AN APPROPRIATE STEP SIZE TO BE USED FOR THE NEXT STEP C NFE- COUNTER ON THE NUMBER OF DERIVATIVE FUNCTION EVALUATIONS C----------------------------------------------------------------------- INTEGER IW(*) INTEGER IFLAG, INIT, JFLAG, K, KFLAG, KOP, 1 MAXNFE, MFLAG, N, NFE DOUBLE PRECISION Y(N),YP(N),F1(N),F2(N),F3(N),F4(N),F5(N),F6(N), 1 F7(N),W(*),A,ABSERR,AE,ALF,TOL,TOLN,YPK, 2 DT,EE,EEOET,ESTOLD,ESTTOL,ET,H,HMIN,HOLD, 3 RELERR,REMIN,RE,S,SAVAE,SAVRE,SCALE,T,TOUT,U,U26 LOGICAL FIRST,HFAILD,OUTPUT EXTERNAL F C----------------------------------------------------------------------- C THE COMPUTER UNIT ROUND OFF ERROR U IS THE SMALLEST POSITIVE VALUE C REPRESENTABLE IN THE MACHINE SUCH THAT 1.+U .GT. 1. C VALUES TO BE USED ARE C U = 9.5D-7 FOR IBM 360/370 C U = 1.5D-8 FOR UNIVAC 1108 C U = 7.5D-9 FOR PDP-10 C U = 7.1D-15 FOR CDC 6000 SERIES C U = 2.2D-16 FOR IBM 360/370 DOUBLE PRECISION C U = 2.2D-16 FOR IBM PS/2 MODEL 70 D.P. DATA U /2.22D-16/ C----------------------------------------------------------------------- C REMIN IS A TOLERANCE THRESHOLD WHICH IS ALSO DETERMINED BY THE C INTEGRATION METHOD. DATA REMIN /1.D-14/ C----------------------------------------------------------------------- DATA MAXNFE /3000/ C CHECK INPUT PARAMETERS IF (N .LT. 1) GO TO 10 IF (RELERR .LT. 0.D0 .OR. ABSERR .LT. 0.D0) GO TO 10 MFLAG = ABS(IFLAG) IF (MFLAG .GE. 1 .AND. MFLAG .LE. 8) GO TO 50 C INVALID INPUT 10 IFLAG = 8 GO TO 290 C IS THIS THE FIRST CALL ? 50 IF (MFLAG .EQ. 1) GO TO 100 C CHECK CONTINUATION POSSIBILITIES IF (T .EQ. TOUT .AND. KFLAG .NE. 3) GO TO 10 IF (MFLAG .NE. 2) GO TO 60 C IFLAG = +2 OR -2 IF (KFLAG .EQ. 3) GO TO 90 IF (INIT .EQ. 0) GO TO 90 IF (KFLAG .EQ. 4) GO TO 80 IF (KFLAG .EQ. 5 .AND. ABSERR .EQ. 0.D0) GO TO 70 IF (KFLAG .EQ. 6 .AND. RELERR .LE. SAVRE .AND. ABSERR .LE. SAVAE) +GO TO 70 GO TO 100 C IFLAG = 3,4,5,6,7, OR 8 60 IF (IFLAG .EQ. 3) GO TO 90 IF (IFLAG .EQ. 4) GO TO 80 IF (IFLAG .EQ. 5 .AND. ABSERR .GT. 0.D0) GO TO 90 C INTEGRATION CANNOT BE CONTINUED SINCE USER DID NOT RESPOND TO C THE INSTRUCTIONS PERTAINING TO IFLAG=5,6,7, OR 8 70 STOP C RESET FUNCTION EVALUATION COUNTER 80 NFE = 0 IF (MFLAG .EQ. 2) GO TO 100 C RESET FLAG VALUE FROM PREVIOUS CALL 90 IFLAG = JFLAG IF (KFLAG .EQ. 3) MFLAG = ABS(IFLAG) C SAVE INPUT IFLAG AND SET CONTINUATION FLAG FOR SUBSEQUENT C INPUT CHECKING 100 JFLAG = IFLAG KFLAG = 0 C SAVE RELERR AND ABSERR FOR CHECKING INPUT ON SUBSEQUENT CALLS SAVRE = RELERR SAVAE = ABSERR C RESTRICT RELATIVE ERROR TOLERANCE TO BE AT LEAST AS LARGE AS C 2U+REMIN TO AVOID LIMITING PRECISION DIFFICULTIES ARRISING FROM C IMPOSSIBLE ACCURACY REQUESTS RE = 2.D0*U + REMIN IF (RELERR .GE. RE) GO TO 110 C RELATIVE ERROR TOLERANCE TOO SMALL RELERR = RE IFLAG = 3 KFLAG = 3 GO TO 290 110 U26 = 26.D0*U ALF = -0.1D0*LOG10(RELERR) DT = TOUT - T IF (MFLAG .EQ. 1) GO TO 120 IF (INIT .EQ. 0) GO TO 130 GO TO 140 C----------------------------------------------------------------------- C INITIALIZATION -- C----------------------------------------------------------------------- C SET INITIALIZATION COMPLETION INDICATOR, INIT C SET INDICATOR FOR TOO MANY OUTPUT POINTS, KOP C EVALUATE INITIAL DERIVATIVES C SET COUNTER FOR FUNCTION EVALUATIONS, NFE C ESTIMATE STARTING STEPSIZE 120 INIT = 0 KOP = 0 FIRST = .TRUE. A = T CALL F(A,Y,YP,W,IW) NFE = 1 IF (T .NE. TOUT) GO TO 130 IFLAG = 2 GO TO 290 130 INIT = 1 TOLN = 0.5D+00*(RELERR + ABSERR) C H = HSTART(F, N, A, TOUT, Y, YP, TOLN, 5, F2, F3, F4, F5, W, IW) C NFE = NFE + 4 H = ABS(DT) TOLN = 0.0D+00 DO 135 K=1,N TOL = RELERR*ABS(Y(K)) + ABSERR IF (TOL .LE. 0.0D+00) GOTO 135 TOLN = TOL YPK = ABS(YP(K)) IF (YPK*H**5 .GT. TOL) H = (TOL/YPK)**0.2D+00 135 CONTINUE IF (TOLN .LE. 0.0D+00) H = 0.0D+00 A = MAX(ABS(T),ABS(DT)) H = MAX(H,U26*A) JFLAG = SIGN(2,IFLAG) C----------------------------------------------------------------------- C SET STEP SIZE FOR INTEGRATION IN THE DIRECTION FROM T TO TOUT C----------------------------------------------------------------------- 140 H = SIGN(H,DT) C TEST TO SEE IF RK54 IS BEING SEVERELY IMPACTED BY TOO MANY C OUTPUT POINTS IF (ABS(H) .GE. 2.D0*ABS(DT)) KOP = KOP + 1 IF (KOP .NE. 100) GO TO 150 C UNNECESSARY FREQUENCY OF OUTPUT KOP = 0 IFLAG = 7 GO TO 290 150 IF (ABS(DT) .GT. U26*ABS(T)) GO TO 170 C IF TOO CLOSE TO OUTPUT POINT, EXTRAPOLATE AND RETURN DO 160 K = 1,N 160 Y(K) = Y(K) + DT*YP(K) A = TOUT CALL F(A,Y,YP,W,IW) T = TOUT NFE = NFE + 1 GO TO 290 C INITIALIZE OUTPUT INDICATOR 170 OUTPUT = .FALSE. C TO AVOID PREMATURE UNDERFLOW IN THE TOLERANCE FUNCTION, C SCALE THE ERROR TOLERANCES SCALE = 2.D0/RELERR AE = SCALE*ABSERR C----------------------------------------------------------------------- C STEP BY STEP INTEGRATION C----------------------------------------------------------------------- 180 HFAILD = .FALSE. C SET SMALLEST ALLOWABLE STEPSIZE C THIS STRATEGY IS THEORETICAL JUSTIFIED FOR C RKF45 (SEE SHAMPINE & WATTS); IT IS HEURISTICALLY C USEFUL FOR RK54, TOO. HMIN = U26*ABS(T) C ADJUST STEPSIZE IF NECESSARY TO HIT THE OUTPUT POINT. C LOOK AHEAD TWO STEPS TO AVOID DRASTIC CHANGES IN THE STEPSIZE C AND THUS LESSEN THE IMPACT OF OUTPUT POINTS ON THE CODE. DT = TOUT - T IF (ABS(DT) .GE. 2.D0*ABS(H)) GO TO 200 IF (ABS(DT) .GT. ABS(H)/0.9D0) GO TO 190 C THE NEXT SUCCESSFUL STEP WILL COMPLETE THE INTEGRATION C TO THE OUTPUT POINT OUTPUT = .TRUE. H = DT GO TO 200 190 H = 0.5D0*DT C----------------------------------------------------------------------- C CORE INTEGRATOR FOR TAKING A SINGLE STEP C----------------------------------------------------------------------- C THE TOLERANCES HAVE BEEN SCALED TO AVOID PREMATURE UNDERFLOW IN C COMPUTING THE ERROR TOLERANCE FUNCTION ET. C TO AVOID PROBLEMS WITH ZERO CROSSINGS, RELATIVE ERROR IS MEASURED C USING THE AVERAGE OF THE MAGNITUDES OF THE SOLUTION AT THE C BEGINNING AND END OF A STEP. C TO DISTINGUISH THE VARIOUS ARGUMENTS, H IS NOT PERMITTED C TO BECOME SMALLER THAN 26 UNITS OF ROUND-OFF IN T. C PRACTICAL LIMITS ON THE CHANGE IN THE STEP SIZE ARE ENFORCED TO C SMOOTH THE STEP SIZE SELECTION PROCESS AND TO AVOID EXCESSIVE C CHATTERING ON PROBLEMS HAVING DISCONTINUITIES. C TO PREVENT UNNECESSARY FAILURES, THE CODE USES 9/10 THE STEP SIZE C IT ESTIMATES WILL SUCCEED. C AFTER A STEPFAILURE, THE STEPSIZE IS NOT ALLOWED TO INCREASE FOR C THE NEXT ATTEMPTED STEP. THIS MAKES THE CODE MORE EFFICIENT ON C PROBLEMS HAVING DISCONTINUITIES AND MORE EFFECTIVE IN GENERAL C SINCE LOCAL EXTRAPOLATION IS BEING USED AND EXTRA CAUTION SEEMS C WARRANTED. C----------------------------------------------------------------------- C TEST NUMBER OF DERIVATIVE FUNCTION EVALUATIONS C IF OKAY, TRY TO ADVANCE THE INTEGRATION FROM T TO T + H 200 IF (NFE .LE. MAXNFE) GO TO 210 C TOO MUCH WORK IFLAG = 4 KFLAG = 4 GO TO 290 C ADVANCE AN APPROXIMATE SOLUTION OVER ONE STEP OF LENGTH H 210 CALL PD54 (F,N,Y,T,H,YP,F1,F2,F3,F4,F5,F6,F7,F1,W,IW) NFE = NFE + 5 C COMPUTE AND TEST ALLOWABLE TOLERANCES VERSUS LOCAL ERROR C ESTIMATES AND REMOVE SCALING OF TOLERANCES. NOTE THAT RELATIVE C ERROR IS MEASURED WITH RESPECT TO THE AVERAGE OF THE MAGNITUDES C OF THE SOLUTION AT THE BEGINNING AND END OF THE STEP. EEOET = 0.D0 DO 230 K = 1,N ET = ABS(Y(K)) + ABS(F1(K)) + AE IF (ET .GT. 0.D0) GO TO 220 C INAPPROPRIATE ERROR TOLERANCE IFLAG = 5 KFLAG = 5 GO TO 290 220 EE = ABS(F7(K)) 230 EEOET = MAX(EEOET,EE/ET) ESTTOL = ABS(H)*EEOET*SCALE IF (ESTTOL .LE. 1.D0) GO TO 240 C UNSUCESSFUL STEP C REDUCE THE STEP SIZE, TRY AGAIN C THE DECREASE IS LIMITED TO A FACTOR OF 1/10 HFAILD = .TRUE. OUTPUT = .FALSE. S = 0.1D0 IF (ESTTOL .LT. 5.9049D+04) S = 0.9D0/ESTTOL**0.2D+00 H = S*H IF (ABS(H) .GT. HMIN) GO TO 200 C REQUESTED ERROR UNATTAINABLE AT SMALLEST ALLOWABLE STEP SIZE IFLAG = 6 KFLAG = 6 GO TO 290 C SUCCESSFUL STEP C STORE SOLUTION AT T + H C AND EVALUATE DERIVATIVES THERE 240 T = T + H DO 250 K = 1,N 250 Y(K) = F1(K) A = T CALL F(A,Y,YP,W,IW) NFE = NFE + 1 C CHOOSE NEXT STEP SIZE C THE INCREASE IS LIMITED TO A FACTOR OF 10 C IF STEP FAILURE HAS JUST OCCURRED, NEXT C STEP SIZE IS NOT ALLOWED TO INCREASE FIRST = .TRUE. IF (.NOT. FIRST) GO TO 260 FIRST = .FALSE. S = 1000D0 IF (ESTTOL .GT. 1.889568D-04) S = 0.9D0/ESTTOL**0.2D+00 GO TO 270 C MODIFIED STEP-SIZE CONTROL DUE TO WATTS 260 ESTTOL = MAX(ESTTOL, 1.889568D-04) S = (ALF/ESTTOL*ESTOLD/ESTTOL)**0.2D+00*H/HOLD S = MIN(S,10.0D0) 270 IF (HFAILD) S = MIN(S,1.D0) HOLD = H ESTOLD = ESTTOL H = SIGN(MAX(S*ABS(H),HMIN),H) C----------------------------------------------------------------------- C END OF CORE INTEGRATOR C----------------------------------------------------------------------- C SHOULD WE TAKE ANOTHER STEP ? IF (OUTPUT) GO TO 280 IF (IFLAG .GT. 0) GO TO 180 C----------------------------------------------------------------------- C INTEGRATION SUCCESSFULLY COMPLETED C----------------------------------------------------------------------- C ONE-STEP MODE IFLAG = -2 GO TO 290 C INTERVAL MODE 280 T = TOUT IFLAG = 2 290 RETURN C END OF RKC54 END SUBROUTINE PD54(F, N, Y, T, H, F1, F2, F3, F4, F5, F6, F7, . E, S, W, IW) C PRINCE - DORMAND FIFTH-(FOURTH)-ORDER RUNGE-KUTTA FORMULAE C----------------------------------------------------------------------- C CORE INTEGRATES A SYSTEM OF N FIRST ORDER C ORDINARY DIFFERENTIAL EQUATIONS OF THE FORM C DY(I)/DT = F(T,Y(1),---,Y(N)) C WHERE THE INITIAL VALUES Y(I) AND THE INITIAL DERIVATIVES C F1(I) ARE SPECIFIED AT THE STARTING POINT T. CORE ADVANCES C THE SOLUTION OVER THE FIXED STEP H AND RETURNS C THE FITH ORDER (SIXTH ORDER ACCURATE LOCALLY) SOLUTION C APPROXIMATION AT T+H IN ARRAY S(I) C----------------------------------------------------------------------- C----------------------------------------------------------------------- C REFERENCE: C P.J. PRINCE, J.R. DORMAND 'A FAMILY OF EMBEDED RUNGE-KUTTA FORMULAE' C J. COMP. APPL. MATH. 6 (1980) 19-26. C----------------------------------------------------------------------- INTEGER IW(*) INTEGER I, N DOUBLE PRECISION T, H, Y(N), E(N), S(N), W(*), . F1(N), F2(N), F3(N), F4(N), F5(N), F6(N), F7(N), . T1, T2, T3, T4, T5, T6 EXTERNAL F C RUNGE-KUTTA FORMULAE T1= ( 1.0D0 . / 5.0D0) * H DO 10 I=1,N 10 E(I) = Y(I)+T1*F1(I) CALL F (T+(1.0D0/5.0D0)*H, E, F2, W, IW) T1 = ( 3.0D0 . / 40.0D0) * H T2 = ( 9.0D0 . / 40.0D0) * H DO 20 I=1,N 20 E(I) = Y(I)+T1*F1(I)+T2*F2(I) CALL F (T+(3.0D0/10.0D0)*H, E, F3, W, IW) T1 = ( 44.0D0 . / 45.0D0) * H T2 = ( -56.0D0 . / 15.0D0) * H T3 = ( 32.0D0 . / 9.0D0) * H DO 30 I=1,N 30 E(I) = Y(I)+T1*F1(I)+T2*F2(I)+T3*F3(I) CALL F (T+(4.0D0/5.0D0)*H, E, F4, W, IW) T1 = ( 19372.0D0 . / 6561.0D0) * H T2 = ( -25360.0D0 . / 2187.0D0) * H T3 = ( 64448.0D0 . / 6561.0D0) * H T4 = ( -212.0D0 . / 729.0D0) * H DO 40 I=1,N 40 E(I) = Y(I)+T1*F1(I)+T2*F2(I)+T3*F3(I)+T4*F4(I) CALL F (T+(8.0D0/9.0D0)*H, E, F5, W, IW) T1 = ( 9017.0D0 . / 3168.0D0) * H T2 = ( -355.0D0 . / 33.0D0) * H T3 = ( 46732.0D0 . / 5247.0D0) * H T4 = ( 49.0D0 . / 176.0D0) * H T5 = ( -5103.0D0 . / 18656.0D0) * H DO 50 I=1,N 50 E(I) = Y(I)+T1*F1(I)+T2*F2(I)+T3*F3(I)+T4*F4(I)+T5*F5(I) CALL F (T+H, E, F6, W, IW) T1 = ( 35.0D0 . / 384.0D0) * H T3 = ( 500.0D0 . / 1113.0D0) * H T4 = ( 125.0D0 . / 192.0D0) * H T5 = ( -2187.0D0 . / 6784.0D0) * H T6 = ( 11.0D0 . / 84.0D0) * H DO 60 I=1,N 60 E(I) = Y(I)+T1*F1(I)+T3*F3(I)+T4*F4(I)+T5*F5(I)+T6*F6(I) CALL F (T+H, E, F7, W, IW) C LOCALLY EXTRAPOLATED SOLUTION & ERROR DO 70 I=1,N S(I)=( 35.0D0 . / 384.0D0) * F1(I) . + ( 500.0D0 . / 1113.0D0) * F3(I) . + ( 125.0D0 . / 192.0D0) * F4(I) . + ( -2187.0D0 . / 6784.0D0) * F5(I) . + ( 11.0D0 . / 84.0D0) * F6(I) E(I)=S(I)- . (( 5179.0D0 . / 57600.0D0) * F1(I) . + ( 7571.0D0 . / 16695.0D0) * F3(I) . + ( 393.0D0 . / 640.0D0) * F4(I) . + ( -92097.0D0 . / 339200.0D0) * F5(I) . + ( 187.0D0 . / 2100.0D0) * F6(I) . + ( 1.0D0 . / 40.0D0) * F7(I)) 70 S(I) = Y(I)+H*S(I) RETURN END SUBROUTINE RK87 (F,N,Y,T,TOUT,RELERR,ABSERR,IFLAG,WORK,IWORK,W,IW) C RK87 INITIAL VALUE SOLVER BASED ON HIGH ORDER RUNGE-KUTTA FORMULAE C ADAPTATION OF THE IMPLEMENTATION OF C FEHLBERG'S FOURTH-FIFTH ORDER RUNGE-KUTTA METHOD C WRITTEN BY C L.F.SHAMPINE & H.A.WATTS C SANDIA LABORATORIES C ALBUQUERQUE, NEW MEXICO C TO THE MORE ACCURATE PRINCE/DORMAND SEVENTH-EIGHTH ORDER FORMULAE C BY C D.KRAFT C INSTITUT FUER DYNAMIK DER FLUGSYSTEME C DLR OBERPFAFFENHOFEN C STATUS: 20. OCTOBER 1984 C RK87 IS PRIMARILY DESIGNED TO SOLVE NON-STIFF AND MILDLY STIFF C DIFFERENTIAL EQUATIONS WITH MEDIUM TO HIGH ACCURACY & LOW OUTPUT C----------------------------------------------------------------------- C ABSTRACT C----------------------------------------------------------------------- C SUBROUTINE RK87 INTEGRATES A SYSTEM OF N FIRST ORDER C ORDINARY DIFFERENTIAL EQUATIONS OF THE FORM C DY(I)/DT = F(T,Y(1),Y(2),...,Y(N)) C WHERE THE Y(I) ARE GIVEN AT T . C TYPICALLY THE SUBROUTINE IS USED TO INTEGRATE FROM T TO TOUT BUT C IT CAN BE USED AS A ONE-STEP INTEGRATOR TO ADVANCE THE SOLUTION A C SINGLE STEP IN THE DIRECTION OF TOUT. ON RETURN THE PARAMETERS IN C THE CALL LIST ARE SET FOR CONTINUING THE INTEGRATION. THE USER HAS C ONLY TO CALL RK87 AGAIN (AND PERHAPS DEFINE A NEW VALUE FOR TOUT). C ACTUALLY, RK87 IS AN INTERFACING ROUTINE WHICH CALLS SUBROUTINE C RKC87 FOR THE SOLUTION. RKC87 IN TURN CALLS SUBROUTINE CORE WHICH C COMPUTES AN APPROXIMATE SOLUTION OVER ONE STEP. C RKF45 USES THE RUNGE-KUTTA-FEHLBERG (4,5) METHOD DESCRIBED IN C THE REFERENCES C E. FEHLBERG, LOW-ORDER CLASSICAL RUNGE-KUTTA FORMULAS WITH STEPSIZE C CONTROL, NASA TR R-315. C ALSO IN COMPUTING 6 (1970) 61-71. C L.F. SHAMPINE AND H.A. WATTS, PRACTICAL SOLUTION OF ORDINARY C DIFFERENTIAL EQUATIONS BY RUNGE-KUTTA METHODS. C SANDIA LABORATORIES REPORT SAND76-0585. C L.F. SHAMPINE AND H.A. WATTS, THE ART OF WRITING A RUNGE-KUTTA CODE C APPL. MATH. COMP. 5 (1979) 93-121. C THE PERFORMANCE OF RKF45 IS ILLUSTRATED IN THE REFERENCE C L.F. SHAMPINE, H.A. WATTS, S. DAVENPORT, SOLVING NON-STIFF ORDINARY C DIFFERENTIAL EQUATIONS-THE STATE OF THE ART. C SANDIA LABORATORIES REPORT SAND78-0182 . ALSO IN C SIAM REVIEW 18 (1976) 376-411. C RK87 USES THE RUNGE-KUTTA (8,7) METHOD DESCRIBED IN C P.J. PRINCE, J.R. DORMAND 'HIGH ORDER EMBEDDED RUNGE-KUTTA FORMULAE' C J. COMP. APPL. MATH. 7 (1981) 67-75. C THE PARAMETERS REPRESENT C F -- SUBROUTINE F(T,Y,YP) TO EVALUATE DERIVATIVES YP(I)=DY(I)/DT C N -- NUMBER OF EQUATIONS TO BE INTEGRATED C Y( ) -- SOLUTION VECTOR AT T C T -- INDEPENDENT VARIABLE C TOUT -- OUTPUT POINT AT WHICH SOLUTION IS DESIRED C RELERR,ABSERR -- RELATIVE AND ABSOULTE ERROR TOLERANCES FOR LOCAL C ERROR TEST. AT EACH STEP THE CODE REQUIRES THAT C ABS(LOCAL ERROR) .LE. RELERR*ABS(Y) + ABSERR C FOR EACH COMPONENT OF THE LOCAL ERROR AND SOLUTION VECTORS C IFLAG -- INDICATOR FOR STATUS OF INTEGRATION C WORK( ) -- ARRAY TO HOLD INFORMATION INTERNAL TO RK87 WHICH IS C NECESSARY FOR SUBSEQUENT CALLS. MUST BE DIMENSIONED C AT LEAST 12*N + 3 C IWORK( ) -- INTEGER ARRAY USED TO HOLD INFORMATION INTERNAL TO C RK87 WHICH IS NECESSARY FOR SUBSEQUENT CALLS. MUST BE C DIMENSIONED AT LEAST 5 C---------------------------------------------------------------------- C FIRST CALL TO RK87 C---------------------------------------------------------------------- C THE USER MUST PROVIDE STORAGE IN HIS CALLING PROGRAM FOR THE ARRAY C IN THE CALL LIST - Y(N), WORK(3+12*N), IWORK(5), C DECLARE F IN AN EXTERNAL STATEMENT, SUPPLY SUBROUTINE F(T,Y,YP) C AND INITIALIZE THE FOLLOWING PARAMETERS C N -- NUMBER OF EQUATIONS TO BE INTEGRATED. (N .GE. 1) C Y( ) -- VECTOR OF INITIAL CONDITIONS C T -- STARTING POINT OF INTEGRATION, MUST BE A VARIABLE C T=TOUT IS ALLOWED ON THE FIRST CALL ONLY, IN WHICH CASE C RK87 RETURNS WITH IFLAG=2 IF CONTINUATION IS POSSIBLE. C RELERR,ABSERR -- RELATIVE AND ABSOLUTE LOCAL ERROR TOLERANCES C WHICH MUST BE NON-NEGATIVE. RELERR MUST BE A VARIABLE WHILE C ABSERR MAY BE A CONSTANT. THE CODE SHOULD NORMALLY NOT BE C USED WITH RELATIVE ERROR CONTROL SMALLER THAN ABOUT 1.D-11. C TO AVOID LIMITING PRECISION DIFFICULTIES THE CODE REQUIRES C RELERR TO BE LARGER THAN AN INTERNALLY COMPUTED RELATIVE C ERROR PARAMETER WHICH IS MACHINE DEPENDENT. IN PARTICULAR, C PURE ABSOLUTE ERROR IS NOT PERMITTED. IF A SMALLER THAN C ALLOWABLE VALUE OF RELERR IS ATTEMPTED, RK87 INCREASES C RELERR APPROPRIATELY AND RETURNS CONTROL TO THE USER BEFORE C CONTINUING THE INTEGRATION. C IFLAG -- +1,-1 INDICATOR TO INITIALIZE THE CODE FOR EACH NEW C PROBLEM. NORMAL INPUT IS +1. THE USER SHOULD SET IFLAG=-1 C ONLY WHEN ONE-STEP INTEGRATOR CONTROL IS ESSENTIAL. IN THIS C CASE, RK87 ATTEMPTS TO ADVANCE THE SOLUTION A SINGLE STEP C IN THE DIRECTION OF TOUT EACH TIME IT IS CALLED. SINCE THIS C MODE OF OPERATION RESULTS IN EXTRA COMPUTING OVERHEAD, IT C SHOULD BE AVOIDED UNLESS NEEDED. C----------------------------------------------------------------------- C OUTPUT FROM RK87 C----------------------------------------------------------------------- C Y( ) -- SOLUTION AT T C T -- LAST POINT REACHED IN INTEGRATION. C IFLAG = 2 -- INTEGRATION REACHED TOUT. INDICATES SUCCESSFUL RETURN C AND IS THE NORMAL MODE FOR CONTINUING INTEGRATION. C =-2 -- A SINGLE SUCCESSFUL STEP IN THE DIRECTION OF TOUT C HAS BEEN TAKEN. NORMAL MODE FOR CONTINUING C INTEGRATION ONE STEP AT A TIME. C = 3 -- INTEGRATION WAS NOT COMPLETED BECAUSE RELATIVE ERROR C TOLERANCE WAS TOO SMALL. RELERR HAS BEEN INCREASED C APPROPRIATELY FOR CONTINUING. C = 4 -- INTEGRATION WAS NOT COMPLETED BECAUSE MORE THAN C 6500 DERIVATIVE EVALUATIONS WERE NEEDED. THIS C IS APPROXIMATELY 500 STEPS C = 5 -- INTEGRATION WAS NOT COMPLETED BECAUSE SOLUTION C VANISHED MAKING A PURE RELATIVE ERROR TEST C IMPOSSIBLE. MUST USE NON-ZERO ABSERR TO CONTINUE. C USING THE ONE-STEP INTEGRATION MODE FOR STEP C IS A GOOD WAY TO PROCEED. C = 6 -- INTEGRATION WAS NOT COMPLETED BECAUSE REQUESTED C ACCURACY COULD NOT BE ACHIEVED USING SMALLEST C ALLOWABLE STEPSIZE. USER MUST INCREASE THE ERROR C TOLERANCE BEFORE CONTINUED INTEGRATION CAN BE C ATTEMPTED. C = 7 -- IT IS LIKELY THAT RK87 IS INEFFICIENT FOR SOLVING C THIS PROBLEM. TOO MUCH OUTPUT IS RESTRICTING THE C NATURAL STEPSIZE CHOICE. USE THE ONE-STEP INTEGRATOR C MODE. C = 8 -- INVALID INPUT PARAMETERS C THIS INDICATOR OCCURS IF ANY OF THE FOLLOWING IS C SATISFIED - N .LE. 0 C T=TOUT AND IFLAG .NE. +1 OR -1 C RELERR OR ABSERR .LT. 0. C IFLAG .EQ. 0 OR .LT. -2 OR .GT.8 C WORK( ),IWORK( ) -- INFORMATION WHICH IS USUALLY OF NO INTEREST C TO THE USER BUT NECESSARY FOR SUBSEQUENT CALLS C WORK(1),...,WORK(N) CONTAINS THE FIRST DERIVATIVES C OF THE SOLUTION VECTOR Y AT T. WORK(N+1) CONTAINS C THE STEP SIZE H TO BE ATTEMPTED ON THE NEXT STEP. C IWORK(1) CONTAINS THE DERIVATIVE EVALUATION COUNTER. C----------------------------------------------------------------------- C SUBSEQUENT CALLS TO RK87 C----------------------------------------------------------------------- C SUBROUTINE RK87 RETURNS WITH ALL INFORMATION NEEDED TO CONTINUE C THE INTEGRATION. IF THE INTEGRATION REACHED TOUT, THE USER NEED ONLY C DEFINE A NEW TOUT AND CALL RK87 AGAIN. IN THE ONE-STEP INTEGRATOR C MODE (IFLAG=-2) THE USER MUST KEEP IN MIND THAT EACH STEP TAKEN IS C IN THE DIRECTION OF THE CURRENT TOUT. UPON REACHING TOUT (INDICATED C BY CHANGING IFLAG TO 2), THE USER MUST THEN DEFINE A NEW TOUT AND C RESET IFLAG TO -2 TO CONTINUE IN THE ONE STEP INTEGRATOR MODE. C IF THE INTEGRATION WAS NOT COMPLETED BUT THE USER STILL WANTS TO C CONTINUE (IFLAG=3,4 CASES), HE JUST CALLS RK87 AGAIN. WITH IFLAG=3 C THE RELERR PARAMETER HAS BEEN ADJUSTED APPROPTIATELY FOR CONTINUING C THE INTEGRATION. IN THE CASE OF IFLAG=4 THE FUNCTION COUNTER WILL C BE RESET TO 0 AND ANOTHER 6000 FUNCTION EVALUATIONS ARE ALLOWED. C HOWEVER, IN THE CASE IFLAG=5, THE USER MUST FIRST ALTER THE ERROR C CRITERION TO USE A POSITIVE VALUE OF ABSERR BEFORE INTEGRATION CAN C PROCEED. IF HE DOES NOT, EXECUTION IS TERMINATED. C ALSO, IN THE CASE IFLAG=6, IT IS NECESSARY FOR THE USER TO RESET C IFLAG TO 2 (OR -2 WHEN THE ONE-STEP INTEGRATION MODE IS BEING USED) C AS WELL AS INCREASING EITHER ABSERR, RELERR OR BOTH BEFORE THE C INTEGRATION CAN BE CONTINUED. IF THIS IS NOT DONE, EXECUTION WILL C BE TERMINATED. THE OCCURRENCE OF IFLAG=6 INDICATES A TROUBLE SPOT C (SOLUTION IS CHANGING RAPIDLY, SINGULARITY MAY BE PRESENT) AND IT C OFTEN IS INADVISABLE TO CONTINUE. C IF IFLAG=7 IS ENCOUNTERED, THE USER SHOULD USE THE ONE-STEP C INTEGRATION MODE WITH THE STEPSIZE DETERMINED BY THE CODE OR C CONSIDER SWITCHING TO THE ADAMS CODES DE,STEP,INTRP. IF THE USER C INSISTS UPON CONTINUING THE INTEGRATION WITH RK87, HE MUST RESET C IFLAG TO 2 BEFORE CALLING RK87 AGAIN. OTHERWISE, EXECUTION WILL C BE TERMINATED. C IF IFLAG=8 IS OBTAINED, INTEGRATION CANNOT BE CONTINUED UNLESS C THE INVALID INPUT PARAMETERS ARE CORRECTED. C IT SHOULD BE NOTED THAT THE ARRAYS WORK,IWORK CONTAIN INFORMATION C REQUIRED FOR SUBSEQUENT INTEGRATION. ACCORDINGLY, WORK AND IWORK C SHOULD NOT BE ALTERED. C----------------------------------------------------------------------- INTEGER IWORK(5), IW(*) INTEGER IFLAG, K1, K2, K3, K4, K5, K6, K7, K8, 1 K9, K10, K11, K12, K1M, N DOUBLE PRECISION Y(N), WORK(*), W(*), T, TOUT, ABSERR, RELERR EXTERNAL F C COMPUTE INDICES FOR THE SPLITTING OF THE WORK ARRAY K1M = N + 1 K1 = K1M + 1 K2 = K1 + N K3 = K2 + N K4 = K3 + N K5 = K4 + N K6 = K5 + N K7 = K6 + N K8 = K7 + N K9 = K8 + N K10 = K9 + N K11 = K10 + N K12 = K11 + N C----------------------------------------------------------------------- C THIS INTERFACING ROUTINE MERELY RELIEVES THE USER OF A LONG C CALLING LIST VIA THE SPLITTING APART OF TWO WORKING STORAGE C ARRAYS. IF THIS IS NOT COMPATIBLE WITH THE USERS COMPILER, C HE MUST USE RKC87 DIRECTLY. C----------------------------------------------------------------------- CALL RKC87(F,N,Y,T,TOUT,RELERR,ABSERR,IFLAG,WORK(1),WORK(K1M), 1 WORK(K1),WORK(K2),WORK(K3),WORK(K4),WORK(K5),WORK(K6), 2 WORK(K7),WORK(K8),WORK(K9),WORK(K10),WORK(K11),WORK(K12), 3 WORK(K12+1),IWORK(1),IWORK(2),IWORK(3),IWORK(4),IWORK(5),W,IW) RETURN END SUBROUTINE RKC87(F,N,Y,T,TOUT,RELERR,ABSERR,IFLAG,YP,H,F1,F2,F3,F4 1,F5,F6,F7,F8,F9,F10,F11,SAVRE,SAVAE,NFE,KOP,INIT,JFLAG,KFLAG,W,IW) C PRINCE/DORMAND 8(7) ORDER RUNGE-KUTTA METHOD C RKC87 INTEGRATES A SYSTEM OF FIRST ORDER ORDINARY DIFFERENTIAL C EQUATIONS AS DESCRIBED IN THE COMMENTS FOR RK87 . C THE ARRAYS YP,F1,....,F10 AND F11 (OF DIMENSION AT LEAST N) AND C THE VARIABLES H,SAVRE,SAVAE,NFE,KOP,INIT,JFLAG,AND KFLAG ARE USED C INTERNALLY BY THE CODE AND APPEAR IN THE CALL LIST TO ELIMINATE C LOCAL RETENTION OF VARIABLES BETWEEN CALLS. ACCORDINGLY, THEY C SHOULD NOT BE ALTERED. ITEMS OF POSSIBLE INTEREST ARE : C YP - DERIVATIVE OF SOLUTION VECTOR AT T C H - AN APPROPRIATE STEP SIZE TO BE USED FOR THE NEXT STEP C NFE- COUNTER ON THE NUMBER OF DERIVATIVE FUNCTION EVALUATIONS C----------------------------------------------------------------------- INTEGER IW(*) INTEGER IFLAG, INIT, JFLAG, K, KFLAG, KOP, 1 MAXNFE, MFLAG, N, NFE DOUBLE PRECISION Y(N),YP(N),F1(N),F2(N),F3(N),F4(N),F5(N),F6(N), 1 F7(N),F8(N),F9(N),F10(N),F11(N),A,ABSERR,AE,ALF, 2 DT,EE,EEOET,ESTOLD,ESTTOL,ET,H,HMIN,HOLD, 3 RELERR,REMIN,RE,S,SAVAE,SAVRE,SCALE,T,TOUT,U,U26, 4 TOL,TOLN,YPK,W(*) LOGICAL FIRST,HFAILD,OUTPUT EXTERNAL F C----------------------------------------------------------------------- C THE COMPUTER UNIT ROUND OFF ERROR U IS THE SMALLEST POSITIVE VALUE C REPRESENTABLE IN THE MACHINE SUCH THAT 1.+U .GT. 1. C VALUES TO BE USED ARE C U = 9.5D-7 FOR IBM 360/370 C U = 1.5D-8 FOR UNIVAC 1108 C U = 7.5D-9 FOR PDP-10 C U = 7.1D-15 FOR CDC 6000 SERIES C U = 2.2D-16 FOR IBM 360/370 DOUBLE PRECISION C U = 2.2D-16 FOR IBM PS/2 Model 70 D. PRECISION DATA U /2.22D-16/ C----------------------------------------------------------------------- C REMIN IS A TOLERANCE THRESHOLD WHICH IS ALSO DETERMINED BY THE C INTEGRATION METHOD. DATA REMIN /1.D-14/ C----------------------------------------------------------------------- DATA MAXNFE /6500/ C CHECK INPUT PARAMETERS IF (N .LT. 1) GO TO 10 IF (RELERR .LT. 0.D0 .OR. ABSERR .LT. 0.D0) GO TO 10 MFLAG = ABS(IFLAG) IF (MFLAG .GE. 1 .AND. MFLAG .LE. 8) GO TO 50 C INVALID INPUT 10 IFLAG = 8 GO TO 290 C IS THIS THE FIRST CALL ? 50 IF (MFLAG .EQ. 1) GO TO 100 C CHECK CONTINUATION POSSIBILITIES IF (T .EQ. TOUT .AND. KFLAG .NE. 3) GO TO 10 IF (MFLAG .NE. 2) GO TO 60 C IFLAG = +2 OR -2 IF (KFLAG .EQ. 3) GO TO 90 IF (INIT .EQ. 0) GO TO 90 IF (KFLAG .EQ. 4) GO TO 80 IF (KFLAG .EQ. 5 .AND. ABSERR .EQ. 0.D0) GO TO 70 IF (KFLAG .EQ. 6 .AND. RELERR .LE. SAVRE .AND. ABSERR .LE. SAVAE) 1GO TO 70 GO TO 100 C IFLAG = 3,4,5,6,7, OR 8 60 IF (IFLAG .EQ. 3) GO TO 90 IF (IFLAG .EQ. 4) GO TO 80 IF (IFLAG .EQ. 5 .AND. ABSERR .GT. 0.D0) GO TO 90 C INTEGRATION CANNOT BE CONTINUED SINCE USER DID NOT RESPOND TO C THE INSTRUCTIONS PERTAINING TO IFLAG=5,6,7, OR 8 70 STOP C RESET FUNCTION EVALUATION COUNTER 80 NFE = 0 IF (MFLAG .EQ. 2) GO TO 100 C RESET FLAG VALUE FROM PREVIOUS CALL 90 IFLAG = JFLAG IF (KFLAG .EQ. 3) MFLAG = ABS(IFLAG) C SAVE INPUT IFLAG AND SET CONTINUATION FLAG FOR SUBSEQUENT C INPUT CHECKING 100 JFLAG = IFLAG KFLAG = 0 C SAVE RELERR AND ABSERR FOR CHECKING INPUT ON SUBSEQUENT CALLS SAVRE = RELERR SAVAE = ABSERR C RESTRICT RELATIVE ERROR TOLERANCE TO BE AT LEAST AS LARGE AS C 2U+REMIN TO AVOID LIMITING PRECISION DIFFICULTIES ARRISING FROM C IMPOSSIBLE ACCURACY REQUESTS RE = 2.D0*U + REMIN IF (RELERR .GE. RE) GO TO 110 C RELATIVE ERROR TOLERANCE TOO SMALL RELERR = RE IFLAG = 3 KFLAG = 3 GO TO 290 110 U26 = 26.D0*U ALF = -0.1D0*LOG10(RELERR) DT = TOUT - T IF (MFLAG .EQ. 1) GO TO 120 IF (INIT .EQ. 0) GO TO 130 GO TO 140 C----------------------------------------------------------------------- C INITIALIZATION -- C----------------------------------------------------------------------- C SET INITIALIZATION COMPLETION INDICATOR, INIT C SET INDICATOR FOR TOO MANY OUTPUT POINTS, KOP C EVALUATE INITIAL DERIVATIVES C SET COUNTER FOR FUNCTION EVALUATIONS, NFE C ESTIMATE STARTING STEPSIZE 120 INIT = 0 KOP = 0 FIRST = .TRUE. A = T CALL F(A,Y,YP,W,IW) NFE = 1 IF (T .NE. TOUT) GO TO 130 IFLAG = 2 GO TO 290 130 INIT = 1 TOLN = 0.5D+00*(RELERR + ABSERR) C H = HSTART(F, N, A, TOUT, Y, YP, TOLN, 8, F2, F3, F4, F5, W, IW) H = ABS(DT) TOLN = 0.0D+00 DO 135 K=1,N TOL = RELERR*ABS(Y(K)) + ABSERR IF (TOL .LE. 0.0D+00) GOTO 135 TOLN = TOL YPK = ABS(YP(K)) IF (YPK*H**8 .GT. TOL) H = (TOL/YPK)**0.125D+00 135 CONTINUE IF (TOLN .LE. 0.0D+00) H = 0.0D+00 A = MAX(ABS(T),ABS(DT)) H = MAX(H,U26*A) C NFE = NFE + 4 JFLAG = SIGN(2,IFLAG) C----------------------------------------------------------------------- C SET STEP SIZE FOR INTEGRATION IN THE DIRECTION FROM T TO TOUT C----------------------------------------------------------------------- 140 H = SIGN(H,DT) C TEST TO SEE IF RK87 IS BEING SEVERELY IMPACTED BY TOO MANY C OUTPUT POINTS IF (ABS(H) .GE. 2.D0*ABS(DT)) KOP = KOP + 1 IF (KOP .NE. 100) GO TO 150 C UNNECESSARY FREQUENCY OF OUTPUT KOP = 0 IFLAG = 7 GO TO 290 150 IF (ABS(DT) .GT. U26*ABS(T)) GO TO 170 C IF TOO CLOSE TO OUTPUT POINT, EXTRAPOLATE AND RETURN DO 160 K = 1,N 160 Y(K) = Y(K) + DT*YP(K) A = TOUT CALL F(A,Y,YP,W,IW) T = TOUT NFE = NFE + 1 GO TO 290 C INITIALIZE OUTPUT INDICATOR 170 OUTPUT = .FALSE. C TO AVOID PREMATURE UNDERFLOW IN THE TOLERANCE FUNCTION, C SCALE THE ERROR TOLERANCES SCALE = 2.D0/RELERR AE = SCALE*ABSERR C----------------------------------------------------------------------- C STEP BY STEP INTEGRATION C----------------------------------------------------------------------- 180 HFAILD = .FALSE. C SET SMALLEST ALLOWABLE STEPSIZE C THIS STRATEGY IS THEORETICAL JUSTIFIED FOR C RKF45 (SEE SHAMPINE & WATTS); IT IS HEURISTICALLY C USEFUL FOR RK87, TOO. HMIN = U26*ABS(T) C ADJUST STEPSIZE IF NECESSARY TO HIT THE OUTPUT POINT. C LOOK AHEAD TWO STEPS TO AVOID DRASTIC CHANGES IN THE STEPSIZE C AND THUS LESSEN THE IMPACT OF OUTPUT POINTS ON THE CODE. DT = TOUT - T IF (ABS(DT) .GE. 2.D0*ABS(H)) GO TO 200 IF (ABS(DT) .GT. ABS(H)/0.9D0) GO TO 190 C THE NEXT SUCCESSFUL STEP WILL COMPLETE THE INTEGRATION C TO THE OUTPUT POINT OUTPUT = .TRUE. H = DT GO TO 200 190 H = 0.5D0*DT C----------------------------------------------------------------------- C CORE INTEGRATOR FOR TAKING A SINGLE STEP C----------------------------------------------------------------------- C THE TOLERANCES HAVE BEEN SCALED TO AVOID PREMATURE UNDERFLOW IN C COMPUTING THE ERROR TOLERANCE FUNCTION ET. C TO AVOID PROBLEMS WITH ZERO CROSSINGS, RELATIVE ERROR IS MEASURED C USING THE AVERAGE OF THE MAGNITUDES OF THE SOLUTION AT THE C BEGINNING AND END OF A STEP. C TO DISTINGUISH THE VARIOUS ARGUMENTS, H IS NOT PERMITTED C TO BECOME SMALLER THAN 26 UNITS OF ROUND-OFF IN T. C PRACTICAL LIMITS ON THE CHANGE IN THE STEP SIZE ARE ENFORCED TO C SMOOTH THE STEP SIZE SELECTION PROCESS AND TO AVOID EXCESSIVE C CHATTERING ON PROBLEMS HAVING DISCONTINUITIES. C TO PREVENT UNNECESSARY FAILURES, THE CODE USES 9/10 THE STEP SIZE C IT ESTIMATES WILL SUCCEED. C AFTER A STEPFAILURE, THE STEPSIZE IS NOT ALLOWED TO INCREASE FOR C THE NEXT ATTEMPTED STEP. THIS MAKES THE CODE MORE EFFICIENT ON C PROBLEMS HAVING DISCONTINUITIES AND MORE EFFECTIVE IN GENERAL C SINCE LOCAL EXTRAPOLATION IS BEING USED AND EXTRA CAUTION SEEMS C WARRANTED. C----------------------------------------------------------------------- C TEST NUMBER OF DERIVATIVE FUNCTION EVALUATIONS C IF OKAY, TRY TO ADVANCE THE INTEGRATION FROM T TO T + H 200 IF (NFE .LE. MAXNFE) GO TO 210 C TOO MUCH WORK IFLAG = 4 KFLAG = 4 GO TO 290 C ADVANCE AN APPROXIMATE SOLUTION OVER ONE STEP OF LENGTH H 210 CALL PD87(F,N,Y,T,H,YP,F1,F2,F3,F4,F5,F6,F7,F8,F9,F10,F11,F1,W,IW) NFE = NFE + 12 C COMPUTE AND TEST ALLOWABLE TOLERANCES VERSUS LOCAL ERROR C ESTIMATES AND REMOVE SCALING OF TOLERANCES. NOTE THAT RELATIVE C ERROR IS MEASURED WITH RESPECT TO THE AVERAGE OF THE MAGNITUDES C OF THE SOLUTION AT THE BEGINNING AND END OF THE STEP. EEOET = 0.D0 DO 230 K = 1,N ET = ABS(Y(K)) + ABS(F1(K)) + AE IF (ET .GT. 0.D0) GO TO 220 C INAPPROPRIATE ERROR TOLERANCE IFLAG = 5 KFLAG = 5 GO TO 290 220 EE = ABS(F11(K)) 230 EEOET = MAX(EEOET,EE/ET) ESTTOL = ABS(H)*EEOET*SCALE IF (ESTTOL .LE. 1.D0) GO TO 240 C UNSUCESSFUL STEP C REDUCE THE STEP SIZE, TRY AGAIN C THE DECREASE IS LIMITED TO A FACTOR OF 1/10 HFAILD = .TRUE. OUTPUT = .FALSE. S = 0.1D0 IF (ESTTOL .LT. 4.3046721D+07) S = 0.9D0/ESTTOL**0.125D0 H = S*H IF (ABS(H) .GT. HMIN) GO TO 200 C REQUESTED ERROR UNATTAINABLE AT SMALLEST ALLOWABLE STEP SIZE IFLAG = 6 KFLAG = 6 GO TO 290 C SUCCESSFUL STEP C STORE SOLUTION AT T + H C AND EVALUATE DERIVATIVES THERE 240 T = T + H DO 250 K = 1,N 250 Y(K) = F1(K) A = T CALL F(A,Y,YP,W,IW) NFE = NFE + 1 C CHOOSE NEXT STEP SIZE C THE INCREASE IS LIMITED TO A FACTOR OF 10 C IF STEP FAILURE HAS JUST OCCURRED, NEXT C STEP SIZE IS NOT ALLOWED TO INCREASE FIRST = .TRUE. IF (.NOT. FIRST) GO TO 260 FIRST = .FALSE. S = 10.0D0 IF (ESTTOL .GT. 4.3046721D-09) S = 0.9D0/ESTTOL**0.125D0 GO TO 270 C MODIFIED STEP-SIZE CONTROL DUE TO WATTS 260 ESTTOL = MAX(ESTTOL, 4.3046721D-9) S = (ALF/ESTTOL*ESTOLD/ESTTOL)**0.125D0*H/HOLD S = MIN(S,10.0D0) 270 IF (HFAILD) S = MIN(S,1.D0) HOLD = H ESTOLD = ESTTOL H = SIGN(MAX(S*ABS(H),HMIN),H) C----------------------------------------------------------------------- C END OF CORE INTEGRATOR C----------------------------------------------------------------------- C SHOULD WE TAKE ANOTHER STEP ? IF (OUTPUT) GO TO 280 IF (IFLAG .GT. 0) GO TO 180 C----------------------------------------------------------------------- C INTEGRATION SUCCESSFULLY COMPLETED C----------------------------------------------------------------------- C ONE-STEP MODE IFLAG = -2 GO TO 290 C INTERVAL MODE 280 T = TOUT IFLAG = 2 290 RETURN C END OF RKC87 END SUBROUTINE PD87(F, N, Y, T, H, F1, F2, F3, F4, F5, . F6, F7, F8, F9, F10, F11, E, S, W, IW) C PRINCE - DORMAND SEVENTH-EIGHTH ORDER RUNGE-KUTTA FORMULAE C----------------------------------------------------------------------- C CORE INTEGRATES A SYSTEM OF N FIRST ORDER C ORDINARY DIFFERENTIAL EQUATIONS OF THE FORM C DY(I)/DT = F(T,Y(1),---,Y(N)) C WHERE THE INITIAL VALUES Y(I) AND THE INITIAL DERIVATIVES C F1(I) ARE SPECIFIED AT THE STARTING POINT T. CORE ADVANCES C THE SOLUTION OVER THE FIXED STEP H AND RETURNS C THE EIGHTH ORDER (NINTH ORDER ACCURATE LOCALLY) SOLUTION C APPROXIMATION AT T+H IN ARRAY S(I) C----------------------------------------------------------------------- C----------------------------------------------------------------------- C REFERENCE: C P.J. PRINCE, J.R. DORMAND 'HIGH ORDER EMBEDDED RUNGE-KUTTA FORMULAE' C J. COMP. APPL. MATH. 7 (1981) 67-75. C----------------------------------------------------------------------- INTEGER IW(*) INTEGER I, N DOUBLE PRECISION T, H, Y(N), E(N), S(N), W(*), . F1(N), F2(N), F3(N), F4(N), F5(N), . F6(N), F7(N), F8(N), F9(N), F10(N), F11(N), . T1, T2, T3, T4, T5, T6, T7, T8, T9 EXTERNAL F C RUNGE-KUTTA FORMULAE T1= ( 1.0D0 . / 18.0D0) * H DO 10 I=1,N 10 E(I) = Y(I)+T1*F1(I) CALL F (T+(1.0D0/18.0D0)*H, E, F2, W, IW) T1 = ( 1.0D0 . / 48.0D0) * H T2 = ( 1.0D0 . / 16.0D0) * H DO 20 I=1,N 20 E(I) = Y(I)+T1*F1(I)+T2*F2(I) CALL F (T+(1.0D0/12.0D0)*H, E, F3, W, IW) T1 = ( 1.0D0 . / 32.0D0) * H T2 = ( 3.0D0 . / 32.0D0) * H DO 30 I=1,N 30 E(I) = Y(I)+T1*F1(I)+T2*F3(I) CALL F (T+(1.0D0/8.0D0)*H, E, F2, W, IW) T1 = ( 5.0D0 . / 16.0D0) * H T2 = ( -75.0D0 . / 64.0D0) * H T3 = ( 75.0D0 . / 64.0D0) * H DO 40 I=1,N 40 E(I) = Y(I)+T1*F1(I)+T2*F3(I)+T3*F2(I) CALL F (T+(5.0D0/16.0D0)*H, E, F3, W, IW) T1 = ( 3.0D0 . / 80.0D0) * H T2 = ( 3.0D0 . / 16.0D0) * H T3 = ( 3.0D0 . / 20.0D0) * H DO 50 I=1,N 50 E(I) = Y(I)+T1*F1(I)+T2*F2(I)+T3*F3(I) CALL F (T+(3.0D0/8.0D0)*H, E, F4, W, IW) T1 = ( 29443841.0D0 . / 614563906.0D0) * H T2 = ( 77736538.0D0 . / 692538347.0D0) * H T3 = ( -28693883.0D0 . / 1125000000.0D0) * H T4 = ( 23124283.0D0 . / 1800000000.0D0) * H DO 60 I=1,N 60 E(I) = Y(I)+T1*F1(I)+T2*F2(I)+T3*F3(I)+T4*F4(I) CALL F (T+(59.0D0/400.0D0)*H, E, F5, W, IW) T1 = ( 16016141.0D0 . / 946692911.0D0) * H T2 = ( 61564180.0D0 . / 158732637.0D0) * H T3 = ( 22789713.0D0 . / 633445777.0D0) * H T4 = ( 545815736.0D0 . / 2771057229.0D0) * H T5 = ( -180193667.0D0 . / 1043307555.0D0) * H DO 70 I=1,N 70 E(I) = Y(I)+T1*F1(I)+T2*F2(I)+T3*F3(I)+T4*F4(I)+T5*F5(I) CALL F (T+(93.0D0/200.0D0)*H, E, F6, W, IW) T1 = ( 39632708.0D0 . / 573591083.0D0) * H T2 = ( -433636366.0D0 . / 683701615.0D0) * H T3 = ( -421739975.0D0 . / 2616292301.0D0) * H T4 = ( 100302831.0D0 . / 723423059.0D0) * H T5 = ( 790204164.0D0 . / 839813087.0D0) * H T6 = ( 800635310.0D0 . / 3783071287.0D0) * H DO 80 I=1,N 80 E(I) = Y(I)+T1*F1(I)+T2*F2(I)+T3*F3(I)+T4*F4(I)+T5*F5(I)+T6*F6(I) CALL F (T+(5490023248.0D0/9719169821.0D0)*H, E, F7, W, IW) T1 = ( 246121993.0D0 . / 1340847787.0D0) * H T2 = (-37695042795.0D0 . / 15268766246.0D0) * H T3 = ( -309121744.0D0 . / 1061227803.0D0) * H T4 = ( -12992083.0D0 . / 490766935.0D0) * H T5 = ( 6005943493.0D0 . / 2108947869.0D0) * H T6 = ( 393006217.0D0 . / 1396673457.0D0) * H T7 = ( 123872331.0D0 . / 1001029789.0D0) * H DO 90 I=1,N 90 E(I) = Y(I)+T1*F1(I)+T2*F2(I)+T3*F3(I)+T4*F4(I)+T5*F5(I)+T6*F6(I) . +T7*F7(I) CALL F (T+(13.0D0/20.0D0)*H, E, F8, W, IW) T1 = ( -1028468189.0D0 . / 846180014.0D0) * H T2 = ( 8478235783.0D0 . / 508512852.0D0) * H T3 = ( 1311729495.0D0 . / 1432422823.0D0) * H T4 = (-10304129995.0D0 . / 1701304382.0D0) * H T5 = (-48777925059.0D0 . / 3047939560.0D0) * H T6 = ( 15336726248.0D0 . / 1032824649.0D0) * H T7 = (-45442868181.0D0 . / 3398467696.0D0) * H T8 = ( 3065993473.0D0 . / 597172653.0D0) * H DO 100 I=1,N 100 E(I) = Y(I)+T1*F1(I)+T2*F2(I)+T3*F3(I)+T4*F4(I)+T5*F5(I)+T6*F6(I) . +T7*F7(I)+T8*F8(I) CALL F (T+(1201146811.0D0/1299019798.0D0)*H, E, F9, W, IW) T1 = ( 185892177.0D0 . / 718116043.0D0) * H T2 = ( -3185094517.0D0 . / 667107341.0D0) * H T3 = ( -477755414.0D0 . / 1098053517.0D0) * H T4 = ( -703635378.0D0 . / 230739211.0D0) * H T5 = ( 5731566787.0D0 . / 1027545527.0D0) * H T6 = ( 5232866602.0D0 . / 850066563.0D0) * H T7 = ( -4093664535.0D0 . / 808688257.0D0) * H T8 = ( 3962137247.0D0 . / 1805957418.0D0) * H T9 = ( 65686358.0D0 . / 487910083.0D0) * H DO 110 I=1,N 110 E(I) = Y(I)+T1*F1(I)+T2*F2(I)+T3*F3(I)+T4*F4(I)+T5*F5(I)+T6*F6(I) . +T7*F7(I)+T8*F8(I)+T9*F9(I) CALL F (T+H, E, F10, W, IW) T1 = ( 403863854.0D0 . / 491063109.0D0) * H T2 = ( -5068492393.0D0 . / 434740067.0D0) * H T3 = ( -411421997.0D0 . / 543043805.0D0) * H T4 = ( 652783627.0D0 . / 914296604.0D0) * H T5 = ( 11173962825.0D0 . / 925320556.0D0) * H T6 = (-13158990841.0D0 . / 6184727034.0D0) * H T7 = ( 3936647629.0D0 . / 1978049680.0D0) * H T8 = ( -160528059.0D0 . / 685178525.0D0) * H T9 = ( 248638103.0D0 . / 1413531060.0D0) * H DO 120 I=1,N 120 E(I) = Y(I)+T1*F1(I)+T2*F2(I)+T3*F3(I)+T4*F4(I)+T5*F5(I)+T6*F6(I) . +T7*F7(I)+T8*F8(I)+T9*F9(I) CALL F (T+H, E, F11, W,IW) C LOCALLY EXTRAPOLATED SOLUTION & ERROR DO 130 I=1,N S(I)=( 14005451.0D0 . / 335480064.0D0) * F1(I) . + ( -59238493.0D0 . / 1068277825.0D0) * F4(I) . + ( 181606767.0D0 . / 758867731.0D0) * F5(I) . + ( 561292985.0D0 . / 797845732.0D0) * F6(I) . + ( -1041891430.0D0 . / 1371343529.0D0) * F7(I) . + ( 760417239.0D0 . / 1151165299.0D0) * F8(I) . + ( 118820643.0D0 . / 751138087.0D0) * F9(I) . + ( -528747749.0D0 . / 2220607170.0D0) * F10(I) . + ( 1.0D0 . / 4.0D0) * F11(I) E(I)=S(I)- . (( 13451932.0D0 . / 455176623.0D0) * F1(I) . + ( -808719846.0D0 . / 976000145.0D0) * F4(I) . + ( 1757004468.0D0 . / 5645159321.0D0) * F5(I) . + ( 656045339.0D0 . / 265891186.0D0) * F6(I) . + ( -3867574721.0D0 . / 1518517206.0D0) * F7(I) . + ( 465885868.0D0 . / 322736535.0D0) * F8(I) . + ( 53011238.0D0 . / 667516719.0D0) * F9(I) . + ( 2.0D0 . / 45.0D0) * F10(I)) 130 S(I) = Y(I)+H*S(I) RETURN END ************************************************************************ * OPTIMIZER * ************************************************************************ SUBROUTINE SLSQP (M, MEQ, LA, N, X, XL, XU, F, C, G, A, * ACC, ITER, MODE, W, L_W, JW, L_JW) C SLSQP S EQUENTIAL L EAST SQ UARES P ROGRAMMING C TO SOLVE GENERAL NONLINEAR OPTIMIZATION PROBLEMS C*********************************************************************** C* * C* * C* A NONLINEAR PROGRAMMING METHOD WITH * C* QUADRATIC PROGRAMMING SUBPROBLEMS * C* * C* * C* THIS SUBROUTINE SOLVES THE GENERAL NONLINEAR PROGRAMMING PROBLEM * C* * C* MINIMIZE F(X) * C* * C* SUBJECT TO C (X) .EQ. 0 , J = 1,...,MEQ * C* J * C* * C* C (X) .GE. 0 , J = MEQ+1,...,M * C* J * C* * C* XL .LE. X .LE. XU , I = 1,...,N. * C* I I I * C* * C* THE ALGORITHM IMPLEMENTS THE METHOD OF HAN AND POWELL * C* WITH BFGS-UPDATE OF THE B-MATRIX AND L1-TEST FUNCTION * C* WITHIN THE STEPLENGTH ALGORITHM. * C* * C* PARAMETER DESCRIPTION: * C* ( * MEANS THIS PARAMETER WILL BE CHANGED DURING CALCULATION ) * C* * C* M IS THE TOTAL NUMBER OF CONSTRAINTS, M .GE. 0 * C* MEQ IS THE NUMBER OF EQUALITY CONSTRAINTS, MEQ .GE. 0 * C* LA SEE A, LA .GE. MAX(M,1) * C* N IS THE NUMBER OF VARIBLES, N .GE. 1 * C* * X() X() STORES THE CURRENT ITERATE OF THE N VECTOR X * C* ON ENTRY X() MUST BE INITIALIZED. ON EXIT X() * C* STORES THE SOLUTION VECTOR X IF MODE = 0. * C* XL() XL() STORES AN N VECTOR OF LOWER BOUNDS XL TO X. * C* XU() XU() STORES AN N VECTOR OF UPPER BOUNDS XU TO X. * C* F IS THE VALUE OF THE OBJECTIVE FUNCTION. * C* C() C() STORES THE M VECTOR C OF CONSTRAINTS, * C* EQUALITY CONSTRAINTS (IF ANY) FIRST. * C* DIMENSION OF C MUST BE GREATER OR EQUAL LA, * C* which must be GREATER OR EQUAL MAX(1,M). * C* G() G() STORES THE N VECTOR G OF PARTIALS OF THE * C* OBJECTIVE FUNCTION; DIMENSION OF G MUST BE * C* GREATER OR EQUAL N+1. * C* A(),LA,M,N THE LA BY N + 1 ARRAY A() STORES * C* THE M BY N MATRIX A OF CONSTRAINT NORMALS. * C* A() HAS FIRST DIMENSIONING PARAMETER LA, * C* WHICH MUST BE GREATER OR EQUAL MAX(1,M). * C* F,C,G,A MUST ALL BE SET BY THE USER BEFORE EACH CALL. * C* * ACC ABS(ACC) CONTROLS THE FINAL ACCURACY. * C* IF ACC .LT. ZERO AN EXACT LINESEARCH IS PERFORMED,* C* OTHERWISE AN ARMIJO-TYPE LINESEARCH IS USED. * C* * ITER PRESCRIBES THE MAXIMUM NUMBER OF ITERATIONS. * C* ON EXIT ITER INDICATES THE NUMBER OF ITERATIONS. * C* * MODE MODE CONTROLS CALCULATION: * C* REVERSE COMMUNICATION IS USED IN THE SENSE THAT * C* THE PROGRAM IS INITIALIZED BY MODE = 0; THEN IT IS* C* TO BE CALLED REPEATEDLY BY THE USER UNTIL A RETURN* C* WITH MODE .NE. IABS(1) TAKES PLACE. * C* IF MODE = -1 GRADIENTS HAVE TO BE CALCULATED, * C* WHILE WITH MODE = 1 FUNCTIONS HAVE TO BE CALCULATED C* MODE MUST NOT BE CHANGED BETWEEN SUBSEQUENT CALLS * C* OF SQP. * C* EVALUATION MODES: * C* MODE = -1: GRADIENT EVALUATION, (G&A) * C* 0: ON ENTRY: INITIALIZATION, (F,G,C&A) * C* ON EXIT : REQUIRED ACCURACY FOR SOLUTION OBTAINED * C* 1: FUNCTION EVALUATION, (F&C) * C* * C* FAILURE MODES: * C* 2: NUMBER OF EQUALITY CONTRAINTS LARGER THAN N * C* 3: MORE THAN 3*N ITERATIONS IN LSQ SUBPROBLEM * C* 4: INEQUALITY CONSTRAINTS INCOMPATIBLE * C* 5: SINGULAR MATRIX E IN LSQ SUBPROBLEM * C* 6: SINGULAR MATRIX C IN LSQ SUBPROBLEM * C* 7: RANK-DEFICIENT EQUALITY CONSTRAINT SUBPROBLEM HFTI* C* 8: POSITIVE DIRECTIONAL DERIVATIVE FOR LINESEARCH * C* 9: MORE THAN ITER ITERATIONS IN SQP * C* >=10: WORKING SPACE W OR JW TOO SMALL, * C* W SHOULD BE ENLARGED TO L_W=MODE/1000 * C* JW SHOULD BE ENLARGED TO L_JW=MODE-1000*L_W * C* * W(), L_W W() IS A ONE DIMENSIONAL WORKING SPACE, * C* THE LENGTH L_W OF WHICH SHOULD BE AT LEAST * C* (3*N1+M)*(N1+1) for LSQ * C* +(N1-MEQ+1)*(MINEQ+2) + 2*MINEQ for LSI * C* +(N1+MINEQ)*(N1-MEQ) + 2*MEQ + N1 for LSEI * C* + N1*N/2 + 2*M + 3*N + 3*N1 + 1 for SLSQPB * C* with MINEQ = M - MEQ + 2*N1 & N1 = N+1 * C* NOTICE: FOR PROPER DIMENSIONING OF W IT IS RECOMMENDED TO * C* COPY THE FOLLOWING STATEMENTS INTO THE HEAD OF * C* THE CALLING PROGRAM (AND REMOVE THE COMMENT C) * c####################################################################### C INTEGER LEN_W, LEN_JW, M, N, N1, MEQ, MINEQ C PARAMETER (M=... , MEQ=... , N=... ) C PARAMETER (N1= N+1, MINEQ= M-MEQ+N1+N1) C PARAMETER (LEN_W= c $ (3*N1+M)*(N1+1) c $ +(N1-MEQ+1)*(MINEQ+2) + 2*MINEQ c $ +(N1+MINEQ)*(N1-MEQ) + 2*MEQ + N1 c $ +(N+1)*N/2 + 2*M + 3*N + 3*N1 + 1, c $ LEN_JW=MINEQ) C DOUBLE PRECISION W(LEN_W) C INTEGER JW(LEN_JW) c####################################################################### C* THE FIRST M+N+N*N1/2 ELEMENTS OF W MUST NOT BE * C* CHANGED BETWEEN SUBSEQUENT CALLS OF SLSQP. * C* ON RETURN W(1) ... W(M) CONTAIN THE MULTIPLIERS * C* ASSOCIATED WITH THE GENERAL CONSTRAINTS, WHILE * C* W(M+1) ... W(M+N(N+1)/2) STORE THE CHOLESKY FACTOR* C* L*D*L(T) OF THE APPROXIMATE HESSIAN OF THE * C* LAGRANGIAN COLUMNWISE DENSE AS LOWER TRIANGULAR * C* UNIT MATRIX L WITH D IN ITS 'DIAGONAL' and * C* W(M+N(N+1)/2+N+2 ... W(M+N(N+1)/2+N+2+M+2N) * C* CONTAIN THE MULTIPLIERS ASSOCIATED WITH ALL * C* ALL CONSTRAINTS OF THE QUADRATIC PROGRAM FINDING * C* THE SEARCH DIRECTION TO THE SOLUTION X* * C* * JW(), L_JW JW() IS A ONE DIMENSIONAL INTEGER WORKING SPACE * C* THE LENGTH L_JW OF WHICH SHOULD BE AT LEAST * C* MINEQ * C* with MINEQ = M - MEQ + 2*N1 & N1 = N+1 * C* * C* THE USER HAS TO PROVIDE THE FOLLOWING SUBROUTINES: * C* LDL(N,A,Z,SIG,W) : UPDATE OF THE LDL'-FACTORIZATION. * C* LINMIN(A,B,F,TOL) : LINESEARCH ALGORITHM IF EXACT = 1 * C* LSQ(M,MEQ,LA,N,NC,C,D,A,B,XL,XU,X,LAMBDA,W,....) : * C* * C* SOLUTION OF THE QUADRATIC PROGRAM * C* QPSOL IS RECOMMENDED: * C* PE GILL, W MURRAY, MA SAUNDERS, MH WRIGHT: * C* USER'S GUIDE FOR SOL/QPSOL: * C* A FORTRAN PACKAGE FOR QUADRATIC PROGRAMMING, * C* TECHNICAL REPORT SOL 83-7, JULY 1983 * C* DEPARTMENT OF OPERATIONS RESEARCH, STANFORD UNIVERSITY * C* STANFORD, CA 94305 * C* QPSOL IS THE MOST ROBUST AND EFFICIENT QP-SOLVER * C* AS IT ALLOWS WARM STARTS WITH PROPER WORKING SETS * C* * C* IF IT IS NOT AVAILABLE USE LSEI, A CONSTRAINT LINEAR LEAST * C* SQUARES SOLVER IMPLEMENTED USING THE SOFTWARE HFTI, LDP, NNLS * C* FROM C.L. LAWSON, R.J.HANSON: SOLVING LEAST SQUARES PROBLEMS, * C* PRENTICE HALL, ENGLEWOOD CLIFFS, 1974. * C* LSEI COMES WITH THIS PACKAGE, together with all necessary SR's. * C* * C* TOGETHER WITH A COUPLE OF SUBROUTINES FROM BLAS LEVEL 1 * C* * C* SQP IS HEAD SUBROUTINE FOR BODY SUBROUTINE SQPBDY * C* IN WHICH THE ALGORITHM HAS BEEN IMPLEMENTED. * C* * C* IMPLEMENTED BY: DIETER KRAFT, DFVLR OBERPFAFFENHOFEN * C* as described in Dieter Kraft: A Software Package for * C* Sequential Quadratic Programming * C* DFVLR-FB 88-28, 1988 * C* which should be referenced if the user publishes results of SLSQP * C* * C* DATE: APRIL - OCTOBER, 1981. * C* STATUS: DECEMBER, 31-ST, 1984. * C* STATUS: MARCH , 21-ST, 1987, REVISED TO FORTAN 77 * C* STATUS: MARCH , 20-th, 1989, REVISED TO MS-FORTRAN * C* STATUS: APRIL , 14-th, 1989, HESSE in-line coded * C* STATUS: FEBRUARY, 28-th, 1991, FORTRAN/2 Version 1.04 * C* accepts Statement Functions * C* STATUS: MARCH , 1-st, 1991, tested with SALFORD * C* FTN77/386 COMPILER VERS 2.40* C* in protected mode * C* * C*********************************************************************** C* * C* Copyright 1991: Dieter Kraft, FHM * C* * C*********************************************************************** INTEGER IL, IM, IR, IS, ITER, IU, IV, IW, IX, L_W, L_JW, * JW(L_JW), LA, M, MEQ, MINEQ, MODE, N, N1 DOUBLE PRECISION ACC, A(LA,N+1), C(LA), F, G(N+1), * X(N), XL(N), XU(N), W(L_W) c dim(W) = N1*(N1+1) + MEQ*(N1+1) + MINEQ*(N1+1) for LSQ c +(N1-MEQ+1)*(MINEQ+2) + 2*MINEQ for LSI c +(N1+MINEQ)*(N1-MEQ) + 2*MEQ + N1 for LSEI c + N1*N/2 + 2*M + 3*N +3*N1 + 1 for SLSQPB c with MINEQ = M - MEQ + 2*N1 & N1 = N+1 C CHECK LENGTH OF WORKING ARRAYS N1 = N+1 MINEQ = M-MEQ+N1+N1 IL = (3*N1+M)*(N1+1) + .(N1-MEQ+1)*(MINEQ+2) + 2*MINEQ + .(N1+MINEQ)*(N1-MEQ) + 2*MEQ + .N1*N/2 + 2*M + 3*N + 4*N1 + 1 IM = MAX(MINEQ, N1-MEQ) IF (L_W .LT. IL .OR. L_JW .LT. IM) THEN MODE = 1000*MAX(10,IL) MODE = MODE+MAX(10,IM) RETURN ENDIF C PREPARE DATA FOR CALLING SQPBDY - INITIAL ADDRESSES IN W IM = 1 IL = IM + MAX(1,M) IL = IM + LA IX = IL + N1*N/2 + 1 IR = IX + N IS = IR + N + N + MAX(1,M) IS = IR + N + N + LA IU = IS + N1 IV = IU + N1 IW = IV + N1 CALL SLSQPB (M, MEQ, LA, N, X, XL, XU, F, C, G, A, ACC, ITER, * MODE, W(IR), W(IL), W(IX), W(IM), W(IS), W(IU), W(IV), W(IW), JW) END SUBROUTINE SLSQPB (M, MEQ, LA, N, X, XL, XU, F, C, G, A, ACC, * ITER, MODE, R, L, X0, MU, S, U, V, W, IW) C NONLINEAR PROGRAMMING BY SOLVING SEQUENTIALLY QUADRATIC PROGRAMS C - L1 - LINE SEARCH, POSITIVE DEFINITE BFGS UPDATE - C BODY SUBROUTINE FOR SLSQP INTEGER IW(*), I, IEXACT, INCONS, IRESET, ITER, ITERMX, * K, J, LA, LINE, M, MEQ, MODE, N, N1, N2, N3 DOUBLE PRECISION A(LA,N+1), C(LA), G(N+1), L((N+1)*(N+2)/2), * MU(LA), R(M+N+N+2), S(N+1), U(N+1), V(N+1), W(*), * X(N), XL(N), XU(N), X0(N), * DDOT, DNRM2, LINMIN, * ACC, ALFMIN, ALPHA, F, F0, GS, H1, H2, H3, H4, * HUN, ONE, T, T0, TEN, TOL, TWO, ZERO c dim(W) = N1*(N1+1) + MEQ*(N1+1) + MINEQ*(N1+1) for LSQ c +(N1-MEQ+1)*(MINEQ+2) + 2*MINEQ c +(N1+MINEQ)*(N1-MEQ) + 2*MEQ + N1 for LSEI c with MINEQ = M - MEQ + 2*N1 & N1 = N+1 SAVE ALPHA, F0, GS, H1, H2, H3, H4, T, T0, TOL, * IEXACT, INCONS, IRESET, ITERMX, LINE, N1, N2, N3 DATA ZERO /0.0D0/, ONE /1.0D0/, ALFMIN /1.0D-1/, * HUN /1.0D+2/, TEN /1.0D+1/, TWO /2.0D0/ IF (MODE) 260, 100, 220 100 ITERMX = ITER IF (ACC.GE.ZERO) THEN IEXACT = 0 ELSE IEXACT = 1 ENDIF ACC = ABS(ACC) TOL = TEN*ACC ITER = 0 IRESET = 0 N1 = N + 1 N2 = N1*N/2 N3 = N2 + 1 S(1) = ZERO MU(1) = ZERO CALL DCOPY(N, S(1), 0, S, 1) CALL DCOPY(M, MU(1), 0, MU, 1) C RESET BFGS MATRIX 110 IRESET = IRESET + 1 IF (IRESET.GT.5) GO TO 255 L(1) = ZERO CALL DCOPY(N2, L(1), 0, L, 1) J = 1 DO 120 I=1,N L(J) = ONE J = J + N1 - I 120 CONTINUE C MAIN ITERATION : SEARCH DIRECTION, STEPLENGTH, LDL'-UPDATE 130 ITER = ITER + 1 MODE = 9 IF (ITER.GT.ITERMX) GO TO 330 C SEARCH DIRECTION AS SOLUTION OF QP - SUBPROBLEM CALL DCOPY(N, XL, 1, U, 1) CALL DCOPY(N, XU, 1, V, 1) CALL DAXPY(N, -ONE, X, 1, U, 1) CALL DAXPY(N, -ONE, X, 1, V, 1) H4 = ONE CALL LSQ (M, MEQ, N , N3, LA, L, G, A, C, U, V, S, R, W, IW, MODE) C AUGMENTED PROBLEM FOR INCONSISTENT LINEARIZATION IF (MODE.EQ.6) THEN IF (N.EQ.MEQ) THEN MODE = 4 ENDIF ENDIF IF (MODE.EQ.4) THEN DO 140 J=1,M IF (J.LE.MEQ) THEN A(J,N1) = -C(J) ELSE A(J,N1) = MAX(-C(J),ZERO) ENDIF 140 CONTINUE S(1) = ZERO CALL DCOPY(N, S(1), 0, S, 1) H3 = ZERO G(N1) = ZERO L(N3) = HUN S(N1) = ONE U(N1) = ZERO V(N1) = ONE INCONS = 0 150 CALL LSQ (M, MEQ, N1, N3, LA, L, G, A, C, U, V, S, R, * W, IW, MODE) H4 = ONE - S(N1) IF (MODE.EQ.4) THEN L(N3) = TEN*L(N3) INCONS = INCONS + 1 IF (INCONS.GT.5) GO TO 330 GOTO 150 ELSE IF (MODE.NE.1) THEN GOTO 330 ENDIF ELSE IF (MODE.NE.1) THEN GOTO 330 ENDIF C UPDATE MULTIPLIERS FOR L1-TEST DO 160 I=1,N V(I) = G(I) - DDOT(M,A(1,I),1,R,1) 160 CONTINUE F0 = F CALL DCOPY(N, X, 1, X0, 1) GS = DDOT(N, G, 1, S, 1) H1 = ABS(GS) H2 = ZERO DO 170 J=1,M IF (J.LE.MEQ) THEN H3 = C(J) ELSE H3 = ZERO ENDIF H2 = H2 + MAX(-C(J),H3) H3 = ABS(R(J)) MU(J) = MAX(H3,(MU(J)+H3)/TWO) H1 = H1 + H3*ABS(C(J)) 170 CONTINUE C CHECK CONVERGENCE MODE = 0 IF (H1.LT.ACC .AND. H2.LT.ACC) GO TO 330 H1 = ZERO DO 180 J=1,M IF (J.LE.MEQ) THEN H3 = C(J) ELSE H3 = ZERO ENDIF H1 = H1 + MU(J)*MAX(-C(J),H3) 180 CONTINUE T0 = F + H1 H3 = GS - H1*H4 MODE = 8 IF (H3.GE.ZERO) GO TO 110 C LINE SEARCH WITH AN L1-TESTFUNCTION LINE = 0 ALPHA = ONE IF (IEXACT.EQ.1) GOTO 210 C INEXACT LINESEARCH 190 LINE = LINE + 1 H3 = ALPHA*H3 CALL DSCAL(N, ALPHA, S, 1) CALL DCOPY(N, X0, 1, X, 1) CALL DAXPY(N, ONE, S, 1, X, 1) MODE = 1 GO TO 330 200 IF (H1.LE.H3/TEN .OR. LINE.GT.10) GO TO 240 ALPHA = MAX(H3/(TWO*(H3-H1)),ALFMIN) GO TO 190 C EXACT LINESEARCH 210 IF (LINE.NE.3) THEN ALPHA = LINMIN(LINE,ALFMIN,ONE,T,TOL) CALL DCOPY(N, X0, 1, X, 1) CALL DAXPY(N, ALPHA, S, 1, X, 1) MODE = 1 GOTO 330 ENDIF CALL DSCAL(N, ALPHA, S, 1) GOTO 240 C CALL FUNCTIONS AT CURRENT X 220 T = F DO 230 J=1,M IF (J.LE.MEQ) THEN H1 = C(J) ELSE H1 = ZERO ENDIF T = T + MU(J)*MAX(-C(J),H1) 230 CONTINUE H1 = T - T0 GOTO (200, 210) IEXACT+1 C CHECK CONVERGENCE 240 H3 = ZERO DO 250 J=1,M IF (J.LE.MEQ) THEN H1 = C(J) ELSE H1 = ZERO ENDIF H3 = H3 + MAX(-C(J),H1) 250 CONTINUE IF ((ABS(F-F0).LT.ACC .OR. DNRM2(N,S,1).LT.ACC) .AND. H3.LT.ACC) * THEN MODE = 0 ELSE MODE = -1 ENDIF GO TO 330 C CHECK relaxed CONVERGENCE in case of positive directional derivative 255 CONTINUE IF ((ABS(F-F0).LT.TOL .OR. DNRM2(N,S,1).LT.TOL) .AND. H3.LT.TOL) * THEN MODE = 0 ELSE MODE = 8 ENDIF GO TO 330 C CALL JACOBIAN AT CURRENT X C UPDATE CHOLESKY-FACTORS OF HESSIAN MATRIX BY MODIFIED BFGS FORMULA 260 DO 270 I=1,N U(I) = G(I) - DDOT(M,A(1,I),1,R,1) - V(I) 270 CONTINUE C L'*S K = 0 DO 290 I=1,N H1 = ZERO K = K + 1 DO 280 J=I+1,N K = K + 1 H1 = H1 + L(K)*S(J) 280 CONTINUE V(I) = S(I) + H1 290 CONTINUE C D*L'*S K = 1 DO 300 I=1,N V(I) = L(K)*V(I) K = K + N1 - I 300 CONTINUE C L*D*L'*S DO 320 I=N,1,-1 H1 = ZERO K = I DO 310 J=1,I - 1 H1 = H1 + L(K)*V(J) K = K + N - J 310 CONTINUE V(I) = V(I) + H1 320 CONTINUE H1 = DDOT(N,S,1,U,1) H2 = DDOT(N,S,1,V,1) H3 = 0.2D0*H2 IF (H1.LT.H3) THEN H4 = (H2-H3)/(H2-H1) H1 = H3 CALL DSCAL(N, H4, U, 1) CALL DAXPY(N, ONE-H4, V, 1, U, 1) ENDIF CALL LDL(N, L, U, +ONE/H1, V) CALL LDL(N, L, V, -ONE/H2, U) C END OF MAIN ITERATION GO TO 130 C END OF SLSQPB 330 END SUBROUTINE LSQ(M,MEQ,N,NL,LA,L,G,A,B,XL,XU,X,Y,W,JW,MODE) C MINIMIZE with respect to X C ||E*X - F|| C 1/2 T C WITH UPPER TRIANGULAR MATRIX E = +D *L , C -1/2 -1 C AND VECTOR F = -D *L *G, C WHERE THE UNIT LOWER TRIDIANGULAR MATRIX L IS STORED COLUMNWISE C DENSE IN THE N*(N+1)/2 ARRAY L WITH VECTOR D STORED IN ITS C 'DIAGONAL' THUS SUBSTITUTING THE ONE-ELEMENTS OF L C SUBJECT TO C A(J)*X - B(J) = 0 , J=1,...,MEQ, C A(J)*X - B(J) >=0, J=MEQ+1,...,M, C XL(I) <= X(I) <= XU(I), I=1,...,N, C ON ENTRY, THE USER HAS TO PROVIDE THE ARRAYS L, G, A, B, XL, XU. C WITH DIMENSIONS: L(N*(N+1)/2), G(N), A(LA,N), B(M), XL(N), XU(N) C THE WORKING ARRAY W MUST HAVE AT LEAST THE FOLLOWING DIMENSION: c DIM(W) = (3*N+M)*(N+1) for LSQ c +(N-MEQ+1)*(MINEQ+2) + 2*MINEQ for LSI c +(N+MINEQ)*(N-MEQ) + 2*MEQ + N for LSEI c with MINEQ = M - MEQ + 2*N C ON RETURN, NO ARRAY WILL BE CHANGED BY THE SUBROUTINE. C X STORES THE N-DIMENSIONAL SOLUTION VECTOR C Y STORES THE VECTOR OF LAGRANGE MULTIPLIERS OF DIMENSION C M+N+N (CONSTRAINTS+LOWER+UPPER BOUNDS) C MODE IS A SUCCESS-FAILURE FLAG WITH THE FOLLOWING MEANINGS: C MODE=1: SUCCESSFUL COMPUTATION C 2: ERROR RETURN BECAUSE OF WRONG DIMENSIONS (N<1) C 3: ITERATION COUNT EXCEEDED BY NNLS C 4: INEQUALITY CONSTRAINTS INCOMPATIBLE C 5: MATRIX E IS NOT OF FULL RANK C 6: MATRIX C IS NOT OF FULL RANK C 7: RANK DEFECT IN HFTI c coded Dieter Kraft, april 1987 c revised march 1989 DOUBLE PRECISION L,G,A,B,W,XL,XU,X,Y, . DIAG,ZERO,ONE,DDOT,XNORM INTEGER JW(*),I,IC,ID,IE,IF,IG,IH,IL,IM,IP,IU,IW, . I1,I2,I3,I4,LA,M,MEQ,MINEQ,MODE,M1,N,NL,N1,N2,N3 DIMENSION A(LA,N), B(LA), G(N), L(NL), . W(*), X(N), XL(N), XU(N), Y(M+N+N) DATA ZERO/0.0D0/, ONE/1.0D0/ N1 = N + 1 MINEQ = M - MEQ M1 = MINEQ + N + N c determine whether to solve problem c with inconsistent linerarization (n2=1) c or not (n2=0) N2 = N1*N/2 + 1 IF (N2.EQ.NL) THEN N2 = 0 ELSE N2 = 1 ENDIF N3 = N-N2 C RECOVER MATRIX E AND VECTOR F FROM L AND G I2 = 1 I3 = 1 I4 = 1 IE = 1 IF = N*N+1 DO 10 I=1,N3 I1 = N1-I DIAG = SQRT (L(I2)) W(I3) = ZERO CALL DCOPY (I1 , W(I3), 0, W(I3), 1) CALL DCOPY (I1-N2, L(I2), 1, W(I3), N) CALL DSCAL (I1-N2, DIAG, W(I3), N) W(I3) = DIAG W(IF-1+I) = (G(I) - DDOT (I-1, W(I4), 1, W(IF), 1))/DIAG I2 = I2 + I1 - N2 I3 = I3 + N1 I4 = I4 + N 10 CONTINUE IF (N2.EQ.1) THEN W(I3) = L(NL) W(I4) = ZERO CALL DCOPY (N3, W(I4), 0, W(I4), 1) W(IF-1+N) = ZERO ENDIF CALL DSCAL (N, - ONE, W(IF), 1) IC = IF + N ID = IC + MEQ*N IF (MEQ .GT. 0) THEN C RECOVER MATRIX C FROM UPPER PART OF A DO 20 I=1,MEQ CALL DCOPY (N, A(I,1), LA, W(IC-1+I), MEQ) 20 CONTINUE C RECOVER VECTOR D FROM UPPER PART OF B CALL DCOPY (MEQ, B(1), 1, W(ID), 1) CALL DSCAL (MEQ, - ONE, W(ID), 1) ENDIF IG = ID + MEQ IF (MINEQ .GT. 0) THEN C RECOVER MATRIX G FROM LOWER PART OF A DO 30 I=1,MINEQ CALL DCOPY (N, A(MEQ+I,1), LA, W(IG-1+I), M1) 30 CONTINUE ENDIF C AUGMENT MATRIX G BY +I AND -I IP = IG + MINEQ DO 40 I=1,N W(IP-1+I) = ZERO CALL DCOPY (N, W(IP-1+I), 0, W(IP-1+I), M1) 40 CONTINUE W(IP) = ONE CALL DCOPY (N, W(IP), 0, W(IP), M1+1) IM = IP + N DO 50 I=1,N W(IM-1+I) = ZERO CALL DCOPY (N, W(IM-1+I), 0, W(IM-1+I), M1) 50 CONTINUE W(IM) = -ONE CALL DCOPY (N, W(IM), 0, W(IM), M1+1) IH = IG + M1*N IF (MINEQ .GT. 0) THEN C RECOVER H FROM LOWER PART OF B CALL DCOPY (MINEQ, B(MEQ+1), 1, W(IH), 1) CALL DSCAL (MINEQ, - ONE, W(IH), 1) ENDIF C AUGMENT VECTOR H BY XL AND XU IL = IH + MINEQ CALL DCOPY (N, XL, 1, W(IL), 1) IU = IL + N CALL DCOPY (N, XU, 1, W(IU), 1) CALL DSCAL (N, - ONE, W(IU), 1) IW = IU + N CALL LSEI (W(IC), W(ID), W(IE), W(IF), W(IG), W(IH), MAX(1,MEQ), . MEQ, N, N, M1, M1, N, X, XNORM, W(IW), JW, MODE) IF (MODE .EQ. 1) THEN c restore Lagrange multipliers CALL DCOPY (M, W(IW), 1, Y(1), 1) CALL DCOPY (N3, W(IW+M), 1, Y(M+1), 1) CALL DCOPY (N3, W(IW+M+N), 1, Y(M+N3+1), 1) ENDIF C END OF SUBROUTINE LSQ END SUBROUTINE LSEI(C,D,E,F,G,H,LC,MC,LE,ME,LG,MG,N,X,XNRM,W,JW,MODE) C FOR MODE=1, THE SUBROUTINE RETURNS THE SOLUTION X OF C EQUALITY & INEQUALITY CONSTRAINED LEAST SQUARES PROBLEM LSEI : C MIN ||E*X - F|| C X C S.T. C*X = D, C G*X >= H. C USING QR DECOMPOSITION & ORTHOGONAL BASIS OF NULLSPACE OF C C CHAPTER 23.6 OF LAWSON & HANSON: SOLVING LEAST SQUARES PROBLEMS. C THE FOLLOWING DIMENSIONS OF THE ARRAYS DEFINING THE PROBLEM C ARE NECESSARY C DIM(E) : FORMAL (LE,N), ACTUAL (ME,N) C DIM(F) : FORMAL (LE ), ACTUAL (ME ) C DIM(C) : FORMAL (LC,N), ACTUAL (MC,N) C DIM(D) : FORMAL (LC ), ACTUAL (MC ) C DIM(G) : FORMAL (LG,N), ACTUAL (MG,N) C DIM(H) : FORMAL (LG ), ACTUAL (MG ) C DIM(X) : FORMAL (N ), ACTUAL (N ) C DIM(W) : 2*MC+ME+(ME+MG)*(N-MC) for LSEI C +(N-MC+1)*(MG+2)+2*MG for LSI C DIM(JW): MAX(MG,L) C ON ENTRY, THE USER HAS TO PROVIDE THE ARRAYS C, D, E, F, G, AND H. C ON RETURN, ALL ARRAYS WILL BE CHANGED BY THE SUBROUTINE. C X STORES THE SOLUTION VECTOR C XNORM STORES THE RESIDUUM OF THE SOLUTION IN EUCLIDIAN NORM C W STORES THE VECTOR OF LAGRANGE MULTIPLIERS IN ITS FIRST C MC+MG ELEMENTS C MODE IS A SUCCESS-FAILURE FLAG WITH THE FOLLOWING MEANINGS: C MODE=1: SUCCESSFUL COMPUTATION C 2: ERROR RETURN BECAUSE OF WRONG DIMENSIONS (N<1) C 3: ITERATION COUNT EXCEEDED BY NNLS C 4: INEQUALITY CONSTRAINTS INCOMPATIBLE C 5: MATRIX E IS NOT OF FULL RANK C 6: MATRIX C IS NOT OF FULL RANK C 7: RANK DEFECT IN HFTI C 18.5.1981, DIETER KRAFT, DFVLR OBERPFAFFENHOFEN C 20.3.1987, DIETER KRAFT, DFVLR OBERPFAFFENHOFEN INTEGER JW(*),I,IE,IF,IG,IW,J,K,KRANK,L,LC,LE,LG, . MC,MC1,ME,MG,MODE,N DOUBLE PRECISION C(LC,N),E(LE,N),G(LG,N),D(LC),F(LE),H(LG),X(N), . W(*),T,DDOT,XNRM,DNRM2,EPMACH,ZERO DATA EPMACH/2.22D-16/,ZERO/0.0D+00/ MODE=2 IF(MC.GT.N) GOTO 75 L=N-MC MC1=MC+1 IW=(L+1)*(MG+2)+2*MG+MC IE=IW+MC+1 IF=IE+ME*L IG=IF+ME C TRIANGULARIZE C AND APPLY FACTORS TO E AND G DO 10 I=1,MC J=MIN(I+1,LC) CALL H12(1,I,I+1,N,C(I,1),LC,W(IW+I),C(J,1),LC,1,MC-I) CALL H12(2,I,I+1,N,C(I,1),LC,W(IW+I),E ,LE,1,ME) 10 CALL H12(2,I,I+1,N,C(I,1),LC,W(IW+I),G ,LG,1,MG) C SOLVE C*X=D AND MODIFY F MODE=6 DO 15 I=1,MC IF(ABS(C(I,I)).LT.EPMACH) GOTO 75 X(I)=(D(I)-DDOT(I-1,C(I,1),LC,X,1))/C(I,I) 15 CONTINUE MODE=1 W(MC1) = ZERO CALL DCOPY (MG-MC,W(MC1),0,W(MC1),1) IF(MC.EQ.N) GOTO 50 DO 20 I=1,ME 20 W(IF-1+I)=F(I)-DDOT(MC,E(I,1),LE,X,1) C STORE TRANSFORMED E & G DO 25 I=1,ME 25 CALL DCOPY(L,E(I,MC1),LE,W(IE-1+I),ME) DO 30 I=1,MG 30 CALL DCOPY(L,G(I,MC1),LG,W(IG-1+I),MG) IF(MG.GT.0) GOTO 40 C SOLVE LS WITHOUT INEQUALITY CONSTRAINTS MODE=7 K=MAX(LE,N) T=SQRT(EPMACH) CALL HFTI (W(IE),ME,ME,L,W(IF),K,1,T,KRANK,XNRM,W,W(L+1),JW) CALL DCOPY(L,W(IF),1,X(MC1),1) IF(KRANK.NE.L) GOTO 75 MODE=1 GOTO 50 C MODIFY H AND SOLVE INEQUALITY CONSTRAINED LS PROBLEM 40 DO 45 I=1,MG 45 H(I)=H(I)-DDOT(MC,G(I,1),LG,X,1) CALL LSI . (W(IE),W(IF),W(IG),H,ME,ME,MG,MG,L,X(MC1),XNRM,W(MC1),JW,MODE) IF(MC.EQ.0) GOTO 75 T=DNRM2(MC,X,1) XNRM=SQRT(XNRM*XNRM+T*T) IF(MODE.NE.1) GOTO 75 C SOLUTION OF ORIGINAL PROBLEM AND LAGRANGE MULTIPLIERS 50 DO 55 I=1,ME 55 F(I)=DDOT(N,E(I,1),LE,X,1)-F(I) DO 60 I=1,MC 60 D(I)=DDOT(ME,E(1,I),1,F,1)-DDOT(MG,G(1,I),1,W(MC1),1) DO 65 I=MC,1,-1 65 CALL H12(2,I,I+1,N,C(I,1),LC,W(IW+I),X,1,1,1) DO 70 I=MC,1,-1 J=MIN(I+1,LC) W(I)=(D(I)-DDOT(MC-I,C(J,I),1,W(J),1))/C(I,I) 70 CONTINUE C END OF SUBROUTINE LSEI 75 END SUBROUTINE LSI(E,F,G,H,LE,ME,LG,MG,N,X,XNORM,W,JW,MODE) C FOR MODE=1, THE SUBROUTINE RETURNS THE SOLUTION X OF C INEQUALITY CONSTRAINED LINEAR LEAST SQUARES PROBLEM: C MIN ||E*X-F|| C X C S.T. G*X >= H C THE ALGORITHM IS BASED ON QR DECOMPOSITION AS DESCRIBED IN C CHAPTER 23.5 OF LAWSON & HANSON: SOLVING LEAST SQUARES PROBLEMS C THE FOLLOWING DIMENSIONS OF THE ARRAYS DEFINING THE PROBLEM C ARE NECESSARY C DIM(E) : FORMAL (LE,N), ACTUAL (ME,N) C DIM(F) : FORMAL (LE ), ACTUAL (ME ) C DIM(G) : FORMAL (LG,N), ACTUAL (MG,N) C DIM(H) : FORMAL (LG ), ACTUAL (MG ) C DIM(X) : N C DIM(W) : (N+1)*(MG+2) + 2*MG C DIM(JW): LG C ON ENTRY, THE USER HAS TO PROVIDE THE ARRAYS E, F, G, AND H. C ON RETURN, ALL ARRAYS WILL BE CHANGED BY THE SUBROUTINE. C X STORES THE SOLUTION VECTOR C XNORM STORES THE RESIDUUM OF THE SOLUTION IN EUCLIDIAN NORM C W STORES THE VECTOR OF LAGRANGE MULTIPLIERS IN ITS FIRST C MG ELEMENTS C MODE IS A SUCCESS-FAILURE FLAG WITH THE FOLLOWING MEANINGS: C MODE=1: SUCCESSFUL COMPUTATION C 2: ERROR RETURN BECAUSE OF WRONG DIMENSIONS (N<1) C 3: ITERATION COUNT EXCEEDED BY NNLS C 4: INEQUALITY CONSTRAINTS INCOMPATIBLE C 5: MATRIX E IS NOT OF FULL RANK C 03.01.1980, DIETER KRAFT: CODED C 20.03.1987, DIETER KRAFT: REVISED TO FORTRAN 77 INTEGER I,J,LE,LG,ME,MG,MODE,N,JW(LG) DOUBLE PRECISION E(LE,N),F(LE),G(LG,N),H(LG),X(N),W(*), . DDOT,XNORM,DNRM2,EPMACH,T,ONE DATA EPMACH/2.22D-16/,ONE/1.0D+00/ C QR-FACTORS OF E AND APPLICATION TO F DO 10 I=1,N J=MIN(I+1,N) CALL H12(1,I,I+1,ME,E(1,I),1,T,E(1,J),1,LE,N-I) 10 CALL H12(2,I,I+1,ME,E(1,I),1,T,F ,1,1 ,1 ) C TRANSFORM G AND H TO GET LEAST DISTANCE PROBLEM MODE=5 DO 30 I=1,MG DO 20 J=1,N IF (ABS(E(J,J)).LT.EPMACH) GOTO 50 20 G(I,J)=(G(I,J)-DDOT(J-1,G(I,1),LG,E(1,J),1))/E(J,J) 30 H(I)=H(I)-DDOT(N,G(I,1),LG,F,1) C SOLVE LEAST DISTANCE PROBLEM CALL LDP(G,LG,MG,N,H,X,XNORM,W,JW,MODE) IF (MODE.NE.1) GOTO 50 C SOLUTION OF ORIGINAL PROBLEM CALL DAXPY(N,ONE,F,1,X,1) DO 40 I=N,1,-1 J=MIN(I+1,N) 40 X(I)=(X(I)-DDOT(N-I,E(I,J),LE,X(J),1))/E(I,I) J=MIN(N+1,ME) T=DNRM2(ME-N,F(J),1) XNORM=SQRT(XNORM*XNORM+T*T) C END OF SUBROUTINE LSI 50 END SUBROUTINE LDP(G,MG,M,N,H,X,XNORM,W,INDEX,MODE) C T C MINIMIZE 1/2 X X SUBJECT TO G * X >= H. C C.L. LAWSON, R.J. HANSON: 'SOLVING LEAST SQUARES PROBLEMS' C PRENTICE HALL, ENGLEWOOD CLIFFS, NEW JERSEY, 1974. C PARAMETER DESCRIPTION: C G(),MG,M,N ON ENTRY G() STORES THE M BY N MATRIX OF C LINEAR INEQUALITY CONSTRAINTS. G() HAS FIRST C DIMENSIONING PARAMETER MG C H() ON ENTRY H() STORES THE M VECTOR H REPRESENTING C THE RIGHT SIDE OF THE INEQUALITY SYSTEM C REMARK: G(),H() WILL NOT BE CHANGED DURING CALCULATIONS BY LDP C X() ON ENTRY X() NEED NOT BE INITIALIZED. C ON EXIT X() STORES THE SOLUTION VECTOR X IF MODE=1. C XNORM ON EXIT XNORM STORES THE EUCLIDIAN NORM OF THE C SOLUTION VECTOR IF COMPUTATION IS SUCCESSFUL C W() W IS A ONE DIMENSIONAL WORKING SPACE, THE LENGTH C OF WHICH SHOULD BE AT LEAST (M+2)*(N+1) + 2*M C ON EXIT W() STORES THE LAGRANGE MULTIPLIERS C ASSOCIATED WITH THE CONSTRAINTS C AT THE SOLUTION OF PROBLEM LDP C INDEX() INDEX() IS A ONE DIMENSIONAL INTEGER WORKING SPACE C OF LENGTH AT LEAST M C MODE MODE IS A SUCCESS-FAILURE FLAG WITH THE FOLLOWING C MEANINGS: C MODE=1: SUCCESSFUL COMPUTATION C 2: ERROR RETURN BECAUSE OF WRONG DIMENSIONS (N.LE.0) C 3: ITERATION COUNT EXCEEDED BY NNLS C 4: INEQUALITY CONSTRAINTS INCOMPATIBLE DOUBLE PRECISION G,H,X,XNORM,W,U,V, . ZERO,ONE,FAC,RNORM,DNRM2,DDOT,DIFF INTEGER INDEX,I,IF,IW,IWDUAL,IY,IZ,J,M,MG,MODE,N,N1 DIMENSION G(MG,N),H(M),X(N),W(*),INDEX(M) DIFF(U,V)= U-V DATA ZERO,ONE/0.0D0,1.0D0/ MODE=2 IF(N.LE.0) GOTO 50 C STATE DUAL PROBLEM MODE=1 X(1)=ZERO CALL DCOPY(N,X(1),0,X,1) XNORM=ZERO IF(M.EQ.0) GOTO 50 IW=0 DO 20 J=1,M DO 10 I=1,N IW=IW+1 10 W(IW)=G(J,I) IW=IW+1 20 W(IW)=H(J) IF=IW+1 DO 30 I=1,N IW=IW+1 30 W(IW)=ZERO W(IW+1)=ONE N1=N+1 IZ=IW+2 IY=IZ+N1 IWDUAL=IY+M C SOLVE DUAL PROBLEM CALL NNLS (W,N1,N1,M,W(IF),W(IY),RNORM,W(IWDUAL),W(IZ),INDEX,MODE) IF(MODE.NE.1) GOTO 50 MODE=4 IF(RNORM.LE.ZERO) GOTO 50 C COMPUTE SOLUTION OF PRIMAL PROBLEM FAC=ONE-DDOT(M,H,1,W(IY),1) IF(DIFF(ONE+FAC,ONE).LE.ZERO) GOTO 50 MODE=1 FAC=ONE/FAC DO 40 J=1,N 40 X(J)=FAC*DDOT(M,G(1,J),1,W(IY),1) XNORM=DNRM2(N,X,1) C COMPUTE LAGRANGE MULTIPLIERS FOR PRIMAL PROBLEM W(1)=ZERO CALL DCOPY(M,W(1),0,W,1) CALL DAXPY(M,FAC,W(IY),1,W,1) C END OF SUBROUTINE LDP 50 END SUBROUTINE NNLS (A, MDA, M, N, B, X, RNORM, W, Z, INDEX, MODE) C C.L.LAWSON AND R.J.HANSON, JET PROPULSION LABORATORY: C 'SOLVING LEAST SQUARES PROBLEMS'. PRENTICE-HALL.1974 C ********** NONNEGATIVE LEAST SQUARES ********** C GIVEN AN M BY N MATRIX, A, AND AN M-VECTOR, B, COMPUTE AN C N-VECTOR, X, WHICH SOLVES THE LEAST SQUARES PROBLEM C A*X = B SUBJECT TO X >= 0 C A(),MDA,M,N C MDA IS THE FIRST DIMENSIONING PARAMETER FOR THE ARRAY,A(). C ON ENTRY A() CONTAINS THE M BY N MATRIX,A. C ON EXIT A() CONTAINS THE PRODUCT Q*A, C WHERE Q IS AN M BY M ORTHOGONAL MATRIX GENERATED C IMPLICITLY BY THIS SUBROUTINE. C EITHER M>=N OR M= M. EITHER M >= N OR M < N IS PERMITTED. C THERE IS NO RESTRICTION ON THE RANK OF A. C THE MATRIX A WILL BE MODIFIED BY THE SUBROUTINE. C B(*,*),MDB,NB IF NB = 0 THE SUBROUTINE WILL MAKE NO REFERENCE C TO THE ARRAY B. IF NB > 0 THE ARRAY B() MUST C INITIALLY CONTAIN THE M x NB MATRIX B OF THE C THE LEAST SQUARES PROBLEM AX = B AND ON RETURN C THE ARRAY B() WILL CONTAIN THE N x NB SOLUTION X. C IF NB>1 THE ARRAY B() MUST BE DOUBLE SUBSCRIPTED C WITH FIRST DIMENSIONING PARAMETER MDB>=MAX(M,N), C IF NB=1 THE ARRAY B() MAY BE EITHER SINGLE OR C DOUBLE SUBSCRIPTED. C TAU ABSOLUTE TOLERANCE PARAMETER FOR PSEUDORANK C DETERMINATION, PROVIDED BY THE USER. C KRANK PSEUDORANK OF A, SET BY THE SUBROUTINE. C RNORM ON EXIT, RNORM(J) WILL CONTAIN THE EUCLIDIAN C NORM OF THE RESIDUAL VECTOR FOR THE PROBLEM C DEFINED BY THE J-TH COLUMN VECTOR OF THE ARRAY B. C H(), G() ARRAYS OF WORKING SPACE OF LENGTH >= N. C IP() INTEGER ARRAY OF WORKING SPACE OF LENGTH >= N C RECORDING PERMUTATION INDICES OF COLUMN VECTORS INTEGER I,J,JB,K,KP1,KRANK,L,LDIAG,LMAX,M, . MDA,MDB,N,NB,IP(N) DOUBLE PRECISION A(MDA,N),B(MDB,NB),H(N),G(N),RNORM(NB),FACTOR, . TAU,ZERO,HMAX,DIFF,TMP,DDOT,DNRM2,U,V DIFF(U,V)= U-V DATA ZERO/0.0D0/, FACTOR/1.0D-3/ K=0 LDIAG=MIN(M,N) IF(LDIAG.LE.0) GOTO 270 C COMPUTE LMAX DO 80 J=1,LDIAG IF(J.EQ.1) GOTO 20 LMAX=J DO 10 L=J,N H(L)=H(L)-A(J-1,L)**2 10 IF(H(L).GT.H(LMAX)) LMAX=L IF(DIFF(HMAX+FACTOR*H(LMAX),HMAX).GT.ZERO) . GOTO 50 20 LMAX=J DO 40 L=J,N H(L)=ZERO DO 30 I=J,M 30 H(L)=H(L)+A(I,L)**2 40 IF(H(L).GT.H(LMAX)) LMAX=L HMAX=H(LMAX) C COLUMN INTERCHANGES IF NEEDED 50 IP(J)=LMAX IF(IP(J).EQ.J) GOTO 70 DO 60 I=1,M TMP=A(I,J) A(I,J)=A(I,LMAX) 60 A(I,LMAX)=TMP H(LMAX)=H(J) C J-TH TRANSFORMATION AND APPLICATION TO A AND B 70 I=MIN(J+1,N) CALL H12(1,J,J+1,M,A(1,J),1,H(J),A(1,I),1,MDA,N-J) 80 CALL H12(2,J,J+1,M,A(1,J),1,H(J),B,1,MDB,NB) C DETERMINE PSEUDORANK DO 90 J=1,LDIAG 90 IF(ABS(A(J,J)).LE.TAU) GOTO 100 K=LDIAG GOTO 110 100 K=J-1 110 KP1=K+1 C NORM OF RESIDUALS DO 130 JB=1,NB 130 RNORM(JB)=DNRM2(M-K,B(KP1,JB),1) IF(K.GT.0) GOTO 160 DO 150 JB=1,NB DO 150 I=1,N 150 B(I,JB)=ZERO GOTO 270 160 IF(K.EQ.N) GOTO 180 C HOUSEHOLDER DECOMPOSITION OF FIRST K ROWS DO 170 I=K,1,-1 170 CALL H12(1,I,KP1,N,A(I,1),MDA,G(I),A,MDA,1,I-1) 180 DO 250 JB=1,NB C SOLVE K*K TRIANGULAR SYSTEM DO 210 I=K,1,-1 J=MIN(I+1,N) 210 B(I,JB)=(B(I,JB)-DDOT(K-I,A(I,J),MDA,B(J,JB),1))/A(I,I) C COMPLETE SOLUTION VECTOR IF(K.EQ.N) GOTO 240 DO 220 J=KP1,N 220 B(J,JB)=ZERO DO 230 I=1,K 230 CALL H12(2,I,KP1,N,A(I,1),MDA,G(I),B(1,JB),1,MDB,1) C REORDER SOLUTION ACCORDING TO PREVIOUS COLUMN INTERCHANGES 240 DO 250 J=LDIAG,1,-1 IF(IP(J).EQ.J) GOTO 250 L=IP(J) TMP=B(L,JB) B(L,JB)=B(J,JB) B(J,JB)=TMP 250 CONTINUE 270 KRANK=K END SUBROUTINE H12 (MODE,LPIVOT,L1,M,U,IUE,UP,C,ICE,ICV,NCV) C C.L.LAWSON AND R.J.HANSON, JET PROPULSION LABORATORY, 1973 JUN 12 C TO APPEAR IN 'SOLVING LEAST SQUARES PROBLEMS', PRENTICE-HALL, 1974 C CONSTRUCTION AND/OR APPLICATION OF A SINGLE C HOUSEHOLDER TRANSFORMATION Q = I + U*(U**T)/B C MODE = 1 OR 2 TO SELECT ALGORITHM H1 OR H2 . C LPIVOT IS THE INDEX OF THE PIVOT ELEMENT. C L1,M IF L1 <= M THE TRANSFORMATION WILL BE CONSTRUCTED TO C ZERO ELEMENTS INDEXED FROM L1 THROUGH M. C IF L1 > M THE SUBROUTINE DOES AN IDENTITY TRANSFORMATION. C U(),IUE,UP C ON ENTRY TO H1 U() STORES THE PIVOT VECTOR. C IUE IS THE STORAGE INCREMENT BETWEEN ELEMENTS. C ON EXIT FROM H1 U() AND UP STORE QUANTITIES DEFINING C THE VECTOR U OF THE HOUSEHOLDER TRANSFORMATION. C ON ENTRY TO H2 U() AND UP C SHOULD STORE QUANTITIES PREVIOUSLY COMPUTED BY H1. C THESE WILL NOT BE MODIFIED BY H2. C C() ON ENTRY TO H1 OR H2 C() STORES A MATRIX WHICH WILL BE C REGARDED AS A SET OF VECTORS TO WHICH THE HOUSEHOLDER C TRANSFORMATION IS TO BE APPLIED. C ON EXIT C() STORES THE SET OF TRANSFORMED VECTORS. C ICE STORAGE INCREMENT BETWEEN ELEMENTS OF VECTORS IN C(). C ICV STORAGE INCREMENT BETWEEN VECTORS IN C(). C NCV NUMBER OF VECTORS IN C() TO BE TRANSFORMED. C IF NCV <= 0 NO OPERATIONS WILL BE DONE ON C(). INTEGER INCR, ICE, ICV, IUE, LPIVOT, L1, MODE, NCV INTEGER I, I2, I3, I4, J, M DOUBLE PRECISION U,UP,C,CL,CLINV,B,SM,ONE,ZERO DIMENSION U(IUE,*), C(*) DATA ONE/1.0D+00/, ZERO/0.0D+00/ IF (0.GE.LPIVOT.OR.LPIVOT.GE.L1.OR.L1.GT.M) GOTO 80 CL=ABS(U(1,LPIVOT)) IF (MODE.EQ.2) GOTO 30 C ****** CONSTRUCT THE TRANSFORMATION ****** DO 10 J=L1,M SM=ABS(U(1,J)) 10 CL=MAX(SM,CL) IF (CL.LE.ZERO) GOTO 80 CLINV=ONE/CL SM=(U(1,LPIVOT)*CLINV)**2 DO 20 J=L1,M 20 SM=SM+(U(1,J)*CLINV)**2 CL=CL*SQRT(SM) IF (U(1,LPIVOT).GT.ZERO) CL=-CL UP=U(1,LPIVOT)-CL U(1,LPIVOT)=CL GOTO 40 C ****** APPLY THE TRANSFORMATION I+U*(U**T)/B TO C ****** 30 IF (CL.LE.ZERO) GOTO 80 40 IF (NCV.LE.0) GOTO 80 B=UP*U(1,LPIVOT) IF (B.GE.ZERO) GOTO 80 B=ONE/B I2=1-ICV+ICE*(LPIVOT-1) INCR=ICE*(L1-LPIVOT) DO 70 J=1,NCV I2=I2+ICV I3=I2+INCR I4=I3 SM=C(I2)*UP DO 50 I=L1,M SM=SM+C(I3)*U(1,I) 50 I3=I3+ICE IF (SM.EQ.ZERO) GOTO 70 SM=SM*B C(I2)=C(I2)+SM*UP DO 60 I=L1,M C(I4)=C(I4)+SM*U(1,I) 60 I4=I4+ICE 70 CONTINUE 80 END SUBROUTINE LDL (N,A,Z,SIGMA,W) C LDL LDL' - RANK-ONE - UPDATE C PURPOSE: C UPDATES THE LDL' FACTORS OF MATRIX A BY RANK-ONE MATRIX C SIGMA*Z*Z' C INPUT ARGUMENTS: (* MEANS PARAMETERS ARE CHANGED DURING EXECUTION) C N : ORDER OF THE COEFFICIENT MATRIX A C * A : POSITIVE DEFINITE MATRIX OF DIMENSION N; C ONLY THE LOWER TRIANGLE IS USED AND IS STORED COLUMN BY C COLUMN AS ONE DIMENSIONAL ARRAY OF DIMENSION N*(N+1)/2. C * Z : VECTOR OF DIMENSION N OF UPDATING ELEMENTS C SIGMA : SCALAR FACTOR BY WHICH THE MODIFYING DYADE Z*Z' IS C MULTIPLIED C OUTPUT ARGUMENTS: C A : UPDATED LDL' FACTORS C WORKING ARRAY: C W : VECTOR OP DIMENSION N (USED ONLY IF SIGMA .LT. ZERO) C METHOD: C THAT OF FLETCHER AND POWELL AS DESCRIBED IN : C FLETCHER,R.,(1974) ON THE MODIFICATION OF LDL' FACTORIZATION. C POWELL,M.J.D. MATH.COMPUTATION 28, 1067-1078. C IMPLEMENTED BY: C KRAFT,D., DFVLR - INSTITUT FUER DYNAMIK DER FLUGSYSTEME C D-8031 OBERPFAFFENHOFEN C STATUS: 15. JANUARY 1980 C SUBROUTINES REQUIRED: NONE INTEGER I, IJ, J, N DOUBLE PRECISION A(*), T, V, W(*), Z(*), U, TP, ONE, BETA, FOUR, * ZERO, ALPHA, DELTA, GAMMA, SIGMA, EPMACH DATA ZERO, ONE, FOUR, EPMACH /0.0D0, 1.0D0, 4.0D0, 2.22D-16/ IF(SIGMA.EQ.ZERO) GOTO 280 IJ=1 T=ONE/SIGMA IF(SIGMA.GT.ZERO) GOTO 220 C PREPARE NEGATIVE UPDATE DO 150 I=1,N 150 W(I)=Z(I) DO 170 I=1,N V=W(I) T=T+V*V/A(IJ) DO 160 J=I+1,N IJ=IJ+1 160 W(J)=W(J)-V*A(IJ) 170 IJ=IJ+1 IF(T.GE.ZERO) T=EPMACH/SIGMA DO 210 I=1,N J=N+1-I IJ=IJ-I U=W(J) W(J)=T 210 T=T-U*U/A(IJ) 220 CONTINUE C HERE UPDATING BEGINS DO 270 I=1,N V=Z(I) DELTA=V/A(IJ) IF(SIGMA.LT.ZERO) TP=W(I) IF(SIGMA.GT.ZERO) TP=T+DELTA*V ALPHA=TP/T A(IJ)=ALPHA*A(IJ) IF(I.EQ.N) GOTO 280 BETA=DELTA/TP IF(ALPHA.GT.FOUR) GOTO 240 DO 230 J=I+1,N IJ=IJ+1 Z(J)=Z(J)-V*A(IJ) 230 A(IJ)=A(IJ)+BETA*Z(J) GOTO 260 240 GAMMA=T/TP DO 250 J=I+1,N IJ=IJ+1 U=A(IJ) A(IJ)=GAMMA*U+BETA*Z(J) 250 Z(J)=Z(J)-V*U 260 IJ=IJ+1 270 T=TP 280 RETURN C END OF LDL END DOUBLE PRECISION FUNCTION LINMIN (MODE, AX, BX, F, TOL) C LINMIN LINESEARCH WITHOUT DERIVATIVES C PURPOSE: C TO FIND THE ARGUMENT LINMIN WHERE THE FUNCTION F TAKES IT'S MINIMUM C ON THE INTERVAL AX, BX. C COMBINATION OF GOLDEN SECTION AND SUCCESSIVE QUADRATIC INTERPOLATION. C INPUT ARGUMENTS: (* MEANS PARAMETERS ARE CHANGED DURING EXECUTION) C *MODE SEE OUTPUT ARGUMENTS C AX LEFT ENDPOINT OF INITIAL INTERVAL C BX RIGHT ENDPOINT OF INITIAL INTERVAL C F FUNCTION VALUE AT LINMIN WHICH IS TO BE BROUGHT IN BY C REVERSE COMMUNICATION CONTROLLED BY MODE C TOL DESIRED LENGTH OF INTERVAL OF UNCERTAINTY OF FINAL RESULT C OUTPUT ARGUMENTS: C LINMIN ABSCISSA APPROXIMATING THE POINT WHERE F ATTAINS A MINIMUM C MODE CONTROLS REVERSE COMMUNICATION C MUST BE SET TO 0 INITIALLY, RETURNS WITH INTERMEDIATE C VALUES 1 AND 2 WHICH MUST NOT BE CHANGED BY THE USER, C ENDS WITH CONVERGENCE WITH VALUE 3. C WORKING ARRAY: C NONE C METHOD: C THIS FUNCTION SUBPROGRAM IS A SLIGHTLY MODIFIED VERSION OF THE C ALGOL 60 PROCEDURE LOCALMIN GIVEN IN C R.P. BRENT: ALGORITHMS FOR MINIMIZATION WITHOUT DERIVATIVES, C PRENTICE-HALL (1973). C IMPLEMENTED BY: C KRAFT, D., DFVLR - INSTITUT FUER DYNAMIK DER FLUGSYSTEME C D-8031 OBERPFAFFENHOFEN C STATUS: 31. AUGUST 1984 C SUBROUTINES REQUIRED: NONE INTEGER MODE DOUBLE PRECISION F, TOL, A, B, C, D, E, P, Q, R, U, V, W, X, M, & FU, FV, FW, FX, EPS, TOL1, TOL2, ZERO, AX, BX DATA C /0.381966011D0/, EPS /1.5D-8/, ZERO /0.0D0/ C EPS = SQUARE - ROOT OF MACHINE PRECISION C C = GOLDEN SECTION RATIO = (3-SQRT(5))/2 GOTO (10, 55), MODE C INITIALIZATION A = AX B = BX E = ZERO V = A + C*(B - A) W = V X = W LINMIN = X MODE = 1 GOTO 100 C MAIN LOOP STARTS HERE 10 FX = F FV = FX FW = FV 20 M = 0.5D0*(A + B) TOL1 = EPS*ABS(X) + TOL TOL2 = TOL1 + TOL1 C TEST CONVERGENCE IF (ABS(X - M) .LE. TOL2 - 0.5D0*(B - A)) GOTO 90 R = ZERO Q = R P = Q IF (ABS(E) .LE. TOL1) GOTO 30 C FIT PARABOLA R = (X - W)*(FX - FV) Q = (X - V)*(FX - FW) P = (X - V)*Q - (X - W)*R Q = Q - R Q = Q + Q IF (Q .GT. ZERO) P = -P IF (Q .LT. ZERO) Q = -Q R = E E = D C IS PARABOLA ACCEPTABLE 30 IF (ABS(P) .GE. 0.5D0*ABS(Q*R) .OR. & P .LE. Q*(A - X) .OR. P .GE. Q*(B-X)) GOTO 40 C PARABOLIC INTERPOLATION STEP D = P/Q C F MUST NOT BE EVALUATED TOO CLOSE TO A OR B IF (U - A .LT. TOL2) D = SIGN(TOL1, M - X) IF (B - U .LT. TOL2) D = SIGN(TOL1, M - X) GOTO 50 C GOLDEN SECTION STEP 40 IF (X .GE. M) E = A - X IF (X .LT. M) E = B - X D = C*E C F MUST NOT BE EVALUATED TOO CLOSE TO X 50 IF (ABS(D) .LT. TOL1) D = SIGN(TOL1, D) U = X + D LINMIN = U MODE = 2 GOTO 100 55 FU = F C UPDATE A, B, V, W, AND X IF (FU .GT. FX) GOTO 60 IF (U .GE. X) A = X IF (U .LT. X) B = X V = W FV = FW W = X FW = FX X = U FX = FU GOTO 85 60 IF (U .LT. X) A = U IF (U .GE. X) B = U IF (FU .LE. FW .OR. W .EQ. X) GOTO 70 IF (FU .LE. FV .OR. V .EQ. X .OR. V .EQ. W) GOTO 80 GOTO 85 70 V = W FV = FW W = U FW = FU GOTO 85 80 V = U FV = FU 85 GOTO 20 C END OF MAIN LOOP 90 LINMIN = X MODE = 3 100 RETURN C END OF LINMIN END C## Following a selection from BLAS Level 1 SUBROUTINE DAXPY(N,DA,DX,INCX,DY,INCY) C CONSTANT TIMES A VECTOR PLUS A VECTOR. C USES UNROLLED LOOPS FOR INCREMENTS EQUAL TO ONE. C JACK DONGARRA, LINPACK, 3/11/78. DOUBLE PRECISION DX(*),DY(*),DA INTEGER I,INCX,INCY,IX,IY,M,MP1,N IF(N.LE.0)RETURN IF(DA.EQ.0.0D0)RETURN IF(INCX.EQ.1.AND.INCY.EQ.1)GO TO 20 C CODE FOR UNEQUAL INCREMENTS OR EQUAL INCREMENTS C NOT EQUAL TO 1 IX = 1 IY = 1 IF(INCX.LT.0)IX = (-N+1)*INCX + 1 IF(INCY.LT.0)IY = (-N+1)*INCY + 1 DO 10 I = 1,N DY(IY) = DY(IY) + DA*DX(IX) IX = IX + INCX IY = IY + INCY 10 CONTINUE RETURN C CODE FOR BOTH INCREMENTS EQUAL TO 1 C CLEAN-UP LOOP 20 M = MOD(N,4) IF( M .EQ. 0 ) GO TO 40 DO 30 I = 1,M DY(I) = DY(I) + DA*DX(I) 30 CONTINUE IF( N .LT. 4 ) RETURN 40 MP1 = M + 1 DO 50 I = MP1,N,4 DY(I) = DY(I) + DA*DX(I) DY(I + 1) = DY(I + 1) + DA*DX(I + 1) DY(I + 2) = DY(I + 2) + DA*DX(I + 2) DY(I + 3) = DY(I + 3) + DA*DX(I + 3) 50 CONTINUE RETURN END SUBROUTINE DCOPY(N,DX,INCX,DY,INCY) C COPIES A VECTOR, X, TO A VECTOR, Y. C USES UNROLLED LOOPS FOR INCREMENTS EQUAL TO ONE. C JACK DONGARRA, LINPACK, 3/11/78. DOUBLE PRECISION DX(*),DY(*) INTEGER I,INCX,INCY,IX,IY,M,MP1,N IF(N.LE.0)RETURN IF(INCX.EQ.1.AND.INCY.EQ.1)GO TO 20 C CODE FOR UNEQUAL INCREMENTS OR EQUAL INCREMENTS C NOT EQUAL TO 1 IX = 1 IY = 1 IF(INCX.LT.0)IX = (-N+1)*INCX + 1 IF(INCY.LT.0)IY = (-N+1)*INCY + 1 DO 10 I = 1,N DY(IY) = DX(IX) IX = IX + INCX IY = IY + INCY 10 CONTINUE RETURN C CODE FOR BOTH INCREMENTS EQUAL TO 1 C CLEAN-UP LOOP 20 M = MOD(N,7) IF( M .EQ. 0 ) GO TO 40 DO 30 I = 1,M DY(I) = DX(I) 30 CONTINUE IF( N .LT. 7 ) RETURN 40 MP1 = M + 1 DO 50 I = MP1,N,7 DY(I) = DX(I) DY(I + 1) = DX(I + 1) DY(I + 2) = DX(I + 2) DY(I + 3) = DX(I + 3) DY(I + 4) = DX(I + 4) DY(I + 5) = DX(I + 5) DY(I + 6) = DX(I + 6) 50 CONTINUE RETURN END DOUBLE PRECISION FUNCTION DDOT(N,DX,INCX,DY,INCY) C FORMS THE DOT PRODUCT OF TWO VECTORS. C USES UNROLLED LOOPS FOR INCREMENTS EQUAL TO ONE. C JACK DONGARRA, LINPACK, 3/11/78. DOUBLE PRECISION DX(*),DY(*),DTEMP INTEGER I,INCX,INCY,IX,IY,M,MP1,N DDOT = 0.0D0 DTEMP = 0.0D0 IF(N.LE.0)RETURN IF(INCX.EQ.1.AND.INCY.EQ.1)GO TO 20 C CODE FOR UNEQUAL INCREMENTS OR EQUAL INCREMENTS C NOT EQUAL TO 1 IX = 1 IY = 1 IF(INCX.LT.0)IX = (-N+1)*INCX + 1 IF(INCY.LT.0)IY = (-N+1)*INCY + 1 DO 10 I = 1,N DTEMP = DTEMP + DX(IX)*DY(IY) IX = IX + INCX IY = IY + INCY 10 CONTINUE DDOT = DTEMP RETURN C CODE FOR BOTH INCREMENTS EQUAL TO 1 C CLEAN-UP LOOP 20 M = MOD(N,5) IF( M .EQ. 0 ) GO TO 40 DO 30 I = 1,M DTEMP = DTEMP + DX(I)*DY(I) 30 CONTINUE IF( N .LT. 5 ) GO TO 60 40 MP1 = M + 1 DO 50 I = MP1,N,5 DTEMP = DTEMP + DX(I)*DY(I) + DX(I + 1)*DY(I + 1) + * DX(I + 2)*DY(I + 2) + DX(I + 3)*DY(I + 3) + DX(I + 4)*DY(I + 4) 50 CONTINUE 60 DDOT = DTEMP RETURN END DOUBLE PRECISION FUNCTION DNRM1(N,X,I,J) INTEGER N, I, J, K DOUBLE PRECISION SNORMX, SUM, X(N), ZERO, ONE, SCALE, TEMP DATA ZERO/0.0D0/, ONE/1.0D0/ C DNRM1 - COMPUTES THE I-NORM OF A VECTOR C BETWEEN THE ITH AND THE JTH ELEMENTS C INPUT - C N LENGTH OF VECTOR C X VECTOR OF LENGTH N C I INITIAL ELEMENT OF VECTOR TO BE USED C J FINAL ELEMENT TO USE C OUTPUT - C DNRM1 NORM SNORMX=ZERO DO 10 K=I,J 10 SNORMX=MAX(SNORMX,ABS(X(K))) DNRM1 = SNORMX IF (SNORMX.EQ.ZERO) RETURN SCALE = SNORMX IF (SNORMX.GE.ONE) SCALE=SQRT(SNORMX) SUM=ZERO DO 20 K=I,J TEMP=ZERO IF (ABS(X(K))+SCALE .NE. SCALE) TEMP = X(K)/SNORMX IF (ONE+TEMP.NE.ONE) SUM = SUM+TEMP*TEMP 20 CONTINUE SUM=SQRT(SUM) DNRM1=SNORMX*SUM RETURN END DOUBLE PRECISION FUNCTION DNRM2 ( N, DX, INCX) INTEGER N, I, J, NN, NEXT, INCX DOUBLE PRECISION DX(*), CUTLO, CUTHI, HITEST, SUM, XMAX, ZERO, ONE DATA ZERO, ONE /0.0D0, 1.0D0/ C EUCLIDEAN NORM OF THE N-VECTOR STORED IN DX() WITH STORAGE C INCREMENT INCX . C IF N .LE. 0 RETURN WITH RESULT = 0. C IF N .GE. 1 THEN INCX MUST BE .GE. 1 C C.L.LAWSON, 1978 JAN 08 C FOUR PHASE METHOD USING TWO BUILT-IN CONSTANTS THAT ARE C HOPEFULLY APPLICABLE TO ALL MACHINES. C CUTLO = MAXIMUM OF SQRT(U/EPS) OVER ALL KNOWN MACHINES. C CUTHI = MINIMUM OF SQRT(V) OVER ALL KNOWN MACHINES. C WHERE C EPS = SMALLEST NO. SUCH THAT EPS + 1. .GT. 1. C U = SMALLEST POSITIVE NO. (UNDERFLOW LIMIT) C V = LARGEST NO. (OVERFLOW LIMIT) C BRIEF OUTLINE OF ALGORITHM.. C PHASE 1 SCANS ZERO COMPONENTS. C MOVE TO PHASE 2 WHEN A COMPONENT IS NONZERO AND .LE. CUTLO C MOVE TO PHASE 3 WHEN A COMPONENT IS .GT. CUTLO C MOVE TO PHASE 4 WHEN A COMPONENT IS .GE. CUTHI/M C WHERE M = N FOR X() REAL AND M = 2*N FOR COMPLEX. C VALUES FOR CUTLO AND CUTHI.. C FROM THE ENVIRONMENTAL PARAMETERS LISTED IN THE IMSL CONVERTER C DOCUMENT THE LIMITING VALUES ARE AS FOLLOWS.. C CUTLO, S.P. U/EPS = 2**(-102) FOR HONEYWELL. CLOSE SECONDS ARE C UNIVAC AND DEC AT 2**(-103) C THUS CUTLO = 2**(-51) = 4.44089E-16 C CUTHI, S.P. V = 2**127 FOR UNIVAC, HONEYWELL, AND DEC. C THUS CUTHI = 2**(63.5) = 1.30438E19 C CUTLO, D.P. U/EPS = 2**(-67) FOR HONEYWELL AND DEC. C THUS CUTLO = 2**(-33.5) = 8.23181D-11 C CUTHI, D.P. SAME AS S.P. CUTHI = 1.30438D19 C DATA CUTLO, CUTHI / 8.232D-11, 1.304D19 / C DATA CUTLO, CUTHI / 4.441E-16, 1.304E19 / DATA CUTLO, CUTHI / 8.232D-11, 1.304D19 / IF(N .GT. 0) GO TO 10 DNRM2 = ZERO GO TO 300 10 ASSIGN 30 TO NEXT SUM = ZERO NN = N * INCX C BEGIN MAIN LOOP I = 1 20 GO TO NEXT,(30, 50, 70, 110) 30 IF( ABS(DX(I)) .GT. CUTLO) GO TO 85 ASSIGN 50 TO NEXT XMAX = ZERO C PHASE 1. SUM IS ZERO 50 IF( DX(I) .EQ. ZERO) GO TO 200 IF( ABS(DX(I)) .GT. CUTLO) GO TO 85 C PREPARE FOR PHASE 2. ASSIGN 70 TO NEXT GO TO 105 C PREPARE FOR PHASE 4. 100 I = J ASSIGN 110 TO NEXT SUM = (SUM / DX(I)) / DX(I) 105 XMAX = ABS(DX(I)) GO TO 115 C PHASE 2. SUM IS SMALL. C SCALE TO AVOID DESTRUCTIVE UNDERFLOW. 70 IF( ABS(DX(I)) .GT. CUTLO ) GO TO 75 C COMMON CODE FOR PHASES 2 AND 4. C IN PHASE 4 SUM IS LARGE. SCALE TO AVOID OVERFLOW. 110 IF( ABS(DX(I)) .LE. XMAX ) GO TO 115 SUM = ONE + SUM * (XMAX / DX(I))**2 XMAX = ABS(DX(I)) GO TO 200 115 SUM = SUM + (DX(I)/XMAX)**2 GO TO 200 C PREPARE FOR PHASE 3. 75 SUM = (SUM * XMAX) * XMAX C FOR REAL OR D.P. SET HITEST = CUTHI/N C FOR COMPLEX SET HITEST = CUTHI/(2*N) 85 HITEST = CUTHI/FLOAT( N ) C PHASE 3. SUM IS MID-RANGE. NO SCALING. DO 95 J =I,NN,INCX IF(ABS(DX(J)) .GE. HITEST) GO TO 100 95 SUM = SUM + DX(J)**2 DNRM2 = SQRT( SUM ) GO TO 300 200 CONTINUE I = I + INCX IF ( I .LE. NN ) GO TO 20 C END OF MAIN LOOP. C COMPUTE SQUARE ROOT AND ADJUST FOR SCALING. DNRM2 = XMAX * SQRT(SUM) 300 CONTINUE RETURN END SUBROUTINE DROT (N,DX,INCX,DY,INCY,C,S) C APPLIES A PLANE ROTATION. C JACK DONGARRA, LINPACK, 3/11/78. DOUBLE PRECISION DX(*),DY(*),DTEMP,C,S INTEGER I,INCX,INCY,IX,IY,N IF(N.LE.0)RETURN IF(INCX.EQ.1.AND.INCY.EQ.1)GO TO 20 C CODE FOR UNEQUAL INCREMENTS OR EQUAL INCREMENTS NOT EQUAL C TO 1 IX = 1 IY = 1 IF(INCX.LT.0)IX = (-N+1)*INCX + 1 IF(INCY.LT.0)IY = (-N+1)*INCY + 1 DO 10 I = 1,N DTEMP = C*DX(IX) + S*DY(IY) DY(IY) = C*DY(IY) - S*DX(IX) DX(IX) = DTEMP IX = IX + INCX IY = IY + INCY 10 CONTINUE RETURN C CODE FOR BOTH INCREMENTS EQUAL TO 1 20 DO 30 I = 1,N DTEMP = C*DX(I) + S*DY(I) DY(I) = C*DY(I) - S*DX(I) DX(I) = DTEMP 30 CONTINUE RETURN END SUBROUTINE DROTG(DA,DB,C,S) C CONSTRUCT GIVENS PLANE ROTATION. C JACK DONGARRA, LINPACK, 3/11/78. C MODIFIED 9/27/86. DOUBLE PRECISION DA,DB,C,S,ROE,SCALE,R,Z,ONE,ZERO DATA ONE, ZERO /1.0D+00, 0.0D+00/ ROE = DB IF( ABS(DA) .GT. ABS(DB) ) ROE = DA SCALE = ABS(DA) + ABS(DB) IF( SCALE .NE. ZERO ) GO TO 10 C = ONE S = ZERO R = ZERO GO TO 20 10 R = SCALE*SQRT((DA/SCALE)**2 + (DB/SCALE)**2) R = SIGN(ONE,ROE)*R C = DA/R S = DB/R 20 Z = S IF( ABS(C) .GT. ZERO .AND. ABS(C) .LE. S ) Z = ONE/C DA = R DB = Z RETURN END SUBROUTINE DSCAL(N,DA,DX,INCX) C SCALES A VECTOR BY A CONSTANT. C USES UNROLLED LOOPS FOR INCREMENT EQUAL TO ONE. C JACK DONGARRA, LINPACK, 3/11/78. DOUBLE PRECISION DA,DX(*) INTEGER I,INCX,M,MP1,N,NINCX IF(N.LE.0)RETURN IF(INCX.EQ.1)GO TO 20 C CODE FOR INCREMENT NOT EQUAL TO 1 NINCX = N*INCX DO 10 I = 1,NINCX,INCX DX(I) = DA*DX(I) 10 CONTINUE RETURN C CODE FOR INCREMENT EQUAL TO 1 C CLEAN-UP LOOP 20 M = MOD(N,5) IF( M .EQ. 0 ) GO TO 40 DO 30 I = 1,M DX(I) = DA*DX(I) 30 CONTINUE IF( N .LT. 5 ) RETURN 40 MP1 = M + 1 DO 50 I = MP1,N,5 DX(I) = DA*DX(I) DX(I + 1) = DA*DX(I + 1) DX(I + 2) = DA*DX(I + 2) DX(I + 3) = DA*DX(I + 3) DX(I + 4) = DA*DX(I + 4) 50 CONTINUE RETURN END ************************************************************************ * test driver for TOMP optimal control calculations * * state inequality constrained brachistochrone problem * ************************************************************************ * np = number of design parameters * nu = number of control functions * nm = interpolation mode (4 = spline under tension) * ny = number of communication points * (at the respective knots the state constraints are evaluated) * nc = number of knots of control discretization * nz = number of state variables (differential equations) * me = number of equality constraints * m = total number of constraints * n = total number of variables * nx = length of vector used for interpolation (see spline) * lw, ljw = length of working arrays for subroutine slsqp parameter (np=1, nu=1, nm=4, ny=21, nc=21, nz=3, me=1) parameter (n=np+nu*nc, nx=np+6*nu*nc, m=me+ny-2) * the following parameter declaration is for the * unconstrained problem * parameter (n=np+nu*nc, nx=np+6*nu*nc, m=me ) parameter (n1=n+1, meq=me, mineq=m-meq+n1+n1) parameter (len_w= $ (3*n1+m)*(n1+1) $ +(n1-meq+1)*(mineq+2) + 2*mineq $ +(n1+mineq)*(n1-meq) + 2*meq + n1 $ +(n+1)*n/2 + 2*m + 3*n + 3*n1 + 1, $ len_jw=mineq) parameter (lw=len_w, ljw=len_jw) integer iv(20), jw(ljw) double precision x(nx), y(ny), z(nz), za(nz), zb(nz), 1 c(m), df(n1), dc(m,n1), w(lw), 2 xl(n), xu(n), dfloat, acc, ac1, f, h external rk87, bra, vbra, obra * elements of vector iv are explained in subroutine d_tomp data iv/nc,nm,0,0,0,0,0,0,0,0,0,0, 1 np,nu,ny,nz,m,1,0,110/ do 1, i=1,nc * initial control parameters set to zero * and bounds xl(i)=0.0d0 x (i)=0.0d0 xu(i)=2.0d0 j=nc+np+i * nc equidistant control points * the time interval is normalized * to 0 .le. t .le. 1 * therefore dz must be multiplied by tf in subroutine bra x(j)=dfloat(i-1)/dfloat(nc-1) k=nc+nc+np+i * tension parameters x(k)=0.0d0 if (i.gt.6 .and. i.lt.14) x(k)=1.0d3 1 continue * initial design parameter (final time tf) set to two * and bounds xl(nc+np)=1.0d0 x (nc+np)=2.0d0 xu(nc+np)=2.0d0 * ny equidistant communication points do 5, i=1,ny 5 y(i)=dfloat(i-1)/dfloat(ny-1) * accuracy of optimizer acc = 1.0d-06 * accuracy of simulator ac1 = 1.0d-08 * itr iterations are allowed itr = 50 mode = 0 l = 19 open (l, file = 'tomp.out') write(l,*) ' constrained brachistochrone problem' write(l,'(10h iteration, 16h value of cost , 1 23h constraint violations)') c optimization loop (see fig.1 in accompanying VDI-report) 10 iv(19) = mode * simulator call d_tomp (rk87,bra,vbra,obra,iv,x,y,z,za,zb,ac1,f,c,df,dc,m) * empty user interface * optimizer call slsqp(m,me,m,n,x,xl,xu,f,c,df,dc,acc,itr,mode,w,lw,jw,ljw) if (mode .eq. 0 .or. mode .eq. -1) then h=0.0d0 do 15 i=1,m if (i.le.me) then h=h+c(i)**2 else h=h+min(c(i),0.0d0)**2 endif 15 continue * print iteration, cost function, norm of constraint violations write(l,'(3x,i3,4x,d16.8,5x,d12.4)') itr, f, sqrt(h) endif if (iabs(mode) .eq. 1) goto 10 c end of optimization loop write 1 (l, '(5x,7hmode =, i4, 8h after , i3, 12h iterations)') 2 mode, itr close(l) * simulation of final solution with dense output * for graphical post-processing in MATLAB or RASP iv(19) = -2 call d_tomp (rk87,bra,vbra,obra,iv,x,y,z,za,zb,ac1,f,c,df,dc,m) * results will be found in file TOMP.M * as appropriate MATLAB matrices for plotting * if MATLAB is available to you, just type * MATLAB * >tomp * >plot(x1,-x2), grid * (and you should see fig.2 of accompanying VDI-report on screen) end subroutine bra(t,z,dz,u,iu) * right hand sides of differential equations * of brachistochrone problem double precision t, z(*), dz(*), u(*) double precision g, theta(1), tf integer iu(20) data g/1.0d0/ nc=iu(01) np=iu(13) tf=u(nc+np) call contrl(t,u,iu,theta) * the time interval is normalized * to 0 .le. t .le. 1 * therefore dz must be multiplied by tf dz(1)=tf*z(3)*cos(theta(1)) dz(2)=tf*z(3)*sin(theta(1)) dz(3)=tf*g*sin(theta(1)) end subroutine vbra(rhs,iu,t,u,y,z,z0,z1,w,f,c,mode) * boundary values, state constraints, and cost function * of brachistochrone problem double precision t,u(*),y(*),z(*),z0(*),z1(*),f,c(*),w(*) integer iu(20), mode external rhs save j nc=iu(01) np=iu(13) ny=iu(15) nz=iu(16) m =iu(17) c boundary values * mode.eq.0 at start of each simulation if (mode.eq.0) then j=1 * initial states z0(1)=0.0d0 z0(2)=0.0d0 z0(3)=0.0d0 * final states (z1(2) & z1(3) are free) z1(1)=1.0d0 z1(2)=0.0d0 z1(3)=0.0d0 end if c state constraints * m.eq.1 means equality constrained problem only if (m.ne.1) then * mode.eq.1 and mode.eq.ny are covered by left and right boundary if (mode.gt.1 .and. mode.lt.ny) then j=j+1 c(j)=0.2d0+0.4d0*z(1)-z(2) end if end if c cost and final states if (mode.eq.ny) then f=u(nc+np) * c(j) for j=1 (equality constraints -- in our case one -- first) c(1)=z(1)-z1(1) end if end subroutine obra (t, k, g, l, n, z, u, iu) * trajectory data (time, states, and controls) * of brachistochrone problem * for possible output on screen or into file double precision t, z(*), u(*) double precision gam(1) real g(l,*) integer iu(*) * call contrl(t,u,iu,gam) * write(*,'(8(1pd10.2))') t, (z(i),i=1,2), gam(1) end ************************************************************************ * end of test driver for TOMP optimal control calculations * * state inequality constrained brachistochrone problem * ************************************************************************