(* * LANGUAGE : ANS Forth * PROJECT : Forth Environments * DESCRIPTION : Design & Plot the reponse of IIR filters * CATEGORY : EE Tools * AUTHOR : Marcel Hendrix * LAST CHANGE : Friday, August 02, 2002 11:43 PM, Marcel Hendrix; other sign bugs, }PLOT * LAST CHANGE : Monday, July 22, 2002 3:32 PM, Marcel Hendrix; bug in sign of a{ * LAST CHANGE : Saturday, July 20, 2002 12:10 PM, Marcel Hendrix; added documentation * LAST CHANGE : July 14, 2002, Marcel Hendrix *) NEEDS -miscutil NEEDS -fsl_util NEEDS -sketcher REVISION -iir "ÄÄÄ Plot IIR output Version 1.03 ÄÄÄ" PRIVATES DOC (* Evaluate the filter H(z) = b(z) / a(z) Filters x with the filter described by vectors A and B to create y. The filter is a "Direct Form II Transposed" implementation of the standard difference equation: a(0)*y(n) = b(0)*x(n) + b(1)*x(n-1) + ... + b(nb)*x(n-nb-1) - a(1)*y(n-1) - ... - a(na)*y(n-na-1) (For a FIR filter a(0) is 1, all other a's are 0.) The operation of filter at sample m is implemented using the transposed direct form II structure. This is calculated by the time domain difference equations for y and the states z_i. b(0)*x(m) + z0(m-1)) y(m) = -------------------- a(0) z0(m) = b(1) * x(m) + z1(m-1) - a(1) * y(m) ... ... zn-3(m) = b(n-2) * x(m) + zn-2(m-1) - a(n-2) * y(m) zn-2(m) = b(n-1) * x(m) - a(n-1) * y(m) Example: a second order Butterworth filter with Fc = 3kHz, Fs = 30kHz has a{ }print -> ( 1.0000e+000 0.0000e+000 ) ( 1.1682e+000 0.0000e+000 ) ( 4.2411e-001 0.0000e+000 ) b{ }print -> ( 6.3964e-002 0.0000e+000 ) ( 1.2792e-001 0.0000e+000 ) ( 6.3964e-002 0.0000e+000 ) *) ENDDOC -- Evaluate H(z) in direct II form. Assumes complex a{ and b{ . -- When a{ and b{ are real, more efficient code is possible. 0 VALUE /aptr PRIVATE 0 VALUE /bptr PRIVATE 0 VALUE /wptr PRIVATE COMPLEX DARRAY a1{ PRIVATE COMPLEX DARRAY b1{ PRIVATE COMPLEX DARRAY w1{ PRIVATE : OPEN-h(z) ( a{ b{ -- ) DUP CDIM TO /bptr SWAP DUP CDIM TO /aptr LOCALS| a{ b{ | /aptr /bptr MAX TO /wptr a1{ /wptr }malloc /aptr 0 ?DO a{ I } Z@ a1{ I } Z! LOOP b1{ /wptr }malloc /bptr 0 ?DO b{ I } Z@ b1{ I } Z! LOOP w1{ /wptr }malloc ; ZVARIABLE xk PRIVATE ZVARIABLE yk PRIVATE : H(z) ( F: cxk -- cyk ) xk Z! b1{ 0 } Z@ xk Z@ Z* w1{ 0 } Z@ Z+ a1{ 0 } Z@ Z/ ZDUP yk Z! ( cyk) /wptr 2- 0 ?DO b1{ I 1+ } Z@ xk Z@ Z* a1{ I 1+ } Z@ yk Z@ Z* Z- w1{ I 1+ } Z@ Z+ w1{ I } Z! LOOP b1{ /wptr 1- } Z@ xk Z@ Z* a1{ /wptr 1- } Z@ yk Z@ Z* Z- w1{ /wptr 2- } Z! ; : CLOSE-h(z) ( -- ) a1{ }free b1{ }free w1{ }free CLEAR /aptr CLEAR /bptr CLEAR /wptr ; : >Log ( mag{ -- ) DUP CDIM 0 ?DO DUP I } DUP DF@ FLOG 20e F* DF! LOOP DROP ; -- Fill the output magnitude and phase arrays ----------------------------------------------- COMPLEX DARRAY response{ PRIVATE -- x = 2*pi*m*k/L :INLINE exp(j*x) ( num denom -- ) ( F: -- c ) S>F S>F PI F* FSWAP F/ FEXP(-jX) ; PRIVATE -- Plots from 0 to lambda = pi; eqv. to 0 to Fs/2 : IIResponse ( a{ b{ magnitude{ phase{ n -- ) LOCALS| #points phase{ magnitude{ | DUP CDIM ROT DUP CDIM LOCALS| N a{ M b{ | magnitude{ #points }malloc phase{ #points }malloc response{ #points }malloc \ DFT of H(z) numerator #points 0 DO 0+0i M 0 DO b{ I } Z@ I J * #points exp(j*x) Z* Z+ LOOP response{ I } Z! LOOP \ DFT of H(z) denominator, Note: a0 always 1 #points 0 DO 1+0i N 1 DO a{ I } Z@ I J * #points exp(j*x) Z* Z+ LOOP response{ I } DUP Z@ ZSWAP Z/ Z! LOOP \ compute magnitude and phase #points 0 DO response{ I } Z@ >POLAR FDEG phase{ I } DF! magnitude{ I } DF! LOOP response{ }free ; -- Design filter from analog prototype, given its poles and zeros --------------------------- COMPLEX DARRAY mu{ PRIVATE 2e 0e ZCONSTANT cTWO PRIVATE -- Note: pole{ 0 } and zero{ 0 } are dummies ( should be 0 ). -- Also a{ 0 } is a dummy; made 1+0i for convenience -- The number of zeros should be at most equal to the number of poles. -- The b{ and a{ arrays can be used by h(z) and h(k) directly. : bilinear ( zero{ pole{ a{ b{ -- ) ( F: H0 Fsample -- ) 2SWAP SWAP DUP CDIM 1- ROT DUP CDIM 1- LOCALS| #poles pole{ #zeros zero{ b{ a{ | PI*2 F/ 1/F 1e FLOCALS| hC bigT H0 | mu{ #poles 1+ }malloc a{ #poles 1+ }malloc b{ #poles 1+ }malloc \ constant gain factor 1+0i #poles 1+ 1 DO cTWO pole{ I } Z@ bigT ZSCALE Z- Z* bigT hC F* TO hC LOOP REAL 1/F H0 hC F* F* TO hC \ compute numerator coefficients 1+0i mu{ 0 } Z! 0 >S ( MaxCoeff ) #poles #zeros - 1+ 1 ?DO 1 S+! 1 S DO mu{ I 1- } Z@ mu{ I } Z+! -1 +LOOP LOOP #zeros 1+ 1 ?DO 1 S+! -2e bigT F/ 0e R,I->Z pole{ I } Z@ Z- ( beta ) 1 S DO FDUP mu{ I 1- } Z@ Z* mu{ I } Z+! -1 +LOOP FDROP LOOP -S ( forget MaxCoeff ) #poles 1+ 0 DO mu{ I } Z@ hC ZSCALE b{ I } Z! LOOP \ compute denominator coefficients #poles 1+ 0 DO 0+0i mu{ I } Z! LOOP 1+0i mu{ 0 } Z! #poles 1+ 1 DO -2e bigT F/ 0e R,I->Z pole{ I } Z@ Z- ( delta ) 2e bigT F/ 0e R,I->Z pole{ I } Z@ Z- ( gamma ) Z/ ( eta ) 1 I DO ZDUP mu{ I 1- } Z@ Z* mu{ I } Z+! -1 +LOOP ZDROP LOOP #poles 1+ 1 DO mu{ I } Z@ a{ I } Z! LOOP 1+0i a{ 0 } Z! mu{ }free ; -- Analog prototype generators -------------------------------------------------------------- -- Return order N butterworth poles and zeros. -- This was normalized for omegac = 1 rad/s and for radial frequency. -- For Hz, we don't need the fc PI*2 F* PI*2 F/ : Proto:Butterworth ( zero{ pole{ order -- ) ( F: fc -- H0 ) FLOCAL omegac LOCALS| order pole{ | DUP 1 }RESIZE 0 } 0+0i Z! pole{ order 1+ }RESIZE 0+0i pole{ 0 } Z! order 1+ 1 ?DO order 1- I 2* + S>F PI F* order 2* S>F F/ FEXP(jX) omegac ZSCALE pole{ I } Z! LOOP omegac order S>F F** ; -- Return order N Chebyshev poles and zeros. -- Note: At Fc is not -3 dB, but -ripple[dB]. -- This was normalized for omegac = 1 rad/s and for radial frequency. -- For Hz, we don't need the fc PI*2 F* PI*2 F/ : Proto:Chebyshev ( zero{ pole{ order -- ) ( F: Fc ripple[dB] -- H0 ) FSWAP 0e 0e 0e 0e FLOCALS| sin* cos* gamma epsilon omegac ripple | ripple 0.1e F* FALOG F1- FSQRT TO epsilon LOCALS| order pole{ | DUP 1 }RESIZE 0 } 0+0i Z! pole{ order 1+ }RESIZE 0+0i pole{ 0 } Z! epsilon FSQR F1+ FSQRT F1+ epsilon F/ order S>F 1/F F** TO gamma gamma 1/F gamma F- F2/ TO sin* gamma 1/F gamma F+ F2/ TO cos* 1+0i order 1+ 1 ?DO I 2* 1- S>F PI F* order 2* S>F F/ FSINCOS cos* F* FSWAP sin* F* FSWAP omegac ZSCALE ZDUP pole{ I } Z! FNEGATE FSWAP FNEGATE FSWAP Z* LOOP ZABS order 1 AND 0= IF ripple 0.05e F* FALOG F* ENDIF ; -- Plotting --------------------------------------------------------------------------------- 0 VALUE addr PRIVATE : [plot] ( ix -- ) ( F: -- r ) addr SWAP } DF@ ;P : }PLOT ( x{ n -- ) SWAP TO addr 0 SWAP addr CDIM 1- MIN ['] [plot] 'SKETCHA ; #100 DOUBLE ARRAY impulse{ PRIVATE -- plot the real part : DIRAC-PLOT ( a{ b{ n -- ) LOCALS| n b{ a{ | impulse{ n }malloc a{ b{ OPEN-H(z) 1+0i H(z) REAL impulse{ 0 } DF! n 1- 1 ?DO 0+0i H(z) REAL impulse{ I } DF! LOOP impulse{ TO addr 0 n 1- ['] [plot] 'SKETCHA CLOSE-H(z) impulse{ }free ; -- plot the real part : STEP-PLOT ( a{ b{ n -- ) LOCALS| n b{ a{ | impulse{ n }malloc a{ b{ OPEN-H(z) n 0 ?DO 1+0i H(z) REAL impulse{ I } DF! LOOP impulse{ TO addr 0 n 1- ['] [plot] 'SKETCHA CLOSE-H(z) impulse{ }free ; -- Not implemented yet: pre-warping --------------------------------------------------------- [DEFINED] testing [IF] COMPLEX DARRAY a{ COMPLEX DARRAY b{ COMPLEX DARRAY pole{ COMPLEX DARRAY zero{ DOUBLE DARRAY magnitude{ DOUBLE DARRAY phase{ 3e3 FVALUE Fr 3e3 FVALUE Fc 60e3 FVALUE Fs 1e FVALUE H0 [THEN] :ABOUT CR ." Usage: ( zero{ pole{ order -- ) ( F: Fc -- H0 ) proto:Butterworth -- Butterworth LPF" CR ." ( zero{ pole{ order -- ) ( F: Fr ripple[dB] -- H0 ) proto:Chebyshev -- Chebyshev LPF" CR ." ( zero{ pole{ a{ b{ -- ) ( F: H0 Fs -- ) bilinear -- compute H(z)'s a{ and b{" CR CR ." ( a{ b{ magnitude{ phase{ n -- ) IIResponse -- n-point gain and phase from a{ and b{" CR ." ( magnitude{ -- ) >Log -- >Log modifies magnitude{ in place" CR ." ( magnitude{ | phase{ m -- ) }PLOT -- plot m points of magnitude or phase" CR ." ( a{ b{ m -- ) dirac-plot | step-plot -- plot m samples ( modifies w{ )" CR CR ." ( a{ b{ -- ) OPEN-h(z) -- prepare to evaluate H(z) in time domain" CR ." ( F: cxk -- cyk ) H(z) -- evaluate H(z)" CR ." ( -- ) CLOSE-H(z) -- stop using H(z)" CR CR ." Note: H0 is the DC gain, Fs is the sample frequency, Fc/Fr is the -3dB/ripple frequency" CR ." a{ is the denominator (poles) polynomial, b{ is the numerator (zeros) polynomial," CR ." w{ is memory. a{ 0 } is 1+0i, pole{ 0 } and zero{ 0 } are dummies and should be 0+0i." CR CR ." Available when 1 =: testing is set before loading..." CR ." ( arrays) a{ b{ pole{ zero{ magnitude{ and phase{" CR ." ( values) Fc Fs H0 order" ; DEPRIVE .ABOUT -iir CR (* End of Source *)