Changeset 5959 in project


Ignore:
Timestamp:
09/09/07 15:34:12 (12 years ago)
Author:
iraikov
Message:

Incorporated the CVODE routines for choosing initial timestep.

File:
1 edited

Legend:

Unmodified
Added
Removed
  • ode-lmm/trunk/rkg4.scm

    r5955 r5959  
    3434(declare (export ode:rkg4))
    3535
    36 (define Ct32 2)
    37 (define Ct21 (/ 3 2))
    3836
    3937(define Ch32  (/ 3 2))
     
    4442(define Ch33  (/ 9 8))
    4543
     44(define C2t     .33)
     45(define C3t     .66)
     46
    4647
    4748;; machine epsilon
     
    5152      (if (= 1.0 (+ 1.0 x1)) x (loop x1)))))
    5253
    53 ;; unit round-off error for IEEE double-precision floating point
    54 (define u 4.4409e-16)
    55 
    56 ;; scaling tolerance when computing lambda
    57 (define  stol  1e-6)
    58 
    59 (define (rkg4 startit t tstep gdval)
    60 
    61   (let loop ((it startit) (t t) (gdval gdval))
    62    
    63     (if gdval
    64         (begin
    65           ;; update the state values
     54
     55(define (ode:rkg4 ode-runtime indep tstart tstop initial-tstep)
     56
     57  (define maxerror   (ode-runtime 'maxerror))
     58  (define hierror?   (ode-runtime 'hierror?))
     59  (define lowerror?  (ode-runtime 'lowerror?))
     60
     61  (define printq  (ode-runtime 'print))
     62  (define eval-rhs  (ode-runtime 'eval))
     63  (define is-state? (ode-runtime 'is-state?))
     64  (define is-asgn?  (ode-runtime 'is-asgn?))
     65  (define solve-env-ref  (ode-runtime 'solve-env-ref))
     66  (define solve-env-set! (ode-runtime 'solve-env-set!))
     67  (define eval-env-ref  (ode-runtime 'eval-env-ref))
     68  (define eval-env-set! (ode-runtime 'eval-env-set!))
     69
     70  (define svec-deriv-idx  (ode-runtime 'svec-deriv-idx))
     71  (define svec-value-idx  (ode-runtime 'svec-value-idx))
     72  (define svec-relerr-idx  (ode-runtime 'svec-relerr-idx))
     73  (define svec-abserr-idx  (ode-runtime 'svec-abserr-idx))
     74 
     75  (define (svec-deriv svec) (f64vector-ref svec svec-deriv-idx))
     76  (define (svec-value svec) (f64vector-ref svec svec-value-idx))
     77  (define (svec-relerr svec) (f64vector-ref svec svec-relerr-idx))
     78  (define (svec-abserr svec) (f64vector-ref svec svec-abserr-idx))
     79
     80  (define (svec-deriv-set! svec v) (f64vector-set! svec svec-deriv-idx v))
     81  (define (svec-value-set! svec v) (f64vector-set! svec svec-value-idx v))
     82  (define (svec-relerr-set! svec v) (f64vector-set! svec svec-relerr-idx v))
     83  (define (svec-abserr-set! svec v) (f64vector-set! svec svec-abserr-idx v))
     84
     85  (define yorder  (ode-runtime 'order))
     86  (define (make-yvect)  (make-f64vector yorder  0.0))
     87
     88  ;; maximum and minimum ratios by which to increase to decrease the
     89  ;; step size
     90  (define M                (ode-runtime'  hmax-factor))
     91  (define m                (ode-runtime'  hmin-factor))
     92  (define S                (ode-runtime'  hscale-factor))
     93  (define relmax           (ode-runtime'  relmax))
     94  (define absmax           (ode-runtime'  absmax))
     95
     96  (define hub-factor  0.1)
     97  (define hlb-factor  100.0)
     98  (define h-bias      0.5)
     99
     100  (define yvect    (make-yvect))
     101  (define y1vect   (make-yvect))
     102  (define nvect    (make-vector yorder #f))
     103
     104  ;; approximation of (||y'/y||_max)^4
     105  (define lam4  #f)
     106
     107  ;; upper and lower bounds for timestep
     108  (define hub   #f)
     109  (define hlb   #f)
     110 
     111  (define (eval-derivs!)
     112    ((ode-runtime 'eval-order-foreach)
     113     (lambda (order sym rhs)
     114       (let ((v     (eval-rhs rhs))
     115             (svec  (solve-env-ref sym)))
     116         (cond ((is-state? sym)  (begin
     117                                   (svec-deriv-set! svec v)))
     118               ((is-asgn? sym)   (begin
     119                                   (eval-env-set! sym v)
     120                                   (svec-value-set! svec v))))))))
     121
     122  (define (eval-derivs t)
     123    (let ((y1vect   (make-yvect)))
     124      (let ((oldt   (eval-env-ref indep)))
     125        (eval-env-set! indep t)
     126        ((ode-runtime 'eval-order-fold)
     127         (lambda (order sym rhs i)
     128           (let ((v     (eval-rhs rhs))
     129                 (svec  (solve-env-ref sym)))
     130             (cond ((is-state? sym)  (f64vector-set! y1vect i v)))
     131             (fx+ 1 i))) 0)
     132        (eval-env-set! indep oldt)
     133        y1vect)))
     134     
     135
     136
     137  ;;
     138  ;; Based on CVYddNorm from CVODE
     139  ;;
     140  ;; This routine computes an estimate of the second derivative of y
     141  ;; using a difference quotient, and returns its WRMS norm.
     142  ;;
     143  (define (yddnorm t hg)
     144    ;; y     <- h*y'(t) + y(t)
     145    (let ((y  (blas:daxpy hg y1vect 1.0 yvect)))
     146      ;; f(tn+hg, y, tempv, f_data);
     147      ;; yd <- f(t+hg, y(t)) - y'(t)
     148      (let* ((yd   (eval-derivs (+ t hg)))
     149             (yd1  (blas:daxpy yorder 1.0 yd -1.0 y1vect))
     150             (ydd  (blas:dscal yorder (/ 1.0 hg) yd1)))
     151 
     152        ;; Estimate ||y''||
     153        (blas:dnrm2 yorder ydd))))
     154
     155
     156  ;;
     157  ;; Based on CVUpperBoundH0 routine from CVODE
     158  ;;
     159  ;; This routine sets an upper bound on abs(h0) based on
     160  ;; tdist = tstop - tstart and the values of y[i] / y'[i]
     161  ;;
     162  (define (compute-h0-upper-bound tdist)
     163    (define  vabsmax?  (f64vector? absmax))
     164
     165    (let ((yabs   (f64vector-abs  yvect))
     166          (y1abs  (f64vector-abs  y1vect)))
     167      (let ((yabs+tol  (if vabsmax?
     168                           (blas:daxpy yorder hub-factor yabs 1.0 absmax)
     169                           (blas:daxpy yorder hub-factor yabs absmax (f64vector yorder 1.0)))))
     170
     171        (let ((y1/y  (f64vector-div y1abs yabs+tol)))
     172          (let ((y1/y-maxnorm  (f64vector-ref y1/y (blas:damax yorder y1/y)))
     173                (hub           (* hub-factor tdist)))
     174            (if (> (* hub hub-inv) 1.0) 
     175                (values (/ 1.0 y1/y-maxnorm)  y1/y-maxnorm)
     176                (values hub y1/y-maxnorm)))))))
     177
     178
     179  ;;
     180  ;; Based on CVHin routine from CVODE
     181  ;;
     182  ;; This routine computes a tentative initial step size h0.
     183  ;;
     184  ;; The algorithm used seeks to find h0 as a solution of
     185  ;;
     186  ;;       (WRMS norm of (h0^2 ydd / 2)) = 1,
     187  ;;
     188  ;; where ydd = estimated second derivative of y.
     189  ;;
     190  (define (initialh)
     191    (define max-iters   4)
     192
     193    (let ((tdiff  (- tstop tstart)))
     194      (let ((sign   (negative? tdiff))
     195            (tdist  (abs tdiff))
     196            (tround (let ((tround (* uround (max (abs tstart) (abs tstop)))))
     197                      (if (< tdist (* 2.0 tround)) (error "tstop too close to tstart"))
     198                      tround)))
     199        ;; Set lower and upper bounds on h0, and take geometric mean
     200        ;; Exit with this value if the bounds cross each other
     201        (let ((hlb  (* hlb-factor tround)))
     202          (let-values (((hub  y1/y-maxnorm)  (compute-h0-upper-bound tdist)))
     203            (set! lam4 (expt y1/y-maxnorm 4))
     204            (set! hlb  hlb)
     205            (set! hub  hub)
     206            (let ((hg  (sqrt (* hlb ulb))))
     207              (if (< hub hlb)
     208                  (if sign  (- hg) hg)
     209                  ;; Loop up to max-iters times to find h0.
     210                  ;; Stop if new and previous values differ by a factor < 2.
     211                  ;; Stop if hnew/hg > 2 after one iteration, as this probably means
     212                  ;; that the ydd value is bad because of cancellation error.       
     213                  (let ((hnew  (let loop ((i 0)  (hnew hg) (hprev hg) (hrat 0.0))
     214                                 (cond ((>= i max-iters)  hnew)
     215                                       ((and (> hrat 0.5) (< hrat 2.0))  hnew)
     216                                       ((and (>= i 2) (> hrat 2.0))      hprev)
     217                                       (else
     218                                        (let* ((hgs     (* hnew (if sign -1 1)))
     219                                               (yddnrm  (yddnorm tstart hgs))
     220                                               (hnew1   (if (> (* yddnrm hub hub) 2.0)
     221                                                            (sqrt (/ 2.0 yddnrm)) (sqrt (* hnew hub)))))
     222                                          (loop (fx+ 1 i) hnew1 hnew (/ hnew1 hnew))))))))
     223                    ;; Apply bounds, bias factor, and attach sign.
     224                    (let ((h0  (* h-bias hnew)))
     225                      (cond ((< h0 hlb)  hlb)
     226                            ((> h0 hub)  hub)
     227                            (else   (if sign (- h0) h0))))))))))))
     228
     229
     230
     231  (define (rkg4 startit t tstep gdval h0)
     232
     233    (let loop ((it startit) (t t) (gdval gdval))
     234     
     235      (if gdval
     236          (begin
     237            ;; update the state values
    66238            ((ode-runtime 'solve-env-state-fold)
    67239             (lambda (sym svec i)
     
    71243               (fx+ i 1))  0)
    72244            ;; update lambda (eq. 5.16)
    73             (set! lam (/ (blas:dnrm2 order y1vect) (blas:dnrm2 order yvect)))
     245            (set! lam (/ y1norm  ynorm))
    74246            ;; update independent variable
    75247            (solve-env-set! indep svec-value-idx t)
     
    82254     
    83255      (if (t<tstop tstep t tstop)
    84           (begin
     256          (let ((h  (/ tstep 3)))
    85257           
    86258            (if (> (* tstep (- (+ t tstep) tstop)) 0.0)
     
    91263            ((ode-runtime 'solve-env-state-fold)
    92264             (lambda (sym svec i)
    93                (let* ((k1  (* tstep (svec-deriv svec)))
     265               (let* ((k1  (* h (svec-deriv svec)))
    94266                      (y31 (+ (svec-value svec) k1)))
    95267                 (f64vector-set! k1vect  i k1)
     
    99271             
    100272
    101             (eval-env-set! indep (+ t tstep))
     273            (eval-env-set! indep (+ t h))
    102274            (eval-derivs)
    103275
     
    107279             (lambda (sym svec i)
    108280               (let* ((k1  (f64vector-ref k1vect i))
    109                       (k2  (* tstep (svec-deriv svec)))
     281                      (k2  (* h (svec-deriv svec)))
    110282                      (y32 (+ (svec-value svec) k1 k2)))
    111283                 (f64vector-set! k2vect  i k2)
     
    114286                 (cons (fx+ 1 i) (cdr hstlist)))) 0)
    115287
    116             (eval-env-set! indep (+ t (* Ct32 tstep)))
     288
     289            (eval-env-set! indep (+ t (* h 2.0)))
    117290            (eval-derivs)
    118291
     
    123296               (let* ((k1  (f64vector-ref k1vect i))
    124297                      (k2  (f64vector-ref k2vect i))
    125                       (k3  (* tstep (svec-deriv svec)))
     298                      (k3  (* h (svec-deriv svec)))
    126299                      (y33 (+ (svec-value svec) k1 k2 k3)))
    127300                 (f64vector-set! k3vect  i k3)
    128301                 (f64vector-set! y33vect i y33)
    129302                 (cons (fx+ 1 i) (cdr hstlist))))  0)
     303
     304
     305            (eval-env-set! indep  t)
     306            (eval-derivs)
    130307
    131308           
     
    139316                   (cons (fx+ 1 i) (cdr hstlist))))  0)
    140317           
    141             (eval-env-set! indep (+ t (* Ct21 tstep)))
     318
     319            (eval-env-set! indep (+ t (* h 1.5)))
    142320            (eval-derivs)
    143321
     
    147325             (lambda (sym svec i)
    148326               (let* ((k1  (f64vector-ref k1vect i))
    149                       (k4  (* tstep (svec-deriv svec)))
     327                      (k4  (* h (svec-deriv svec)))
    150328                      (y22 (+ (svec-value svec) (* 1.5 k1) (* 1.5 k4))))
    151329                 (f64vector-set! k4vect  i k4)
     
    153331                 (cons (fx+ 1 i) (cdr hstlist))))  0)
    154332           
     333            (eval-env-set! indep t)
     334            (eval-derivs)
    155335
    156336            ;; step y11
     
    187367                     (vector-set! nvect i (f64vector y0 hy1 h2y2 h3y3))))))  0)
    188368
    189 
    190             ;; Advance state to next step
    191             ((ode-runtime 'solve-env-state-fold)
    192              (lambda (sym svec i)
    193                (let* ((y33 (f64vector-ref y33vect i)))
    194                  (eval-env-set! sym y33)
    195                  (cons (fx+ 1 i))))  0)
    196 
    197 
    198             (eval-derivs)
    199            
     369            ;; compute new estimate of lambda
     370
     371            ;; define relative error as (max (- hub (* h^4 lam4))  (- (* h^4 lam4) hlb))
    200372           
    201373            (let ((err (maxerror)))
     
    217389                          (eval-env-set! sym (svec-value svec))))
    218390                       (loop it t #f)))
    219                     (else (loop (fx+ 1 it) (+ t tstep) #t)))))))
     391                    (else (loop (fx+ 1 it) (+ t tstep) #t))))))))
    220392         
    221   (let* ((ynorm   ...)
    222          (y4norm  (* (expt lam 4) ynorm))
    223          (hm      (* S (expt (/ eps (* Cm y4norm)) 0.25))))
    224     (values it t tstep (list hm nvect)))))
     393    (values it t tstep (list nvect)))
Note: See TracChangeset for help on using the changeset viewer.