source: project/ode/trunk/rkf45.scm @ 8001

Last change on this file since 8001 was 8001, checked in by Ivan Raikov, 13 years ago

Bug fixes and changes to ode-hhsm and the solvers.

File size: 15.0 KB
Line 
1
2;;
3;; Runge-Kutta-Fehlberg integration method.
4;;
5;;
6;; Copyright 2007 Ivan Raikov and the Okinawa Institute of Science and Technology
7;;
8;; This program is free software: you can redistribute it and/or
9;; modify it under the terms of the GNU General Public License as
10;; published by the Free Software Foundation, either version 3 of the
11;; License, or (at your option) any later version.
12;;
13;; This program is distributed in the hope that it will be useful, but
14;; WITHOUT ANY WARRANTY; without even the implied warranty of
15;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
16;; General Public License for more details.
17;;
18;; A full copy of the GPL license can be found at
19;; <http://www.gnu.org/licenses/>.
20;;
21
22;;
23;;  From http://math.fullerton.edu/mathews/n2003/RungeKuttaFehlbergMod.html
24;;
25;;    One way to guarantee accuracy in the solution of an IVP. is to
26;;    solve the problem twice using step sizes h and h/2 and compare
27;;    answers at the mesh points corresponding to the larger step
28;;    size.
29;;
30;;    But this requires a significant amount of computation for the
31;;    smaller step size and must be repeated if it is determined that
32;;    the agreement is not good enough.
33;;
34;;    The Runge-Kutta-Fehlberg method (denoted RKF45) is one way to
35;;    try to resolve this problem.  At each step, two different
36;;    approximations for the solution are made and compared.  If the
37;;    two answers are in close agreement, the approximation is
38;;    accepted. If the two answers do not agree to a specified
39;;    accuracy, the step size is reduced.  If the answers agree to
40;;    more significant digits than required, the step size is
41;;    increased.
42;;
43;;    Each Runge-Kutta-Fehlberg step requires the use of the following
44;;    six values:
45;;
46;;      k1 = h*f(t,y)
47;;      k2 = h*f(t + 1/4*h, y + 1/4*k1)
48;;      k3 = h*f(t + 3/8*h, y + 3/32*k1 + 9/32*k2)
49;;      k4 = h*f(t + 12/13*h, y + 1932/2197*k1 - 7200/2197*k2 + 7296/2197*k3)
50;;      k5 = h*f(t + h, y + 439/216*k1 - 8*k2 + 3680/513*k3 - 845/4104*k4)
51;;      k6 = h*f(t + 1/2*h, y - 8/27*k1 + 2*k2 - 3544/2565*k3 + 1859/4104*k4 - 11/40*k5)
52;;
53;;    Then an approximation to the solution of the IVP. is made using
54;;    a Runge-Kutta method of order 4:
55;;
56;;      y4th = y + 25/216*k1 + 1408/2565*k3 + 2197/4104*k4 - 1/5*k5
57;;
58;;    And a better value for the solution is determined using a
59;;    Runge-Kutta method of order 5:
60;;
61;;      y5th = y + 16/135*k1 + 6656/12825*k3 + 28561/56430*k4 - 9/50*k5 + 2/55*k6
62;;
63;;    The optimal step size h' can be determined by multiplying the
64;;    scalar s times the current step size h. The scalar s is
65;;
66;;      s = {eps*h / (2*(abs (y5th-y4th)))}^1/4 =
67;;        =  0.840896 * {eps*h / (abs (y5th-y4th))}^1/4
68
69
70
71(include "box.scm")
72(include "stack.scm")
73(include "lists.scm")
74
75
76;; these constants are taken from Nicholas B. Tufillaro's numerical
77;; solver routines
78(define A0      .11574074074074074074)   ;; 25/216       
79(define A2      .54892787524366471734)   ;; 1408/2565   
80(define A3      .53533138401559454191)   ;; 2197/4104   
81(define A4      -.20000000000000000000)  ;; -1/5         
82(define B0      .11851851851851851851)   ;; 16/135       
83(define B2      .51898635477582846003)   ;; 6656/12825   
84(define B3      .50613149034201665780)   ;; 28561/56430 
85(define B4      -.18000000000000000000)  ;; -9/50       
86(define B5      .03636363636363636363)   ;; 2/55         
87(define C2t     .25)                     ;; 1/4         
88(define C20     .25)                     ;; 1/4         
89(define C3t     .37500000000000000000)   ;; 3/8         
90(define C30     .09375000000000000000)   ;; 3/32         
91(define C31     .28125000000000000000)   ;; 9/32         
92(define C4t     .92307692307692307692)   ;; 12/13       
93(define C40     .87938097405553026854)   ;; 1932/2197   
94(define C41     -3.27719617660446062812) ;; -7200/2197   
95(define C42     3.32089212562585343650)  ;; 7296/2197   
96(define C50     2.03240740740740740740)  ;; 439/216     
97(define C51     -8.0)                    ;; -8           
98(define C52     7.17348927875243664717)  ;; 3680/513     
99(define C53     -.20589668615984405458)  ;; -845/4104   
100(define C6t     0.5)                     ;; 1/2         
101(define C60     -.29629629629629629629)  ;; -8/27       
102(define C61     2.0)                     ;; 2           
103(define C62     -1.38167641325536062378) ;; -3544/2565   
104(define C63     .45297270955165692007)   ;; 1859/4104   
105(define C64     -.27500000000000000000)  ;; -11/40       
106
107
108(define history-len 3)
109
110(define (t<tstop tstep t tstop)
111  (if (> tstep 0.0) (< t tstop) (> t tstop)))
112
113(define (make-ode:rkf45  numerror make-vector vector-ref vector-set! vector?)
114
115 (lambda (ode-runtime indep tstart tstop dt initial-tstep  . rest)
116
117  (define guards (if (pair? rest) (car rest) (list)))
118  (define debug  (and (pair? rest) (and (pair? (cdr rest)) (second rest))))
119
120  (define maxerror   (ode-runtime 'maxerror))
121  (define hierror?   (ode-runtime 'hierror?))
122  (define lowerror?  (ode-runtime 'lowerror?))
123
124  (define printq  (ode-runtime 'print))
125  (define eval-rhs  (ode-runtime 'eval-rhs))
126  (define is-state? (ode-runtime 'is-state?))
127  (define is-asgn?  (ode-runtime 'is-asgn?))
128  (define solve-env-ref  (ode-runtime 'solve-env-ref))
129  (define solve-env-set! (ode-runtime 'solve-env-set!))
130  (define eval-env-ref  (ode-runtime 'eval-env-ref))
131  (define eval-env-set! (ode-runtime 'eval-env-set!))
132
133  (define svec-deriv-idx  (ode-runtime 'svec-deriv-idx))
134  (define svec-value-idx  (ode-runtime 'svec-value-idx))
135  (define svec-relerr-idx  (ode-runtime 'svec-relerr-idx))
136  (define svec-abserr-idx  (ode-runtime 'svec-abserr-idx))
137 
138  (define (svec-deriv svec) (vector-ref svec svec-deriv-idx))
139  (define (svec-value svec) (vector-ref svec svec-value-idx))
140  (define (svec-relerr svec) (vector-ref svec svec-relerr-idx))
141  (define (svec-abserr svec) (vector-ref svec svec-abserr-idx))
142
143  (define (svec-deriv-set! svec v) (vector-set! svec svec-deriv-idx v))
144  (define (svec-value-set! svec v) (vector-set! svec svec-value-idx v))
145  (define (svec-relerr-set! svec v) (vector-set! svec svec-relerr-idx v))
146  (define (svec-abserr-set! svec v) (vector-set! svec svec-abserr-idx v))
147
148 
149  (define (eval-derivs!)
150    ((ode-runtime 'eval-poset-foreach)
151     (lambda (order lst)
152       (for-each
153        (lambda (x)
154          (let ((xrec (if (> (length x) 2) x (cons #f x))))
155            (let ((df   (first xrec))
156                  (sym  (second xrec))
157                  (rhs  (third xrec)))
158            (let ((v     (eval-rhs sym rhs))
159                  (svec  (solve-env-ref sym)))
160              (cond ((is-state? sym)  (case df 
161                                        ((q)  (svec-value-set! svec v))
162                                        ((d)  (svec-deriv-set! svec v))))
163                    ((is-asgn? sym)   (begin
164                                        (eval-env-set! sym v)
165                                        (if (vector? svec)
166                                            (svec-value-set! svec v)
167                                            (set-car! svec v)))))))))
168        lst))))
169 
170
171  ;; Per-state history
172  (define history 
173    (list-tabulate (ode-runtime 'order) 
174                   (lambda (i) (list->stack (list (cons 0.0 0.0)
175                                                  (cons 0.0 0.0)
176                                                  (cons 0.0 0.0))))))
177
178  ;; Per-state Runge-Kutta coefficients
179  (define k0vect   (make-vector (ode-runtime 'order) 0.0))
180  (define k1vect   (make-vector (ode-runtime 'order) 0.0))
181  (define k2vect   (make-vector (ode-runtime 'order) 0.0))
182  (define k3vect   (make-vector (ode-runtime 'order) 0.0))
183  (define k4vect   (make-vector (ode-runtime 'order) 0.0))
184  (define k5vect   (make-vector (ode-runtime 'order) 0.0))
185
186  ;; Per-state predictor vector
187  (define predvect   (make-vector (ode-runtime 'order) 0.0))
188
189  ;; remember previous timestep to prevent infinite loops
190  (define prevstep-history-max 100)
191  (define prevstep-history-min 3)
192  (define prevstep-history prevstep-history-min)
193  (define prevstep prevstep-history)
194
195  ;; maximum and minimum ratios by which to increase to decrease the
196  ;; step size
197  (define M                (ode-runtime'  hmax-factor))
198  (define m                (ode-runtime'  hmin-factor))
199  (define S                (ode-runtime'  hscale-factor))
200  (define relmax           (ode-runtime' relmax))
201  (define derivderivderivrelmin           (ode-runtime' relmin))
202
203  ;; fifth-order Runge-Kutta-Fehlberg
204  (define (rk5 startit t tstep gdval)
205
206    (eval-env-set! indep t)
207    (eval-env-set! dt tstep)
208    (eval-derivs!)
209
210    (let loop ((it startit) (t t) (gdval gdval))
211
212      (for-each (lambda (g)
213                  (if (not (eval-rhs 'guard g))
214                      (numerror "guard failed: " g))) guards)
215     
216      (if gdval
217          (begin
218            ;; update the derivative and value history
219            ((ode-runtime 'solve-env-state-fold) 
220             (lambda (sym svec hstlist)
221               (svec-value-set! svec (eval-env-ref sym))
222               (let ((sthist  (car hstlist)))
223                 (let ((sthist1 (stack-push! sthist 
224                                             (cons (svec-value svec)
225                                                   (svec-deriv svec)))))
226                   (let ((dep (stack-depth sthist1)))
227                     (if (fx<= history-len dep) (stack-cut! sthist1 history-len dep)))
228                   (cdr hstlist))))
229             history)
230            ;; update independent variable
231            (solve-env-set! indep svec-value-idx t)
232            ;; print quantities
233            (printq)))
234     
235      (if (t<tstop tstep t tstop)
236          (begin
237           
238            (if (> (* tstep (- (+ t tstep) tstop)) 0.0)
239                (begin
240                  (set! tstep (- tstop t))
241                  (eval-env-set! dt tstep)))
242
243            (eval-env-set! indep t)
244            (eval-env-set! dt tstep)
245            (eval-derivs!)
246           
247            ;; step k0
248            ((ode-runtime 'solve-env-state-fold) 
249             (lambda (sym svec i+hstlist)
250               (let ((i        (car i+hstlist))
251                     (hstlist  (cdr i+hstlist)))
252                 (let ((y1 (svec-deriv svec)))
253                   (if (zero? y1)
254                       (begin
255                         (vector-set! k0vect i 0)
256                         (eval-env-set! sym (svec-value svec)))
257                       (let ((sthist   (car hstlist)))
258                         (let ((k0  (* tstep y1)))
259                           (vector-set! k0vect i k0)
260                           (eval-env-set! sym (+ (svec-value svec) (* C20 k0))))))
261                   (cons (fx+ 1 i) (cdr hstlist)))))
262             (cons 0 history))
263           
264            (eval-env-set! indep (+ t (* C2t tstep)))
265            (eval-derivs!)
266           
267            ;; step k1
268            ((ode-runtime 'solve-env-state-fold) 
269             (lambda (sym svec i+hstlist)
270               (let ((i        (car i+hstlist))
271                     (hstlist  (cdr i+hstlist)))
272                 (let ((y1  (svec-deriv svec))
273                       (k0  (vector-ref k0vect i)))
274                   (if (and (zero? y1) (zero? k0))
275                       (begin
276                         (vector-set! k1vect i 0)
277                         (eval-env-set! sym (svec-value svec)))
278                       (let ((sthist   (car hstlist)))
279                         (let ((k1  (* tstep y1)))
280                           (vector-set! k1vect i k1)
281                           (eval-env-set! sym (+ (car (stack-peek sthist)) 
282                                                 (* C30 k0) (* C31 k1))))))
283                   (cons (fx+ 1 i) (cdr hstlist)))))
284             (cons 0 history))
285           
286            (eval-env-set! indep  (+ t (* C3t tstep)))
287            (eval-derivs!)
288           
289            ;; step k2
290            ((ode-runtime 'solve-env-state-fold) 
291             (lambda (sym svec i+hstlist)
292               (let ((i        (car i+hstlist))
293                     (hstlist  (cdr i+hstlist)))
294                 (let ((y1 (svec-deriv svec))
295                       (k1  (vector-ref k1vect i))
296                       (k0  (vector-ref k0vect i)))
297                   (if (and (zero? y1) (zero? k1) (zero? k0))
298                       (begin
299                         (vector-set! k2vect i 0)
300                         (eval-env-set! sym (svec-value svec)))
301                       (let ((sthist   (car hstlist)))
302                         (let ((k2  (* tstep y1)))
303                           (vector-set! k2vect i k2)
304                           (eval-env-set! sym (+ (car (stack-peek sthist)) 
305                                                 (* C40 k0) (* C41 k1) (* C42 k2))))))
306                   (cons (fx+ 1 i) (cdr hstlist)))))
307             (cons 0 history))
308           
309            (eval-env-set! indep (+ t (* C4t tstep)))
310            (eval-derivs!)
311           
312            ;; step k3
313            ((ode-runtime 'solve-env-state-fold) 
314             (lambda (sym svec i+hstlist)
315               (let ((i        (car i+hstlist))
316                     (hstlist  (cdr i+hstlist)))
317                 (let ((y1  (svec-deriv svec))
318                       (k2  (vector-ref k2vect i))
319                       (k1  (vector-ref k1vect i))
320                       (k0  (vector-ref k0vect i)))
321                   (if (and (zero? y1) (zero? k2) (zero? k1) (zero? k0))
322                       (begin
323                         (vector-set! k3vect i 0)
324                         (eval-env-set! sym (svec-value svec)))
325                       (let ((sthist   (car hstlist)))
326                         (let ((k3  (* tstep y1)))
327                           (vector-set! k3vect i k3)
328                           (eval-env-set! sym (+ (car (stack-peek sthist)) 
329                                                 (* C50 k0) (* C51 k1) 
330                                                 (* C52 k2) (* C53 k3))))))
331                   (cons (fx+ 1 i) (cdr hstlist)))))
332             (cons 0 history))
333           
334            (eval-env-set! indep (+ t tstep))
335            (eval-derivs!)
336           
337            ;; step k4
338            ((ode-runtime 'solve-env-state-fold) 
339             (lambda (sym svec i+hstlist)
340               (let ((i        (car i+hstlist))
341                     (hstlist  (cdr i+hstlist)))
342                 (let ((y1  (svec-deriv svec))
343                       (k3  (vector-ref k3vect i))
344                       (k2  (vector-ref k2vect i))
345                       (k1  (vector-ref k1vect i))
346                       (k0  (vector-ref k0vect i)))
347                   (if (and (zero? y1) (zero? k3) (zero? k2) (zero? k1) (zero? k0)) 
348                       (begin
349                         (vector-set! k4vect i 0)
350                         (eval-env-set! sym (svec-value svec)))
351                       (let ((sthist   (car hstlist)))
352                         (let ((k4  (* tstep y1)))
353                           (vector-set! k4vect i k4)
354                           (eval-env-set! sym (+ (car (stack-peek sthist)) 
355                                                 (* C60 k0) (* C61 k1) (* C62 k2) 
356                                                 (* C63 k3) (* C64 k4))))))
357                   (cons (fx+ 1 i) (cdr hstlist)))))
358             (cons 0 history))
359           
360            (eval-env-set! indep  (+ t (* C6t tstep)))
361            (eval-derivs!)
362           
363            ;; step k5
364            ((ode-runtime 'solve-env-state-fold) 
365             (lambda (sym svec i+hstlist)
366               (let ((i        (car i+hstlist))
367                     (hstlist  (cdr i+hstlist)))
368                 (let ((y1 (svec-deriv svec))
369                       (k4  (vector-ref k4vect i))
370                       (k3  (vector-ref k3vect i))
371                       (k2  (vector-ref k2vect i))
372                       (k1  (vector-ref k1vect i))
373                       (k0  (vector-ref k0vect i)))
374                   (if (and (zero? y1) (zero? k4) (zero? k3) (zero? k2) (zero? k1) (zero? k0))
375                       (begin
376                         (eval-env-set! sym (svec-value svec)))
377                       (let ((sthist   (car hstlist))
378                             (k5       (* tstep y1)))
379                         (vector-set! k5vect i k5)
380                         (let ((predval (+ (car (stack-peek  sthist))
381                                           (* A0 k0) (* A2 k2)
382                                           (* A3 k3) (* A4 k4))))
383                           (vector-set! predvect i predval)
384                           (let ((val  (+ (car (stack-peek sthist))
385                                          (* B0 k0) (* B2 k2) 
386                                          (* B3 k3) (* B4 k4) 
387                                          (* B5 k5))))
388                             (if (not (= val 0.0))
389                                 (svec-relerr-set! svec (abs (- 1.0 (/ predval val)))))
390                             (svec-abserr-set! svec (abs (- val predval)))
391                             (eval-env-set! sym val)))))
392                   (cons (fx+ 1 i) (cdr hstlist)))))
393             (cons 0 history))
394           
395            (let ((err (maxerror)))
396              (let ((hierr (hierror? t tstep err))
397                    (loerr (lowerror? t tstep err)))
398
399                (cond (hierr
400                       (begin
401                         (set! tstep (* tstep (max m (sqrt (/ relmax (abs hierr))))))
402                         (set! prevstep (fx+ it prevstep-history))
403                         (if (fx< prevstep-history prevstep-history-max)
404                                  (set! prevstep-history (fx+ 1 prevstep-history)))
405                         ((ode-runtime 'solve-env-state-foreach)
406                          (lambda (sym svec)
407                            (eval-env-set! sym (svec-value svec))))
408                         (loop it t #f)))
409                      ((and loerr (positive? loerr) 
410                            (fx<= prevstep it) (< (+ t tstep) tstop))
411                       (begin
412                         (set! tstep (* tstep S (min M (sqrt (/ relmax (abs loerr))))))
413                         ((ode-runtime 'solve-env-state-foreach)
414                          (lambda (sym svec)
415                            (eval-env-set! sym (svec-value svec))))
416                         (loop it t #f)))
417                      (else
418                       (begin
419                         (if (fx> prevstep-history prevstep-history-min)
420                             (set! prevstep-history (fx- prevstep-history 1)))
421                         (loop (fx+ 1 it) (+ t tstep) #t)))))))
422         
423          (begin
424            (list it t tstep (list))))))
425
426  (if debug (print "rkf45: " (ode-runtime 'debug)))
427  (rk5 0 0.0 initial-tstep #t)))
Note: See TracBrowser for help on using the repository browser.