├── README.md ├── hilbert.h ├── iirfilter.h ├── hilbert.c ├── LICENSE └── iirfilter.c /README.md: -------------------------------------------------------------------------------- 1 | HilbertTransform 2 | ================ 3 | 4 | Creation of analytic signals and applications 5 | -------------------------------------------------------------------------------- /hilbert.h: -------------------------------------------------------------------------------- 1 | /* hilbert.h - hilbert transform via allpass filters 2 | * Michael A. Casey - Dartmouth College, USA 3 | */ 4 | 5 | #ifndef __HILBERT_H 6 | #define __HILBERT_H 7 | #include "iirfilter.h" 8 | 9 | /* #define __HILBERTTEST__ */ /* Set to compile main() test program */ 10 | 11 | typedef struct SAMPLETVEC { 12 | sampleT* data; 13 | int len; 14 | } vec; 15 | 16 | typedef struct HILBERTSTRUCT{ 17 | double fs; 18 | FILTER** ap_pair; 19 | int buflen; 20 | vec* imvec; 21 | sampleT* y1; 22 | sampleT* y2; 23 | sampleT* cpx; 24 | sampleT phase; 25 | } Hilbert; 26 | 27 | 28 | /* API */ 29 | Hilbert * init_hilbert(int buffer_length, double fs); 30 | void free_hilbert(Hilbert* h); 31 | void analytic(Hilbert* h, sampleT* x); 32 | void freq_shift(Hilbert* h, sampleT* x, double f0); 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /iirfilter.h: -------------------------------------------------------------------------------- 1 | /* iirfilter.h */ 2 | 3 | /* Author: Michael A. Casey 4 | * Language: C 5 | * Copyright (C) 1997 Michael A. Casey, MIT Media Lab, All Rights Reserved 6 | * 7 | * Implementation of filter opcode for general purpose filtering. 8 | * This opcode implements the following difference equation: 9 | * 10 | * (1)*y(n) = b(0)*x(n) + b(1)*x(n-1) + ... + b(nb)*x(n-nb) 11 | * - a(1)*y(n-1) - ... - a(na)*y(n-na) 12 | * 13 | * whose system function is represented by: 14 | * 15 | * -1 -nb 16 | * jw B(z) b(0) + b(1)z + .... + b(nb)z 17 | * H(e) = ---- = ---------------------------- 18 | * -1 -na 19 | * A(z) 1 + a(1)z + .... + a(na)z 20 | * 21 | * 22 | * This is the same as scipy.signal's lfilter is a Direct Form II Transposed Filter: 23 | * -1 -nb 24 | * b[0] + b[1]z + ... + b[nb] z 25 | * Y(z) = ---------------------------------- X(z) 26 | * -1 -na 27 | * a[0] + a[1]z + ... + a[na] z 28 | * 29 | */ 30 | 31 | #ifndef __filter_h 32 | #define __filter_h 33 | #include 34 | 35 | /*#define __FILTERTEST__*/ 36 | #define MAXZEROS 50 /* Allow up to 50th-order digital filters */ 37 | #define MAXPOLES 50 38 | #define OK 104 39 | #define CS_KSMPS 4096 /* samples per buffer */ 40 | 41 | typedef double sampleT; 42 | typedef struct FCOMPLEX {sampleT r,i;} fcomplex; 43 | 44 | /* Structures for FILTER opcode */ 45 | typedef struct { 46 | sampleT *out; /* output signal */ 47 | sampleT *in; /* input signal */ 48 | int *nb, *na; /* filter-order input arguments */ 49 | sampleT coeffs[MAXPOLES+MAXZEROS+1]; /* filter-coefficient input arguments */ 50 | sampleT *d1,*d2; /* These allow ZFILTER to access FILTER routines */ 51 | 52 | int numa; /* i-var p-time storage registers */ 53 | int numb; 54 | 55 | sampleT* delay; /* delay-line state memory base pointer */ 56 | sampleT* currPos; /* delay-line current position pointer */ /* >>Was float<< */ 57 | int ndelay; /* length of delay line (i.e. filter order) */ 58 | } FILTER; 59 | 60 | typedef struct { 61 | sampleT *out; /* output signal */ 62 | sampleT *in; /* input signal */ 63 | sampleT *kmagf, *kphsf; /* magnitude and phase pole nudging factors */ 64 | int *nb, *na; /* filter-order input arguments */ 65 | sampleT coeffs[MAXPOLES+MAXZEROS+1]; /* filter-coefficient input arguments */ 66 | 67 | int numa; /* i-var p-time storage registers */ 68 | int numb; 69 | 70 | sampleT* delay; /* delay-line state memory base pointer */ 71 | sampleT* currPos; /* delay-line current position pointer */ /* >>Was float<< */ 72 | int ndelay; /* length of delay line (i.e. filter order) */ 73 | fcomplex* roots; /* pole roots memory for zfilter */ 74 | } ZFILTER; 75 | 76 | /* API */ 77 | int ifilter(FILTER* p); 78 | void free_filter(FILTER* p); 79 | int izfilter(ZFILTER *p); 80 | void free_zfilter(ZFILTER* p); 81 | int afilter(FILTER* p, uint32_t nsmps); 82 | int azfilter(ZFILTER* p, uint32_t nsmps); 83 | int kfilter(FILTER* p); 84 | 85 | #endif 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /hilbert.c: -------------------------------------------------------------------------------- 1 | /* hilbert.c - hilbert transform via allpass filters 2 | * 3 | * Ideas from: http://yehar.com/blog/?p=368 4 | * originally from http://ldesoras.free.fr/prod.html#src_hiir 5 | * Also see http://www.mathworks.com/help/signal/examples/single-sideband-modulation-via-the-hilbert-transform.html 6 | * 7 | * Copyright (C) 2014 Michael A. Casey, Bregman Media Labs, Dartmouth College, USA 8 | */ 9 | 10 | /*****************************************************************************************************************************/ 11 | /* Here's a quick diagram of the allpass pair: */ 12 | /* */ 13 | /* ................ filter 1 ................. */ 14 | /* +--> allpass --> allpass --> allpass --> allpass --> delay --> out1 */ 15 | /* | */ 16 | /* in */ 17 | /* | ................ filter 2 ................. */ 18 | /* +--> allpass --> allpass --> allpass --> allpass ------------> out2 (+90 deg) */ 19 | /* */ 20 | /* We can use cookbook formulas to convert an allpass section into code. A general IIR recurrence relation: */ 21 | /* */ 22 | /* out(t) = a0*in(t) + a1*in(t-1) + a2*in(t-2) + ... */ 23 | /* + b1*out(t-1) + b2*out(t-2) + ... */ 24 | /* */ 25 | /* results in the transfer function: */ 26 | /* */ 27 | /* a0 + a1*z^-1 + a2*z^-2 + ... */ 28 | /* H(z) = ---------------------------- */ 29 | /* 1 - b1*z^-1 - b2*z^-2 - ... */ 30 | /* */ 31 | /* The allpass section in question has the following transfer function: */ 32 | /* */ 33 | /* a^2 - z^-2 */ 34 | /* H(z) = ------------ */ 35 | /* 1 - a^2 z^-2 */ 36 | /* */ 37 | /* We want to convert this into the recurrence relation. According to the cookbook formulas and the above transfer function: */ 38 | /* */ 39 | /* a0 = a^2, a2 = -1, b2 = a^2, rest of coefficients zero */ 40 | /* */ 41 | /* => out(t) = a^2*in(t) - in(t-2) + a^2*out(t-2) */ 42 | /* */ 43 | /* which simplifies to the one-multiplication allpass section: */ 44 | /* */ 45 | /* out(t) = a^2*(in(t) + out(t-2)) - in(t-2) */ 46 | /*****************************************************************************************************************************/ 47 | 48 | #include 49 | #include 50 | #include 51 | #include "hilbert.h" 52 | 53 | sampleT HilbertCoeffs[2][4] = {{0.6923878, 0.9360654322959, 0.9882295226860, 0.9987488452737}, 54 | {0.4021921162426, 0.8561710882420, 0.9722909545651, 0.9952884791278}}; 55 | 56 | static vec ** H_sect(sampleT a); 57 | static void free_H_sect(vec** allpass_coeffs); 58 | static vec * imp(size_t N); 59 | static vec* convolve(vec* X, vec* Y); 60 | static vec ** H(vec* A0); 61 | static vec ** H_1(vec* A0); 62 | static vec ** H_2(vec* A0); 63 | static FILTER* init_biquad_filter(vec* B, vec* A); 64 | static void free_allpass_filters(FILTER** ap_pair); 65 | static FILTER ** init_allpass_filters(); 66 | 67 | 68 | /* Utility functions */ 69 | 70 | static vec* new_vec(int N, int allocate){ 71 | vec* v = (vec*) calloc(1,sizeof(vec)); 72 | if(allocate){ 73 | v->data = (sampleT*) calloc(N,sizeof(sampleT)); 74 | } 75 | else{ 76 | v->data = NULL; 77 | } 78 | v->len = N; 79 | return v; 80 | } 81 | 82 | static void free_vec(vec* v){ 83 | if(v->data!=NULL){ 84 | free(v->data); 85 | } 86 | free(v); 87 | } 88 | 89 | 90 | /* 91 | * Impulse signal 92 | */ 93 | static vec * imp(size_t N){ 94 | vec *I = new_vec(N,1); 95 | I->data[0] = 1.0; 96 | return I; 97 | } 98 | 99 | /* 100 | * Full direct convolution of two vectors 101 | * Returns a new vector of length N1+N2-1 102 | */ 103 | static vec* convolve(vec* X, vec* Y){ 104 | int i,k; 105 | vec* v = new_vec(X->len+Y->len-1, 1); 106 | for(i=0; ilen; i++){ 107 | for(k=0; klen; k++){ 108 | v->data[i+k] += X->data[i] * Y->data[k]; 109 | } 110 | } 111 | return v; 112 | } 113 | 114 | 115 | 116 | /*************************************************************/ 117 | /* Allpass biquadratic section: */ 118 | /* */ 119 | /* a^2 - z^-2 */ 120 | /* H(z) = ------------- */ 121 | /* 1 - a^2 z^-2 */ 122 | /* */ 123 | /* Implemenation as direct Form II Transposed Filter: */ 124 | /* */ 125 | /* -1 -nb */ 126 | /* b[0] + b[1]z + ... + b[nb] z */ 127 | /* Y(z) = ---------------------------------- X(z) */ 128 | /* -1 -na */ 129 | /* a[0] + a[1]z + ... + a[na] z */ 130 | /*************************************************************/ 131 | static vec ** H_sect(sampleT a){ 132 | vec ** coeffs = (vec**) calloc(2,sizeof(vec*)); 133 | coeffs[0] = new_vec(4,1); 134 | coeffs[1] = new_vec(4,1); 135 | coeffs[0]->data[0] = a*a; 136 | coeffs[0]->data[2] = -1.0; 137 | coeffs[1]->data[0] = 1.0; 138 | coeffs[1]->data[2] = -(a*a); 139 | return coeffs; 140 | } 141 | 142 | static void free_H_sect(vec** allpass_coeffs){ 143 | free_vec(allpass_coeffs[0]);free_vec(allpass_coeffs[1]); 144 | free(allpass_coeffs); 145 | } 146 | 147 | /* 148 | * Allpass hilbert transform sections denominator 149 | */ 150 | static vec ** H(vec* A0){ 151 | vec *B1, *A1, *B2, *A2, *B3, *A3, *B4, *A4, **tmp; 152 | vec *D2, *C2, *D3, *C3, *outB, *outA; 153 | sampleT* a0 = A0->data; 154 | tmp = H_sect(a0[0]); B1=tmp[0]; A1=tmp[1]; 155 | free(tmp); // avoid memory leaks 156 | tmp = H_sect(a0[1]); B2=tmp[0]; A2=tmp[1]; 157 | free(tmp); // avoid memory leaks 158 | tmp = H_sect(a0[2]); B3=tmp[0]; A3=tmp[1]; 159 | free(tmp); // avoid memory leaks 160 | tmp = H_sect(a0[3]); B4=tmp[0]; A4=tmp[1]; 161 | D2 = convolve(B1,B2); C2 = convolve(A1,A2); 162 | D3 = convolve(D2,B3); C3 = convolve(C2,A3); 163 | outB = convolve(D3,B4); outA = convolve(C3,A4); 164 | free_vec(B1);free_vec(A1); 165 | free_vec(B2);free_vec(A2); 166 | free_vec(B3);free_vec(A3); 167 | free_vec(B4);free_vec(A4); 168 | free_vec(D2);free_vec(C2); 169 | free_vec(D3);free_vec(C3); 170 | tmp[0]=outB; tmp[1]=outA; 171 | return tmp; 172 | } 173 | 174 | /* 175 | * Allpass hilbert transform numerator 176 | */ 177 | static vec ** H_1(vec* A0){ 178 | vec ** tmp = H(A0); 179 | vec* unit_delay = new_vec(2,1); 180 | unit_delay->data[1]=1; 181 | vec *B = convolve(tmp[0], unit_delay); 182 | free_vec(unit_delay); 183 | free_vec(tmp[0]); // avoid memory leaks 184 | tmp[0] = B; 185 | return tmp; 186 | } 187 | 188 | /* 189 | * Allpass hilbert transform denominator 190 | */ 191 | static vec ** H_2(vec* A0){ 192 | return H(A0); 193 | } 194 | 195 | /* 196 | * Allpass filters from series biquad filter sections 197 | */ 198 | static FILTER* init_biquad_filter(vec* B, vec* A){ 199 | FILTER* biquad = (FILTER*) calloc(1,sizeof(FILTER)); 200 | biquad->numb = B->len; 201 | biquad->numa = A->len-1; // Assume A[0]=1 and crop array 202 | int i; 203 | for(i=0; inumb; i++){ 204 | biquad->coeffs[i] = B->data[i]; 205 | } 206 | for(i=1; inuma; i++){ // Assume A[0]=1 and crop array 207 | biquad->coeffs[biquad->numb+i-1] = A->data[i]; 208 | } 209 | ifilter(biquad); 210 | return biquad; 211 | } 212 | 213 | /* 214 | * Deallocate allpass filter (multi-section numerator and denominator) 215 | */ 216 | static void free_allpass_filters(FILTER** ap_pair){ 217 | free_filter(ap_pair[0]); 218 | free_filter(ap_pair[1]); 219 | free(ap_pair); 220 | } 221 | 222 | /* 223 | * Initialize allpass filter (multi-section numerator and denominator) 224 | */ 225 | static FILTER ** init_allpass_filters(){ 226 | vec ** allpass_coeffs; 227 | vec* a1 = new_vec(4, 0); 228 | vec* a2 = new_vec(4, 0); 229 | a1->data = HilbertCoeffs[0]; 230 | a2->data = HilbertCoeffs[1]; 231 | allpass_coeffs = H_1(a1); 232 | FILTER* allpass1 = init_biquad_filter(allpass_coeffs[0], allpass_coeffs[1]); 233 | free_H_sect(allpass_coeffs); 234 | 235 | allpass_coeffs = H_2(a2); 236 | FILTER* allpass2 = init_biquad_filter(allpass_coeffs[0], allpass_coeffs[1]); 237 | free_H_sect(allpass_coeffs); 238 | 239 | FILTER** ap_pair = (FILTER**) calloc(2, sizeof(FILTER*)); 240 | ap_pair[0] = allpass1; 241 | ap_pair[1] = allpass2; 242 | a1->data = NULL; a2->data = NULL; 243 | free_vec(a1); free_vec(a2); 244 | return ap_pair; 245 | } 246 | 247 | /* Hilbert transform constructor */ 248 | Hilbert* init_hilbert(int buffer_length, double fs){ 249 | Hilbert* H = (Hilbert*) calloc(1, sizeof(Hilbert)); 250 | H->fs = fs; 251 | H->buflen = buffer_length; 252 | H->ap_pair = init_allpass_filters(); 253 | H->imvec = new_vec(H->buflen,1); // Imaginary part of analytic signal 254 | H->y1 = (sampleT*) calloc(H->buflen,sizeof(sampleT)); 255 | H->y2 = (sampleT*) calloc(H->buflen,sizeof(sampleT)); 256 | H->cpx = (sampleT*) calloc(H->buflen,sizeof(sampleT)); 257 | H->ap_pair[0]->out = H->y1; 258 | H->ap_pair[1]->out = H->y2; 259 | H->phase = 0.0; 260 | if(!(H->ap_pair&&H->buflen&&H->imvec&&H->y1&&H->y2)){ 261 | fprintf(stderr, "Hilbert transformer initialization failed init_hilbert()\n"); 262 | exit(1); 263 | } 264 | return H; 265 | } 266 | 267 | /* Hilbert transform destructor */ 268 | void free_hilbert(Hilbert* H){ 269 | free(H->y1); 270 | free(H->y2); 271 | free(H->cpx); 272 | free_vec(H->imvec); 273 | free_allpass_filters(H->ap_pair); 274 | free(H); 275 | } 276 | 277 | /* 278 | Analytic (complex) signal, in-place, given real and zero-imag input. 279 | H := z -> 0.5*(H_2(z)+I*H_1(z)); 280 | */ 281 | void analytic(Hilbert* H, sampleT* x){ 282 | int i; 283 | if(!H){ 284 | fprintf(stderr, "Hilbert transformer initialization not previously called, analytic()\n"); 285 | exit(1); 286 | } 287 | H->ap_pair[0]->in = x; 288 | afilter(H->ap_pair[0], H->buflen); 289 | H->ap_pair[1]->in = x; 290 | afilter(H->ap_pair[1], H->buflen); 291 | for(i=0; ibuflen; i++){ 292 | x[i] = H->ap_pair[1]->out[i]*0.5; 293 | H->cpx[i] = H->ap_pair[0]->out[i]*0.5; 294 | } 295 | } 296 | 297 | /* 298 | In-place hilbert transformer frequency shifter, by constant offset 299 | Uses single sideband modulation of input signal to carrier (offset) 300 | */ 301 | void freq_shift(Hilbert* H, sampleT* x, double f0){ 302 | double ws = 2*M_PI*f0/H->fs; // Carrier freq 303 | int i; 304 | analytic(H, x); 305 | for(i = 0; ibuflen; i++){ 306 | x[i] = 2 * (cos(ws*i+H->phase)*x[i] - sin(ws*i+H->phase)*H->cpx[i]); 307 | } 308 | H->phase = fmod(H->phase + ws*H->buflen, 2*M_PI); 309 | } 310 | 311 | #ifdef __HILBERTTEST__ 312 | int main(int argc, char* argv[]){ 313 | sampleT* in = (sampleT*)calloc(CS_KSMPS, sizeof(sampleT)); 314 | if(!in){ 315 | fprintf(stderr, "Could not allocate in buffer.\n"); 316 | exit(1); 317 | } 318 | Hilbert* H = init_hilbert(CS_KSMPS, 44100.0); // initialize hilbert transform freq shifter 319 | double w0 = 2 * M_PI * 110.0 / H->fs; 320 | double phase = 0.0; 321 | // 4 buffers of samples 322 | int i,j,k; 323 | for (i=0; i < 4 ; i++){ 324 | for(j=0; j < H->buflen; j++){ 325 | in[j]=cos(w0*j+phase); 326 | } 327 | phase = fmod(phase + w0*H->buflen, 2*M_PI); 328 | freq_shift(H, in, 10.0); 329 | for(j=0; j < H->buflen; j++){ 330 | fprintf(stdout, "%5.4f ", in[j]); 331 | } 332 | } 333 | fprintf(stdout, "\n"); 334 | free_hilbert(H); 335 | free(in); 336 | exit(0); 337 | } 338 | 339 | #endif 340 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "{}" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright {yyyy} {name of copyright owner} 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | 203 | -------------------------------------------------------------------------------- /iirfilter.c: -------------------------------------------------------------------------------- 1 | /* iirfilter.c */ 2 | 3 | /* Author: Michael A. Casey 4 | * Language: C 5 | * 6 | * Implementation of filter opcode for general purpose filtering. 7 | * This opcode implements the following difference equation: 8 | * 9 | * (1)*y(n) = b(0)*x(n) + b(1)*x(n-1) + ... + b(nb)*x(n-nb) 10 | * - a(1)*y(n-1) - ... - a(na)*y(n-na) 11 | * 12 | * whose system function is represented by: 13 | * 14 | * -1 -nb 15 | * jw B(z) b(0) + b(1)z + .... + b(nb)z 16 | * H(e) = ---- = ---------------------------- 17 | * -1 -na 18 | * A(z) 1 + a(1)z + .... + a(na)z 19 | * 20 | * 21 | * This is the same as scipy.signal's lfilter and Matlab's filter: 22 | * It is a Direct Form II Transposed Filter: 23 | * -1 -nb 24 | * b[0] + b[1]z + ... + b[nb] z 25 | * Y(z) = ---------------------------------- X(z) 26 | * -1 -na 27 | * 1 + a[1]z + ... + a[na] z 28 | * 29 | * 30 | * Copyright (C) 1997 Michael A. Casey, MIT Media Lab, All Rights Reserved 31 | * 32 | */ 33 | 34 | #include 35 | #include 36 | #include 37 | #include "iirfilter.h" 38 | 39 | 40 | static sampleT readFilter(FILTER*, int); 41 | static void insertFilter(FILTER*,sampleT); 42 | 43 | #ifndef MAX 44 | #define MAX(a,b) ((a>b)?(a):(b)) 45 | #define MIN(a,b) ((a>b)?(b):(a)) 46 | #endif 47 | 48 | /*#define POLISH (1) */ /* 1=polish pole roots after Laguer root finding */ 49 | 50 | typedef struct FPOLAR {sampleT mag,ph;} fpolar; 51 | 52 | /* Routines associated with pole control */ 53 | static void expandPoly(fcomplex[], sampleT[], int); 54 | static void complex2polar(fcomplex[],fpolar[], int); 55 | static void polar2complex(fpolar[],fcomplex[], int); 56 | static void sortRoots(fcomplex roots[], int dim); 57 | static int sortfun(fpolar *a, fpolar *b); 58 | static void nudgeMags(fpolar a[], fcomplex b[], int dim, sampleT fact); 59 | static void nudgePhases(fpolar a[], fcomplex b[], int dim, sampleT fact); 60 | 61 | static void zroots(fcomplex [], int, fcomplex []); 62 | static fcomplex Cadd(fcomplex, fcomplex); 63 | static fcomplex Csub(fcomplex, fcomplex); 64 | static fcomplex Cmul(fcomplex, fcomplex); 65 | static fcomplex Cdiv(fcomplex, fcomplex); 66 | static fcomplex Complex(sampleT, sampleT); 67 | static sampleT Cabs(fcomplex); 68 | static fcomplex Csqrt(fcomplex); 69 | static fcomplex RCmul(sampleT, fcomplex); 70 | 71 | /* Filter initialization routine */ 72 | int ifilter(FILTER* p) 73 | { 74 | int i; 75 | 76 | /* since i-time arguments are not guaranteed to propegate to p-time 77 | * we must copy the i-vars into the p structure. 78 | */ 79 | 80 | /* First check bounds on initialization arguments */ 81 | if ((p->numb<1) || (p->numb>(MAXZEROS+1)) || 82 | (p->numa<0) || (p->numa>MAXPOLES)){ 83 | fprintf(stderr, "Filter order out of bounds: (1 <= nb < 51, 0 <= na <= 50)"); 84 | return 0; 85 | } 86 | 87 | /* Calculate the total delay in samples and allocate memory for it */ 88 | p->ndelay = MAX(p->numb-1,p->numa); 89 | p->delay = (sampleT*) calloc(p->ndelay, sizeof(sampleT)); 90 | 91 | /* Set current position pointer to beginning of delay */ 92 | p->currPos = p->delay; 93 | 94 | return OK; 95 | } 96 | 97 | void free_filter(FILTER* p){ 98 | if(p->delay!=NULL){ 99 | free(p->delay); 100 | } 101 | free(p); 102 | } 103 | 104 | /* izfilter - initialize z-plane controllable filter */ 105 | int izfilter(ZFILTER *p) 106 | { 107 | fcomplex a[MAXPOLES]; 108 | fcomplex *roots; 109 | sampleT *coeffs; 110 | int i, dim; 111 | 112 | /* since i-time arguments are not guaranteed to propagate to p-time 113 | * we must copy the i-vars into the p structure. 114 | */ 115 | 116 | /* First check bounds on initialization arguments */ 117 | if ((p->numb<1) || (p->numb>(MAXZEROS+1)) || 118 | (p->numa<0) || (p->numa>MAXPOLES)){ 119 | fprintf(stderr, "Filter order out of bounds: (1 <= nb < 51, 0 <= na <= 50)"); 120 | return 0; 121 | } 122 | 123 | /* Calculate the total delay in samples and allocate memory for it */ 124 | p->ndelay = MAX(p->numb-1,p->numa); 125 | p->delay = (sampleT*) calloc(p->ndelay, sizeof(sampleT)); 126 | 127 | /* Set current position pointer to beginning of delay */ 128 | p->currPos = p->delay; 129 | 130 | /* Add auxillary root memory */ 131 | p->roots = (fcomplex*) calloc(p->numa, sizeof(fcomplex)); 132 | roots = p->roots; 133 | dim = p->numa; 134 | 135 | coeffs = p->coeffs + p->numb; 136 | 137 | /* Reverse coefficient order for root finding */ 138 | a[dim] = Complex(1.0,0.0); 139 | for (i=dim-1; i>=0; i--) 140 | a[i] = Complex(coeffs[dim-i-1],0.0); 141 | 142 | /* NRIC root finding routine, a[0..M] roots[1..M] */ 143 | zroots(a, dim, roots-1); 144 | 145 | /* Sort roots into descending order of magnitudes */ 146 | sortRoots(roots, dim); 147 | return OK; 148 | } 149 | 150 | void free_zfilter(ZFILTER* p){ 151 | if(p->delay!=NULL){ 152 | free(p->delay); 153 | } 154 | free(p); 155 | } 156 | 157 | /* a-rate filter routine 158 | * 159 | * Implements the following difference equation 160 | * 161 | * (1)*y(n) = b(0)*x(n) + b(1)*x(n-1) + ... + b(nb)*x(n-nb) 162 | * - a(1)*y(n-1) - ... - a(na)*y(n-na) 163 | * 164 | */ 165 | int afilter(FILTER* p, uint32_t nsmps) 166 | { 167 | int i; 168 | uint32_t offset = 0; //p->h.insdshead->ksmps_offset; 169 | uint32_t early = 0; //p->h.insdshead->ksmps_no_end; 170 | uint32_t n; 171 | 172 | sampleT* a = p->coeffs+p->numb; 173 | sampleT* b = p->coeffs+1; 174 | sampleT b0 = p->coeffs[0]; 175 | 176 | sampleT poleSamp, zeroSamp, inSamp; 177 | 178 | /* Outer loop */ 179 | /*if (UNLIKELY(offset)) memset(p->out, '\0', offset*sizeof(sampleT)); 180 | if (UNLIKELY(early)) { 181 | nsmps -= early; 182 | memset(&p->out[nsmps], '\0', early*sizeof(sampleT)); 183 | }*/ 184 | for (n=offset; nin[n]; 187 | poleSamp = inSamp; 188 | zeroSamp = 0.0; 189 | 190 | /* Inner filter loop */ 191 | for (i=0; i< p->ndelay; i++) { 192 | 193 | /* Do poles first */ 194 | /* Sum of products of a's and delays */ 195 | if (inuma) 196 | poleSamp += -(a[i])*readFilter(p,i+1); 197 | 198 | /* Now do the zeros */ 199 | if (i<(p->numb-1)) 200 | zeroSamp += (b[i])*readFilter(p,i+1); 201 | 202 | } 203 | 204 | p->out[n] = (b0)*poleSamp + zeroSamp; 205 | /* update filter delay line */ 206 | insertFilter(p, poleSamp); 207 | } 208 | return OK; 209 | } 210 | 211 | /* k-rate filter routine 212 | * 213 | * Implements the following difference equation at the k rate 214 | * 215 | * (1)*y(k) = b(0)*x(k) + b(1)*x(k-1) + ... + b(nb)*x(k-nb) 216 | * - a(1)*y(k-1) - ... - a(na)*y(k-na) 217 | * 218 | */ 219 | int kfilter(FILTER* p) 220 | { 221 | int i; 222 | 223 | sampleT* a = p->coeffs+p->numb; 224 | sampleT* b = p->coeffs+1; 225 | sampleT b0 = p->coeffs[0]; 226 | 227 | sampleT poleSamp, zeroSamp, inSamp; 228 | 229 | inSamp = *p->in; 230 | poleSamp = inSamp; 231 | zeroSamp = 0.0; 232 | 233 | /* Filter loop */ 234 | for (i=0; indelay; i++) { 235 | 236 | /* Do poles first */ 237 | /* Sum of products of a's and delays */ 238 | if (inuma) 239 | poleSamp += -(a[i])*readFilter(p,i+1); 240 | 241 | /* Now do the zeros */ 242 | if (i<(p->numb-1)) 243 | zeroSamp += (b[i])*readFilter(p,i+1); 244 | } 245 | 246 | *p->out = ((b0)*poleSamp + zeroSamp); 247 | 248 | /* update filter delay line */ 249 | insertFilter(p, poleSamp); 250 | return OK; 251 | } 252 | 253 | /* azfilter - a-rate controllable pole filter 254 | * 255 | * This filter allows control over the magnitude 256 | * and frequency response of the filter by efficient 257 | * manipulation of the poles. 258 | * 259 | * The k-rate controls are: 260 | * 261 | * kmag, kfreq 262 | * 263 | * The rest of the filter is the same as filter 264 | * 265 | */ 266 | int azfilter(ZFILTER* p, uint32_t nsmps) 267 | { 268 | int i; 269 | uint32_t offset = 0; // p->h.insdshead->ksmps_offset; 270 | uint32_t early = 0; // p->h.insdshead->ksmps_no_end; 271 | uint32_t n; 272 | 273 | sampleT* a = p->coeffs+p->numb; 274 | sampleT* b = p->coeffs+1; 275 | sampleT b0 = p->coeffs[0]; 276 | 277 | sampleT poleSamp, zeroSamp, inSamp; 278 | 279 | fpolar B[MAXPOLES]; 280 | fcomplex C[MAXPOLES+1]; 281 | 282 | fcomplex *roots = p->roots; 283 | sampleT kmagf = *p->kmagf; /* Mag nudge factor */ 284 | sampleT kphsf = *p->kphsf; /* Phs nudge factor */ 285 | 286 | int dim = p->numa; 287 | 288 | /* Nudge pole magnitudes */ 289 | complex2polar(roots,B,dim); 290 | nudgeMags(B,roots,dim,kmagf); 291 | nudgePhases(B,roots,dim,kphsf); 292 | polar2complex(B,C,dim); 293 | expandPoly(C,a,dim); 294 | 295 | /* C now contains the complex roots of the nudged filter */ 296 | /* and a contains their associated real coefficients. */ 297 | 298 | /* Outer loop */ 299 | /*if (UNLIKELY(offset)) memset(p->out, '\0', offset*sizeof(sampleT)); 300 | if (UNLIKELY(early)) { 301 | nsmps -= early; 302 | memset(&p->out[nsmps], '\0', early*sizeof(sampleT)); 303 | } 304 | */ 305 | for (n=offset; nin[n]; 307 | poleSamp = inSamp; 308 | zeroSamp = 0.0; 309 | 310 | /* Inner filter loop */ 311 | for (i=0; i< p->ndelay; i++) { 312 | 313 | /* Do poles first */ 314 | /* Sum of products of a's and delays */ 315 | if (inuma) 316 | poleSamp += -(a[i])*readFilter((FILTER*)p,i+1); 317 | 318 | /* Now do the zeros */ 319 | if (i<(p->numb-1)) 320 | zeroSamp += (b[i])*readFilter((FILTER*)p,i+1); 321 | } 322 | 323 | p->out[n] = (b0)*poleSamp + zeroSamp; 324 | 325 | /* update filter delay line */ 326 | insertFilter((FILTER*)p, poleSamp); 327 | } 328 | return OK; 329 | } 330 | 331 | /* readFilter -- delay-line access routine 332 | * 333 | * Reads sample x[n-i] from a previously established delay line. 334 | * With this syntax i is +ve for a time delay and -ve for a time advance. 335 | * 336 | * The use of explicit indexing rather than implicit index incrementing 337 | * allows multiple lattice structures to access the same delay line. 338 | * 339 | */ 340 | static inline sampleT readFilter(FILTER* p, int i) 341 | { 342 | sampleT* readPoint; /* Generic pointer address */ 343 | 344 | /* Calculate the address of the index for this read */ 345 | readPoint = p->currPos - i; 346 | 347 | /* Wrap around for time-delay if necessary */ 348 | if (readPoint < (p->delay) ) 349 | readPoint += p->ndelay; 350 | else 351 | /* Wrap for time-advance if necessary */ 352 | if (readPoint > (p->delay + (p->ndelay-1)) ) 353 | readPoint -= p->ndelay; 354 | 355 | return *readPoint; /* Dereference read address for delayed value */ 356 | } 357 | 358 | /* insertFilter -- delay-line update routine 359 | * 360 | * Inserts the passed value into the currPos and increments the 361 | * currPos pointer modulo the length of the delay line. 362 | * 363 | */ 364 | static inline void insertFilter(FILTER* p, sampleT val) 365 | { 366 | /* Insert the passed value into the delay line */ 367 | *p->currPos = val; 368 | 369 | /* Update the currPos pointer and wrap modulo the delay length */ 370 | if ((++p->currPos) > 371 | (p->delay + (p->ndelay-1)) ) 372 | p->currPos -= p->ndelay; 373 | } 374 | 375 | /* Compute polynomial coefficients from the roots */ 376 | /* The expanded polynomial is computed as a[0..N] in 377 | * descending powers of Z 378 | */ 379 | static void expandPoly(fcomplex roots[], sampleT a[], int dim) 380 | { 381 | int j,k; 382 | fcomplex z[MAXPOLES],d[MAXPOLES]; 383 | 384 | z[0] = Complex(1.0, 0.0); 385 | for (j=1;j<=dim;j++) 386 | z[j] = Complex(0.0,0.0); 387 | 388 | /* Recursive coefficient expansion about the roots of A(Z) */ 389 | for (j=0;jmagmag) 441 | return 1; 442 | else if (a->mag==b->mag) 443 | return 0; 444 | else 445 | return -1; 446 | } 447 | 448 | /* nudgeMags - Pole magnitude nudging routine 449 | * 450 | * Find the largest-magnitude pole off the real axis 451 | * and nudge all non-real poles by a factor of the distance 452 | * of the largest pole to the unit circle (or zero if fact is -ve). 453 | * 454 | * This has the effect of changing the time-response of the filter 455 | * without affecting the overall frequency response characteristic. 456 | * 457 | */ 458 | static void nudgeMags(fpolar a[], fcomplex b[], int dim, sampleT fact) 459 | { 460 | sampleT eps = .000001; /* To avoid underflow comparisons */ 461 | sampleT nudgefact; 462 | int i; 463 | 464 | /* Check range of nudge factor */ 465 | if (fact>0 && fact<=1) { 466 | /* The largest magnitude pole will be at the beginning of 467 | * the array since it was previously sorted by the init routine. 468 | */ 469 | for (i=0;ieps) /* Check if pole is complex */ 471 | break; 472 | 473 | nudgefact = 1 + (1/a[i].mag-1)*fact; 474 | 475 | /* Nudge all complex-pole magnitudes by this factor */ 476 | for (i=dim-1;i>=0;i--) 477 | if (fabs(b[i].i)>eps) 478 | a[i].mag *= nudgefact; 479 | } 480 | else if (fact < 0 && fact >=-1) { 481 | 482 | nudgefact = (fact + 1); 483 | 484 | /* Nudge all complex-pole magnitudes by this factor */ 485 | for (i=dim-1;i>=0;i--) 486 | if (fabs(b[i].i)>eps) 487 | a[i].mag *= nudgefact; 488 | } 489 | else { 490 | /* Factor is out of range, do nothing */ 491 | } 492 | } 493 | 494 | /* nudgePhases - Pole phase nudging routine 495 | * 496 | * Multiply phases of all poles by factor 497 | */ 498 | static void nudgePhases(fpolar a[], fcomplex b[], int dim, sampleT fact) 499 | { 500 | sampleT eps = .000001; /* To avoid underflow comparisons */ 501 | sampleT nudgefact; 502 | int i; 503 | sampleT phmax=0.0; 504 | 505 | /* Check range of nudge factor */ 506 | if (fact>0 && fact<=1) { 507 | /* Find the largest angled non-real pole */ 508 | for (i=0;iphmax) 510 | phmax = a[i].ph; 511 | 512 | phmax /= M_PI; /* Normalize to radian frequency */ 513 | 514 | nudgefact = 1 + (1-phmax)*fact; 515 | 516 | /* Nudge all complex-pole magnitudes by this factor */ 517 | for (i=dim-1;i>=0;i--) 518 | if (fabs(b[i].i)>eps) 519 | a[i].ph *= nudgefact; 520 | } 521 | else if (fact < 0 && fact >=-1) { 522 | nudgefact = (fact + 1); 523 | 524 | /* Nudge all complex-pole magnitudes by this factor */ 525 | for (i=dim-1;i>=0;i--) 526 | if (fabs(b[i].i)>eps) 527 | a[i].ph *= nudgefact; 528 | } 529 | else { 530 | /* Factor is out of range, do nothing */ 531 | } 532 | } 533 | 534 | /* ------------------------------------------------------------ */ 535 | 536 | /* Code from Press, Teukolsky, Vettering and Flannery 537 | * Numerical Recipes in C, 2nd Edition, Cambridge 1992. 538 | */ 539 | 540 | #define EPSS (1.0e-7) 541 | #define MR (8) 542 | #define MT (10) 543 | #define MAXIT (MT*MR) 544 | 545 | /* Simple definition is sufficient */ 546 | #define FPMAX(a,b) (a>b ? a : b) 547 | 548 | static void laguer(fcomplex a[], int m, fcomplex *x, int *its) 549 | { 550 | int iter,j; 551 | sampleT abx,abp,abm,err; 552 | fcomplex dx,x1,b,d,f,g,h,sq,gp,gm,g2; 553 | static const sampleT frac[MR+1] = {0.0,0.5,0.25,0.75,0.13,0.38,0.62,0.88,1.0}; 554 | 555 | for (iter=1; iter<=MAXIT; iter++) { 556 | *its = iter; 557 | b = a[m]; 558 | err = Cabs(b); 559 | d = f = Complex(0.0,0.0); 560 | abx = Cabs(*x); 561 | for (j=m-1; j>=0; j--) { 562 | f = Cadd(Cmul(*x,f),d); 563 | d = Cadd(Cmul(*x,d),b); 564 | b = Cadd(Cmul(*x,b),a[j]); 565 | err = Cabs(b)+abx*err; 566 | } 567 | err *= EPSS; 568 | if (Cabs(b) <= err) return; 569 | g = Cdiv(d,b); 570 | g2 = Cmul(g,g); 571 | h = Csub(g2,RCmul(2.0,Cdiv(f,b))); 572 | sq = Csqrt(RCmul( (m-1),Csub(RCmul( m,h),g2))); 573 | gp = Cadd(g,sq); 574 | gm = Csub(g,sq); 575 | abp = Cabs(gp); 576 | abm = Cabs(gm); 577 | if (abp < abm) gp = gm; 578 | dx = ((FPMAX(abp,abm) > 0.0 ? Cdiv(Complex( m,0.0),gp) 579 | : RCmul(exp(log(1.0+abx)), 580 | Complex(cos(iter), 581 | sin(iter))))); 582 | x1 = Csub(*x,dx); 583 | if (x->r == x1.r && x->i == x1.i) return; 584 | if (iter % MT) *x = x1; 585 | else *x = Csub(*x,RCmul(frac[iter/MT],dx)); 586 | } 587 | fprintf(stderr,"too many iterations in laguer"); 588 | return; 589 | } 590 | #undef EPSS 591 | #undef MR 592 | #undef MT 593 | #undef MAXIT 594 | /* (C) Copr. 1986-92 Numerical Recipes Software *%&&"U^3. */ 595 | 596 | /* ------------------------------------------------------------ */ 597 | 598 | /* Code from Press, Teukolsky, Vettering and Flannery 599 | * Numerical Recipes in C, 2nd Edition, Cambridge 1992. 600 | */ 601 | 602 | #define EPS (2.0e-6) 603 | #define MAXM (100) 604 | 605 | static void zroots(fcomplex a[], int m, fcomplex roots[]) 606 | { 607 | int i,its,j,jj; 608 | fcomplex x,b,c,ad[MAXM]; 609 | 610 | for (j=0; j<=m; j++) ad[j] = a[j]; 611 | for (j=m; j>=1; j--) { 612 | x = Complex(0.0,0.0); 613 | laguer(ad,j,&x,&its); 614 | if (fabs(x.i) <= 2.0*EPS*fabs(x.r)) x.i = 0.0; 615 | roots[j] = x; 616 | b = ad[j]; 617 | for (jj=j-1; jj>=0; jj--) { 618 | c = ad[jj]; 619 | ad[jj] = b; 620 | b = Cadd(Cmul(x,b),c); 621 | } 622 | } 623 | /* if (poleish) */ 624 | for (j=1; j<=m; j++) 625 | laguer(a,m,&roots[j],&its); 626 | for (j=2; j<=m; j++) { 627 | x = roots[j]; 628 | for (i=j-1; i>=1; i--) { 629 | if (roots[i].r <= x.r) break; 630 | roots[i+1] = roots[i]; 631 | } 632 | roots[i+1] = x; 633 | } 634 | } 635 | #undef EPS 636 | #undef MAXM 637 | /* (C) Copr. 1986-92 Numerical Recipes Software *%&&"U^3. */ 638 | 639 | /* Code from Press, Teukolsky, Vettering and Flannery 640 | * Numerical Recipes in C, 2nd Edition, Cambridge 1992. 641 | */ 642 | 643 | static fcomplex Cadd(fcomplex a, fcomplex b) 644 | { 645 | fcomplex c; 646 | c.r = a.r+b.r; 647 | c.i = a.i+b.i; 648 | return c; 649 | } 650 | 651 | static fcomplex Csub(fcomplex a, fcomplex b) 652 | { 653 | fcomplex c; 654 | c.r = a.r-b.r; 655 | c.i = a.i-b.i; 656 | return c; 657 | } 658 | 659 | static fcomplex Cmul(fcomplex a, fcomplex b) 660 | { 661 | fcomplex c; 662 | c.r = a.r*b.r-a.i*b.i; 663 | c.i = a.i*b.r+a.r*b.i; 664 | return c; 665 | } 666 | 667 | static fcomplex Complex(sampleT re, sampleT im) 668 | { 669 | fcomplex c; 670 | c.r = re; 671 | c.i = im; 672 | return c; 673 | } 674 | 675 | /* fcomplex Conjg(fcomplex z) */ 676 | /* { */ 677 | /* fcomplex c; */ 678 | /* c.r = z.r; */ 679 | /* c.i = -z.i; */ 680 | /* return c; */ 681 | /* } */ 682 | 683 | static fcomplex Cdiv(fcomplex a, fcomplex b) 684 | { 685 | fcomplex c; 686 | sampleT r,den; 687 | if (fabs(b.r) >= fabs(b.i)) { 688 | r = b.i/b.r; 689 | den = b.r+r*b.i; 690 | c.r = (a.r+r*a.i)/den; 691 | c.i = (a.i-r*a.r)/den; 692 | } 693 | else { 694 | r = b.r/b.i; 695 | den = b.i+r*b.r; 696 | c.r = (a.r*r+a.i)/den; 697 | c.i = (a.i*r-a.r)/den; 698 | } 699 | return c; 700 | } 701 | 702 | static sampleT Cabs(fcomplex z) 703 | { 704 | sampleT x,y,ans; 705 | sampleT temp; 706 | x = fabs(z.r); 707 | y = fabs(z.i); 708 | if (x == 0.0) 709 | ans = y; 710 | else if (y == 0.0) 711 | ans = x; 712 | else if (x > y) { 713 | temp = (y/x); 714 | ans = x*sqrt(1.0+temp*temp); 715 | } 716 | else { 717 | temp = (x/y); 718 | ans = y*sqrt(1.0+temp*temp); 719 | } 720 | return ans; 721 | } 722 | 723 | static fcomplex Csqrt(fcomplex z) 724 | { 725 | fcomplex c; 726 | sampleT w; 727 | sampleT x,y,r; 728 | if ((z.r == 0.0) && (z.i == 0.0)) { 729 | c.r = 0.0; 730 | c.i = 0.0; 731 | return c; 732 | } 733 | else { 734 | x = fabs(z.r); 735 | y = fabs(z.i); 736 | if (x >= y) { 737 | r = y/x; 738 | w = sqrt(x)*sqrt(0.5*(1.0+sqrt(1.0+r*r))); 739 | } 740 | else { 741 | r = x/y; 742 | w = sqrt(y)*sqrt(0.5*(r+sqrt(1.0+r*r))); 743 | } 744 | if (z.r >= 0.0) { 745 | c.r = w; 746 | c.i = z.i/(2.0*w); 747 | } else { 748 | c.i = (z.i >= 0.0) ? w : -w; 749 | c.r = z.i/(2.0*c.i); 750 | } 751 | return c; 752 | } 753 | } 754 | 755 | static fcomplex RCmul(sampleT x, fcomplex a) 756 | { 757 | fcomplex c; 758 | c.r = x*a.r; 759 | c.i = x*a.i; 760 | return c; 761 | } 762 | 763 | #ifdef __FILTERTEST__ 764 | 765 | int main(int argc, char* argv[]){ 766 | 767 | FILTER *f = (FILTER*) calloc(1, sizeof(FILTER)); 768 | int na=1,nb=1; 769 | sampleT in[CS_KSMPS]; 770 | sampleT out[CS_KSMPS]; 771 | 772 | if(f == NULL){ 773 | fprintf(stderr, "Could not allocate filter.\n"); 774 | exit(1); 775 | } 776 | fprintf(stderr, "Filter allocated: %d bytes\n", sizeof(FILTER)); 777 | 778 | f->numa = na; 779 | f->numb = nb; 780 | f->in = in; 781 | f->out= out; 782 | f->coeffs[0] = 0.5; // b[0], nb=1 783 | f->coeffs[1] = 0.5; // a[1], na=1 (a[0]=1) 784 | 785 | ifilter(f); // initialize 786 | 787 | // 4 buffers of samples 788 | int i,j,k; 789 | for (i=0; i<4; i++){ 790 | if(i==0){ 791 | f->in[0]=1.0; // Measure impulse response 792 | } 793 | else{ 794 | f->in[0]=0.0; 795 | } 796 | afilter(f, CS_KSMPS); 797 | for(j=0; jout[j]); 799 | } 800 | fprintf(stdout, "\n"); 801 | } 802 | 803 | free_filter(f); 804 | exit(0); 805 | 806 | 807 | } 808 | 809 | #endif 810 | --------------------------------------------------------------------------------