C语言实现python的butter函数

/* * COPYRIGHT * * liir - Recursive digital filter functions * Copyright (C) 2007 Exstrom Laboratories LLC * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * A copy of the GNU General Public License is available on the internet at: * * http://www.gnu.org/copyleft/gpl.html * * or you can write to: * * The Free Software Foundation, Inc. * 675 Mass Ave * Cambridge, MA 02139, USA * * You can contact Exstrom Laboratories LLC via Email at: * * stefan(AT)exstrom.com * * or you can write to: * * Exstrom Laboratories LLC * P.O. Box 7651 * Longmont, CO 80501, USA * */ #include <stdlib.h> #include <stdio.h> #include <string.h> #include <math.h> #include "iir.h" /********************************************************************** binomial_mult - multiplies a series of binomials together and returns the coefficients of the resulting polynomial. The multiplication has the following form: (x+p[0])*(x+p[1])*...*(x+p[n-1]) The p[i] coefficients are assumed to be complex and are passed to the function as a pointer to an array of doubles of length 2n. The resulting polynomial has the following form: x^n + a[0]*x^n-1 + a[1]*x^n-2 + ... +a[n-2]*x + a[n-1] The a[i] coefficients can in general be complex but should in most cases turn out to be real. The a[i] coefficients are returned by the function as a pointer to an array of doubles of length 2n. Storage for the array is allocated by the function and should be freed by the calling program when no longer needed. Function arguments: n - The number of binomials to multiply p - Pointer to an array of doubles where p[2i] (i=0...n-1) is assumed to be the real part of the coefficient of the ith binomial and p[2i+1] is assumed to be the imaginary part. The overall size of the array is then 2n. */ double *binomial_mult( int n, double *p ) { int i, j; double *a; a = (double *)IIR_CALLOC( 2 * n, sizeof(double) ); if( a == NULL ) return( NULL ); for( i = 0; i < n; ++i ) { for( j = i; j > 0; --j ) { a[2*j] += p[2*i] * a[2*(j-1)] - p[2*i+1] * a[2*(j-1)+1]; a[2*j+1] += p[2*i] * a[2*(j-1)+1] + p[2*i+1] * a[2*(j-1)]; } a[0] += p[2*i]; a[1] += p[2*i+1]; } return( a ); } /********************************************************************** trinomial_mult - multiplies a series of trinomials together and returns the coefficients of the resulting polynomial. The multiplication has the following form: (x^2 + b[0]x + c[0])*(x^2 + b[1]x + c[1])*...*(x^2 + b[n-1]x + c[n-1]) The b[i] and c[i] coefficients are assumed to be complex and are passed to the function as a pointers to arrays of doubles of length 2n. The real part of the coefficients are stored in the even numbered elements of the array and the imaginary parts are stored in the odd numbered elements. The resulting polynomial has the following form: x^2n + a[0]*x^2n-1 + a[1]*x^2n-2 + ... +a[2n-2]*x + a[2n-1] The a[i] coefficients can in general be complex but should in most cases turn out to be real. The a[i] coefficients are returned by the function as a pointer to an array of doubles of length 4n. The real and imaginary parts are stored, respectively, in the even and odd elements of the array. Storage for the array is allocated by the function and should be freed by the calling program when no longer needed. Function arguments: n - The number of trinomials to multiply b - Pointer to an array of doubles of length 2n. c - Pointer to an array of doubles of length 2n. */ double *trinomial_mult( int n, double *b, double *c ) { int i, j; double *a; a = (double *)IIR_CALLOC( 4 * n, sizeof(double) ); if( a == NULL ) return( NULL ); a[2] = c[0]; a[3] = c[1]; a[0] = b[0]; a[1] = b[1]; for( i = 1; i < n; ++i ) { a[2*(2*i+1)] += c[2*i]*a[2*(2*i-1)] - c[2*i+1]*a[2*(2*i-1)+1]; a[2*(2*i+1)+1] += c[2*i]*a[2*(2*i-1)+1] + c[2*i+1]*a[2*(2*i-1)]; for( j = 2*i; j > 1; --j ) { a[2*j] += b[2*i] * a[2*(j-1)] - b[2*i+1] * a[2*(j-1)+1] + c[2*i] * a[2*(j-2)] - c[2*i+1] * a[2*(j-2)+1]; a[2*j+1] += b[2*i] * a[2*(j-1)+1] + b[2*i+1] * a[2*(j-1)] + c[2*i] * a[2*(j-2)+1] + c[2*i+1] * a[2*(j-2)]; } a[2] += b[2*i] * a[0] - b[2*i+1] * a[1] + c[2*i]; a[3] += b[2*i] * a[1] + b[2*i+1] * a[0] + c[2*i+1]; a[0] += b[2*i]; a[1] += b[2*i+1]; } return( a ); } /********************************************************************** dcof_bwlp - calculates the d coefficients for a butterworth lowpass filter. The coefficients are returned as an array of doubles. */ double *dcof_bwlp( int n, double fcf ) { int k; // loop variables double theta; // M_PI * fcf / 2.0 double st; // sine of theta double ct; // cosine of theta double parg; // pole angle double sparg; // sine of the pole angle double cparg; // cosine of the pole angle double a; // workspace variable double *rcof; // binomial coefficients double *dcof; // dk coefficients rcof = (double *)IIR_CALLOC( 2 * n, sizeof(double) ); if( rcof == NULL ) return( NULL ); theta = M_PI * fcf; st = sin(theta); ct = cos(theta); for( k = 0; k < n; ++k ) { parg = M_PI * (double)(2*k+1)/(double)(2*n); sparg = sin(parg); cparg = cos(parg); a = 1.0 + st*sparg; rcof[2*k] = -ct/a; rcof[2*k+1] = -st*cparg/a; } dcof = binomial_mult( n, rcof ); IIR_FREE( rcof ); dcof[1] = dcof[0]; dcof[0] = 1.0; for( k = 3; k <= n; ++k ) dcof[k] = dcof[2*k-2]; return( dcof ); } /********************************************************************** dcof_bwhp - calculates the d coefficients for a butterworth highpass filter. The coefficients are returned as an array of doubles. */ double *dcof_bwhp( int n, double fcf ) { return( dcof_bwlp( n, fcf ) ); } /********************************************************************** dcof_bwbp - calculates the d coefficients for a butterworth bandpass filter. The coefficients are returned as an array of doubles. */ double *dcof_bwbp( int n, double f1f, double f2f ) { int k; // loop variables double theta; // M_PI * (f2f - f1f) / 2.0 double cp; // cosine of phi double st; // sine of theta double ct; // cosine of theta double s2t; // sine of 2*theta double c2t; // cosine 0f 2*theta double *rcof; // z^-2 coefficients double *tcof; // z^-1 coefficients double *dcof; // dk coefficients double parg; // pole angle double sparg; // sine of pole angle double cparg; // cosine of pole angle double a; // workspace variables cp = cos(M_PI * (f2f + f1f) / 2.0); theta = M_PI * (f2f - f1f) / 2.0; st = sin(theta); ct = cos(theta); s2t = 2.0*st*ct; // sine of 2*theta c2t = 2.0*ct*ct - 1.0; // cosine of 2*theta rcof = (double *)IIR_CALLOC( 2 * n, sizeof(double) ); tcof = (double *)IIR_CALLOC( 2 * n, sizeof(double) ); for( k = 0; k < n; ++k ) { parg = M_P