source: project/ode/trunk/abm4.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: 16.1 KB
Line 
1;;
2;;
3;; 4th order Adams-Bashford-Moulton integration method with adaptive
4;; step size.
5;;
6;;
7;; Copyright 2007 Ivan Raikov and the Okinawa Institute of Science and Technology
8;;
9;; This program is free software: you can redistribute it and/or
10;; modify it under the terms of the GNU General Public License as
11;; published by the Free Software Foundation, either version 3 of the
12;; License, or (at your option) any later version.
13;;
14;; This program is distributed in the hope that it will be useful, but
15;; WITHOUT ANY WARRANTY; without even the implied warranty of
16;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
17;; General Public License for more details.
18;;
19;; A full copy of the GPL license can be found at
20;; <http://www.gnu.org/licenses/>.
21;;
22
23
24(include "box.scm")
25(include "stack.scm")
26(include "lists.scm")
27
28
29;; Error constant for 3rd order Adams-Moulton method from p. 103 in
30;; _Numerical Methods for Ordinary Differential Equations_,
31;; by J.C. Butcher.
32(define ECONST 0.0263888888888889) ;; 19/720       
33
34;; these constants are taken from Nicholas B. Tufillaro's numerical
35;; solver routines
36
37(define C20     .25)                     ;; 1/4         
38(define C2t     .25)                     ;; 1/4         
39(define C3t     .37500000000000000000)   ;; 3/8         
40(define C4t     .92307692307692307692)   ;; 12/13       
41(define C6t     0.5)                     ;; 1/2         
42
43(define  C30  .09375000000000000000)   ;; 3/32         
44(define  C31  .28125000000000000000)   ;; 9/32         
45(define  C40  .87938097405553026854)   ;; 1932/2197   
46(define  C41  -3.27719617660446062812) ;; -7200/2197   
47(define  C42  3.32089212562585343650)  ;; 7296/2197   
48(define  C50  2.03240740740740740740)  ;; 439/216     
49(define  C51  -8.0)                    ;; -8           
50(define  C52  7.17348927875243664717)  ;; 3680/513     
51(define  C53  -.20589668615984405458)  ;; -845/4104   
52(define  C60  -.29629629629629629629)  ;; -8/27       
53(define  C61  2.0)                     ;; 2           
54(define  C62  -1.38167641325536062378) ;; -3544/2565   
55(define  C63  .45297270955165692007)   ;; 1859/4104   
56(define  C64 -.27500000000000000000)   ;; -11/40       
57
58(define  A0   .11574074074074074074)   ;; 25/216       
59(define  A2   .54892787524366471734)   ;; 1408/2565   
60(define  A3   .53533138401559454191)   ;; 2197/4104   
61(define  A4   -.20000000000000000000)  ;; -1/5         
62(define  B0   .11851851851851851851)   ;; 16/135       
63(define  B2   .51898635477582846003)   ;; 6656/12825   
64(define  B3   .50613149034201665780)   ;; 28561/56430 
65(define  B4   -.18000000000000000000)  ;; -9/50       
66(define  B5  .03636363636363636363)    ;; 2/55         
67
68
69(define AM0  9.0)
70(define AM1  19.0) 
71(define AM2  -5.0)
72(define AM3  1.0)
73
74(define AB0  55.0)
75(define AB1  -59.0)
76(define AB2  37.0)
77(define AB3  -9.0)
78
79(define history-len 3)
80
81
82(define (t<tstop tstep t tstop)
83  (if (> tstep 0.0) (< t tstop) (> t tstop)))
84
85
86(define (make-ode:abm4 numerror make-vector vector-ref vector-set! vector?)
87 (lambda (ode-runtime indep tstart tstop dt initial-tstep . rest)
88
89  (define guards (if (pair? rest) (car rest) (list)))
90  (define debug  (and (pair? rest) (and (pair? (cdr rest)) (second rest))))
91
92  (define maxerror   (ode-runtime 'maxerror))
93  (define hierror?   (ode-runtime 'hierror?))
94  (define lowerror?  (ode-runtime 'lowerror?))
95
96  (define printq    (ode-runtime 'print))
97  (define eval-rhs  (ode-runtime 'eval-rhs))
98  (define is-state? (ode-runtime 'is-state?))
99  (define is-asgn?  (ode-runtime 'is-asgn?))
100  (define solve-env-ref  (ode-runtime 'solve-env-ref))
101  (define solve-env-set! (ode-runtime 'solve-env-set!))
102  (define eval-env-ref  (ode-runtime 'eval-env-ref))
103  (define eval-env-set! (ode-runtime 'eval-env-set!))
104
105  (define svec-deriv-idx  (ode-runtime 'svec-deriv-idx))
106  (define svec-value-idx  (ode-runtime 'svec-value-idx))
107  (define svec-relerr-idx  (ode-runtime 'svec-relerr-idx))
108  (define svec-abserr-idx  (ode-runtime 'svec-abserr-idx))
109 
110  (define (svec-deriv svec) (vector-ref svec svec-deriv-idx))
111  (define (svec-value svec) (vector-ref svec svec-value-idx))
112  (define (svec-relerr svec) (vector-ref svec svec-relerr-idx))
113  (define (svec-abserr svec) (vector-ref svec svec-abserr-idx))
114
115  (define (svec-deriv-set! svec v) (vector-set! svec svec-deriv-idx v))
116  (define (svec-value-set! svec v) (vector-set! svec svec-value-idx v))
117  (define (svec-relerr-set! svec v) (vector-set! svec svec-relerr-idx v))
118  (define (svec-abserr-set! svec v) (vector-set! svec svec-abserr-idx v))
119 
120  (define (eval-derivs!)
121    ((ode-runtime 'eval-poset-foreach)
122     (lambda (order lst)
123       (for-each
124        (lambda (x)
125          (let ((xrec (if (> (length x) 2) x (cons #f x))))
126            (let ((df   (first xrec))
127                  (sym  (second xrec))
128                  (rhs  (third xrec)))
129              (let ((v     (eval-rhs sym rhs))
130                    (svec  (solve-env-ref sym)))
131                (cond ((is-state? sym)  (case df 
132                                          ((q)  (svec-value-set! svec v))
133                                          ((d)  (svec-deriv-set! svec v))))
134                      ((is-asgn? sym)   (begin
135                                          (eval-env-set! sym v)
136                                          (if (vector? svec)
137                                              (svec-value-set! svec v)
138                                              (set-car! svec v)))))))))
139          lst))))
140 
141
142  ;; Per-state history
143  (define history 
144    (list-tabulate (ode-runtime 'order) 
145                   (lambda (i) (list->stack (list (cons 0.0 0.0)
146                                                  (cons 0.0 0.0)
147                                                  (cons 0.0 0.0))))))
148
149  ;; Per-state Runge-Kutta coefficients
150  (define k0vect   (make-vector (ode-runtime 'order) 0.0))
151  (define k1vect   (make-vector (ode-runtime 'order) 0.0))
152  (define k2vect   (make-vector (ode-runtime 'order) 0.0))
153  (define k3vect   (make-vector (ode-runtime 'order) 0.0))
154  (define k4vect   (make-vector (ode-runtime 'order) 0.0))
155  (define k5vect   (make-vector (ode-runtime 'order) 0.0))
156
157
158  (define (step-k1 i v k1 k0vect)
159    (+ v (* C31 k1) (* C30 (vector-ref k0vect i))))
160
161  (define (step-k2 i v k2 k1vect k0vect)
162    (+ v (* C42 k2) (* C41 (vector-ref k1vect i)) (* C40 (vector-ref k0vect i))))
163
164  (define (step-k3 i v k3 k2vect k1vect k0vect)
165    (+ v (* C53 k3) (* C52 (vector-ref k2vect i)) 
166       (* C51 (vector-ref k1vect i)) (* C50 (vector-ref k0vect i))))
167
168  (define (step-k4 i v k4 k3vect k2vect k1vect k0vect)
169    (+ v (* C64 k4) (* C63 (vector-ref k3vect i)) (* C62 (vector-ref k2vect i)) 
170       (* C61 (vector-ref k1vect i)) (* C60 (vector-ref k0vect i))))
171
172  (define (step-k5-predval i v k4vect k3vect k2vect k0vect)
173    (+ v (* A0 (vector-ref k0vect i)) (* A2 (vector-ref k2vect i)) 
174       (* A3 (vector-ref k3vect i)) (* A4 (vector-ref k4vect i))))
175 
176  (define (step-k5-val i v k5 k4vect k3vect k2vect k0vect)
177    (+ v (* B5 k5) (* B0 (vector-ref k0vect i)) (* B2 (vector-ref k2vect i)) 
178       (* B3 (vector-ref k3vect i)) (* B4 (vector-ref k4vect i))))
179
180  (define (am-corrector v tstep pri0 pri1 pri2 pri3)
181    (+ v (* (/ tstep 24) (+ (* AM3 pri3) (* AM2 pri2) (* AM1 pri1) (* AM0 pri0)))))
182
183  (define (ab-predictor v tstep pri0 pri1 pri2 pri3)
184    (+ v (* (/ tstep 24) (+ (* AB3 pri3) (* AB2 pri2) (* AB1 pri1) (* AB0 pri0)))))
185
186
187  ;; maximum and minimum ratios by which to increase to decrease the
188  ;; step size
189  (define M                (ode-runtime'  hmax-factor))
190  (define m                (ode-runtime'  hmin-factor))
191  (define S                (ode-runtime'  hscale-factor))
192  (define relmax           (ode-runtime' relmax))
193  (define relmin           (ode-runtime' relmin))
194
195  ;; Per-state predictor vector
196  (define predvect   (make-vector (ode-runtime 'order) 0.0))
197
198  ;; fifth-order Runge-Kutta-Fehlberg
199  (define (rk5 startit t tstep gdval)
200
201    (eval-env-set! indep t)
202    (eval-env-set! dt tstep)
203    (eval-derivs!)
204
205    (let ((itstop (fx+ startit history-len)))
206      (let loop ((it startit) (t t) (gdval gdval))
207
208        (for-each (lambda (g)
209                    (if (not (eval-rhs 'guard g))
210                        (numerror "guard failed: " g))) guards)
211
212       
213        (if gdval
214            (begin
215              ;; update the derivative and value history
216              ((ode-runtime 'solve-env-state-fold) 
217                     (lambda (sym svec hstlist)
218                       (svec-value-set! svec (eval-env-ref sym))
219                       (let ((sthist  (car hstlist)))
220                         (let ((sthist1 (stack-push! sthist 
221                                                     (cons (svec-value svec)
222                                                           (svec-deriv svec)))))
223                           (let ((dep (stack-depth sthist1)))
224                             (if (fx<= history-len dep) (stack-cut! sthist1 history-len dep)))
225                           (cdr hstlist))))
226                     history)
227              ;; update independent variable
228              (solve-env-set! indep svec-value-idx t)
229              ;; print quantities
230              (printq)))
231
232        (if (and (fx< it itstop) (t<tstop tstep t tstop))
233
234            (begin ;; if not done with startup
235
236              (if (> (* tstep (- (+ t tstep) tstop)) 0.0)
237                  (begin
238                    (set! tstep (- tstop t))
239                    (eval-env-set! dt tstep)))
240
241              (eval-env-set! indep t)
242              (eval-env-set! dt tstep)
243              (eval-derivs!)
244             
245              ;; step k0
246              ((ode-runtime 'solve-env-state-fold) 
247               (lambda (sym svec i+hstlist)
248                 (let ((i        (car i+hstlist))
249                       (hstlist  (cdr i+hstlist)))
250                   (let ((y1 (svec-deriv svec)))
251                     (if (zero? y1)
252                         (begin
253                           (vector-set! k0vect i 0)
254                           (eval-env-set! sym (svec-value svec)))
255                         (let ((sthist   (car hstlist)))
256                           (let ((k0     (* tstep y1)))
257                             (vector-set! k0vect i k0)
258                             (eval-env-set! sym (+ (svec-value svec) (* C20 k0)))))))
259                   (cons (fx+ 1 i) (cdr hstlist))))
260               (cons 0 history))
261             
262              (eval-env-set! indep (+ t (* C2t tstep)))
263              (eval-derivs!)
264             
265              ;; step k1
266              ((ode-runtime 'solve-env-state-fold) 
267               (lambda (sym svec i+hstlist)
268                 (let ((i        (car i+hstlist))
269                       (hstlist  (cdr i+hstlist)))
270                   (let ((y1  (svec-deriv svec)))
271                     (let ((sthist   (car hstlist)))
272                       (let ((k1  (* tstep y1)))
273                         (vector-set! k1vect i k1)
274                         (eval-env-set! sym (step-k1 i (car (stack-peek sthist)) k1 k0vect))
275                         (cons (fx+ 1 i) (cdr hstlist)))))))
276               (cons 0 history))
277             
278              (eval-env-set! indep  (+ t (* C3t tstep)))
279              (eval-derivs!)
280             
281              ;; step k2
282              ((ode-runtime 'solve-env-state-fold) 
283               (lambda (sym svec i+hstlist)
284                 (let ((i        (car i+hstlist))
285                       (hstlist  (cdr i+hstlist)))
286                   (let ((y1  (svec-deriv svec))
287                         (sthist   (car hstlist)))
288                     (let ((k2  (* tstep y1)))
289                       (vector-set! k2vect i k2)
290                       (eval-env-set! sym (step-k2 i (car (stack-peek sthist)) k2 k1vect k0vect))
291                       (cons (fx+ 1 i) (cdr hstlist))))))
292               (cons 0 history))
293             
294              (eval-env-set! indep (+ t (* C4t tstep)))
295              (eval-derivs!)
296             
297              ;; step k3
298              ((ode-runtime 'solve-env-state-fold) 
299               (lambda (sym svec i+hstlist)
300                 (let ((i        (car i+hstlist))
301                       (hstlist  (cdr i+hstlist)))
302                   (let ((y1  (svec-deriv svec))
303                         (sthist   (car hstlist)))
304                     (let ((k3  (* tstep y1)))
305                       (vector-set! k3vect i k3)
306                       (eval-env-set! sym (step-k3 i (car (stack-peek sthist)) k3 k2vect k1vect k0vect))
307                       (cons (fx+ 1 i) (cdr hstlist))))))
308               (cons 0 history))
309             
310              (eval-env-set! indep (+ t tstep))
311              (eval-derivs!)
312             
313              ;; step k4
314              ((ode-runtime 'solve-env-state-fold) 
315               (lambda (sym svec i+hstlist)
316                 (let ((i        (car i+hstlist))
317                       (hstlist  (cdr i+hstlist)))
318                   (let ((y1  (svec-deriv svec))
319                         (sthist   (car hstlist)))
320                     (let ((k4  (* tstep y1)))
321                       (vector-set! k4vect i k4)
322                       (eval-env-set! sym (step-k4 i (car (stack-peek sthist)) 
323                                                   k4 k3vect k2vect k1vect k0vect))
324                       (cons (fx+ 1 i) (cdr hstlist))))))
325               (cons 0 history))
326             
327              (eval-env-set! indep  (+ t (* C6t tstep)))
328              (eval-derivs!)
329             
330              ;; step k5
331              ((ode-runtime 'solve-env-state-fold) 
332               (lambda (sym svec i+hstlist)
333                 (let ((i        (car i+hstlist))
334                       (hstlist  (cdr i+hstlist)))
335                   (let ((y1  (svec-deriv svec))
336                         (sthist   (car hstlist)))
337                     (let ((k5       (* tstep y1)))
338                       (vector-set! k5vect i k5)
339                       (let ((predval (step-k5-predval i (car (stack-peek  sthist))
340                                                       k4vect k3vect k2vect k0vect))
341                             (val  (step-k5-val i (car (stack-peek sthist))
342                                                k5 k4vect k3vect k2vect k0vect)))
343                           (if (not (= val 0.0))
344                               (svec-relerr-set! svec (abs (- 1.0 (/ predval val)))))
345                           (svec-abserr-set! svec (abs (- val predval)))
346                           (eval-env-set! sym val)
347                           (cons (fx+ 1 i) (cdr hstlist)))))))
348               (cons 0 history))
349             
350              (let ((err (maxerror)))
351                (let ((hierr (hierror? t tstep err))
352                      (loerr (lowerror? t tstep err))) 
353                (cond (hierr
354                       (begin
355                         (set! tstep (* tstep 0.5))
356                         ((ode-runtime 'solve-env-state-foreach)
357                          (lambda (sym svec)
358                            (eval-env-set! sym (svec-value svec))))
359                         (loop it t #f)))
360                      (else (loop (fx+ 1 it) (+ t tstep) #t))))))
361
362            (begin
363              (list it t tstep))))))
364       
365  ;; Predictor-corrector
366  (define (abm4 it t tstep) 
367
368    (eval-env-set! indep 0.0)
369    (eval-env-set! dt tstep)
370    (eval-derivs!)
371
372    (let ((rk5rec (rk5 it t tstep #t)))
373      (let ((it     (first rk5rec))
374            (t      (second rk5rec))
375            (tstep  (third rk5rec)))
376
377        (let loop ((it it) (t t) (tstep tstep))
378
379          (for-each (lambda (g)
380                      (if (not (eval-rhs 'guard g))
381                          (numerror "guard failed: " g))) guards)
382
383          (if (t<tstop tstep t tstop)
384              (begin
385                (if (> (* tstep (- (+ t tstep) tstop)) 0.0)
386                    (begin
387                      (set! tstep (- tstop t))
388                      (eval-env-set! dt tstep)))
389               
390                ;; Adams-Bashforth predictor
391                ((ode-runtime 'solve-env-state-fold) 
392                 (lambda (sym svec i+hstlist)
393                   (let ((i        (car i+hstlist))
394                         (hstlist  (cdr i+hstlist)))
395                     (let* ((sthist   (car hstlist))
396                            (pri      (cons (svec-deriv svec) (map cdr (stack->list sthist))))
397                            (predval  (apply ab-predictor 
398                                             (cons (car (stack-peek sthist)) 
399                                                   (cons tstep pri)))))
400                       (vector-set! predvect i predval)
401                       (svec-value-set! svec predval)
402                       (eval-env-set! sym predval)
403                       (cons (fx+ 1 i) (cdr hstlist)))))
404                 (cons 0 history))
405               
406                (eval-env-set! indep t)
407                (eval-env-set! dt tstep)
408                (eval-derivs!)
409               
410                ;; Adams-Moulton corrector
411                ((ode-runtime 'solve-env-state-fold) 
412                 (lambda (sym svec i+hstlist)
413                   (let ((i        (car i+hstlist))
414                         (hstlist  (cdr i+hstlist)))
415                     (let* ((sthist   (car hstlist))
416                            (pri      (cons (svec-deriv svec) (map cdr (stack->list sthist))))
417                            (val      (apply am-corrector 
418                                             (cons (car (stack-peek sthist)) 
419                                                   (cons tstep pri)))))
420                       (let ((predval (vector-ref predvect i)))
421                         (if (not (= val 0.0))
422                             (svec-relerr-set! svec (* ECONST (abs (- 1.0 (/ predval val))))))
423                         (svec-abserr-set! svec (* ECONST (abs (- val predval))))
424                         (let ((cval  (+ val (* ECONST (- predval val)))))
425                           (eval-env-set! sym cval)
426                           (svec-value-set! svec cval))
427                         (cons (fx+ 1 i) (cdr hstlist))))))
428                 (cons 0 history))
429               
430                (let ((err (maxerror)))
431                  (let ((hierr (hierror? t tstep err))
432                        (loerr (lowerror? t tstep err)))
433                    (cond (hierr
434                           (begin
435                             (set! tstep (* tstep (max m (sqrt (/ relmax (abs hierr))))))
436                             ((ode-runtime 'solve-env-state-fold) 
437                              (lambda (sym svec hstlist)
438                                (let* ((sthist   (car hstlist))
439                                       (pri      (cdr (stack-peek sthist)))
440                                       (val      (car (stack-peek sthist))))
441                                  (eval-env-set! sym val)
442                                  (svec-value-set! svec val)
443                                  (svec-deriv-set! svec pri))
444                                (cdr hstlist))
445                              history)
446                             (apply loop (rk5 it (eval-env-ref indep) tstep #f))))
447                         
448                          (else
449                           (begin
450                             
451                             
452                             ;; update the derivative and value history
453                             ((ode-runtime 'solve-env-state-fold) 
454                              (lambda (sym svec hstlist)
455                                (let ((sthist  (car hstlist)))
456                                  (let ((sthist1 (stack-push! sthist 
457                                                              (cons (svec-value svec)
458                                                                    (svec-deriv svec)))))
459                                    (let ((dep (stack-depth sthist1)))
460                                      (if (fx<= history-len dep) (stack-cut! sthist1 history-len dep)))
461                                    (cdr hstlist))))
462                              history)
463                             ;; update independent variable
464                             (solve-env-set! indep svec-value-idx (+ t tstep))
465                             (printq)
466                             
467                             (if (and loerr (positive? loerr))
468                                 (set! tstep (* tstep S (min M (sqrt (/ relmax (abs loerr)))))))
469                             
470                             (eval-env-set! indep t)
471                             (eval-env-set! dt tstep)
472                             (eval-derivs!)
473                             
474                             (loop (fx+ 1 it) (+ t tstep) tstep))))))))
475            (list it t tstep (list))))))
476 
477  (if debug (print "abm4: " (ode-runtime 'debug)))   
478  (abm4 0 0.0 initial-tstep)))
Note: See TracBrowser for help on using the repository browser.