iir.c

Go to the documentation of this file.

/*
 *                            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>
#ifndef __USE_GNU
#define __USE_GNU
#endif
#include <math.h>
#include "iir.h"

#ifndef M_PIl
#define M_PIl (3.14159265358979323846264338327950288)
#endif


/**********************************************************************
  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 long double s 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 long double s 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 long double s 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.
*/

long double  *binomial_mult( int n, long double  *p )
{
    int i, j;
    long double  *a;

    a = (long double  *)calloc( 2 * n, sizeof(long 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 long double s 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 long double s 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 long double s of length 2n.
  c  -  Pointer to an array of long double s of length 2n.
*/

long double  *trinomial_mult( int n, long double  *b, long double  *c )
{
    int i, j;
    long double  *a;

    a = (long double  *)calloc( 4 * n, sizeof(long 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 long double s.

*/

long double  *dcof_bwlp( int n, long double  fcf )
{
    int k;            // loop variables
    long double  theta;     // M_PIl * fcf / 2.0
    long double  st;        // sine of theta
    long double  ct;        // cosine of theta
    long double  parg;      // pole angle
    long double  sparg;     // sine of the pole angle
    long double  cparg;     // cosine of the pole angle
    long double  a;         // workspace variable
    long double  *rcof;     // binomial coefficients
    long double  *dcof;     // dk coefficients

    rcof = (long double  *)calloc( 2 * n, sizeof(long double ) );
    if( rcof == NULL ) return( NULL );

    theta = M_PIl * fcf;
    st = sinl(theta);
    ct = cosl(theta);

    for( k = 0; k < n; ++k )
    {
  parg = M_PIl * (long double )(2*k+1)/(long double )(2*n);
  sparg = sinl(parg);
  cparg = cosl(parg);
  a = 1.0 + st*sparg;
  rcof[2*k] = -ct/a;
  rcof[2*k+1] = -st*cparg/a;
    }

    dcof = binomial_mult( n, rcof );
    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 long double s.

*/

long double  *dcof_bwhp( int n, long 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 long double s.

*/

long double  *dcof_bwbp( int n, long double  f1f, long double  f2f )
{
    int k;            // loop variables
    long double  theta;     // M_PIl * (f2f - f1f) / 2.0
    long double  cp;        // cosine of phi
    long double  st;        // sine of theta
    long double  ct;        // cosine of theta
    long double  s2t;       // sine of 2*theta
    long double  c2t;       // cosine 0f 2*theta
    long double  *rcof;     // z^-2 coefficients
    long double  *tcof;     // z^-1 coefficients
    long double  *dcof;     // dk coefficients
    long double  parg;      // pole angle
    long double  sparg;     // sine of pole angle
    long double  cparg;     // cosine of pole angle
    long double  a;         // workspace variables

    cp = cosl(M_PIl * (f2f + f1f) / 2.0);
    theta = M_PIl * (f2f - f1f) / 2.0;
    st = sinl(theta);
    ct = cosl(theta);
    s2t = 2.0*st*ct;        // sine of 2*theta
    c2t = 2.0*ct*ct - 1.0;  // cosine of 2*theta

    rcof = (long double  *)calloc( 2 * n, sizeof(long double ) );
    tcof = (long double  *)calloc( 2 * n, sizeof(long double ) );

    for( k = 0; k < n; ++k )
    {
  parg = M_PIl * (long double )(2*k+1)/(long double )(2*n);
  sparg = sinl(parg);
  cparg = cosl(parg);
  a = 1.0 + s2t*sparg;
  rcof[2*k] = c2t/a;
  rcof[2*k+1] = s2t*cparg/a;
  tcof[2*k] = -2.0*cp*(ct+st*sparg)/a;
  tcof[2*k+1] = -2.0*cp*st*cparg/a;
    }

    dcof = trinomial_mult( n, tcof, rcof );
    free( tcof );
    free( rcof );

    dcof[1] = dcof[0];
    dcof[0] = 1.0;
    for( k = 3; k <= 2*n; ++k )
        dcof[k] = dcof[2*k-2];
    return( dcof );
}

/**********************************************************************
  dcof_bwbs - calculates the d coefficients for a butterworth bandstop
  filter. The coefficients are returned as an array of long double s.

*/

long double  *dcof_bwbs( int n, long double  f1f, long double  f2f )
{
    int k;            // loop variables
    long double  theta;     // M_PIl * (f2f - f1f) / 2.0
    long double  cp;        // cosine of phi
    long double  st;        // sine of theta
    long double  ct;        // cosine of theta
    long double  s2t;       // sine of 2*theta
    long double  c2t;       // cosine 0f 2*theta
    long double  *rcof;     // z^-2 coefficients
    long double  *tcof;     // z^-1 coefficients
    long double  *dcof;     // dk coefficients
    long double  parg;      // pole angle
    long double  sparg;     // sine of pole angle
    long double  cparg;     // cosine of pole angle
    long double  a;         // workspace variables

    cp = cosl(M_PIl * (f2f + f1f) / 2.0);
    theta = M_PIl * (f2f - f1f) / 2.0;
    st = sinl(theta);
    ct = cosl(theta);
    s2t = 2.0*st*ct;        // sine of 2*theta
    c2t = 2.0*ct*ct - 1.0;  // cosine 0f 2*theta

    rcof = (long double  *)calloc( 2 * n, sizeof(long double ) );
    tcof = (long double  *)calloc( 2 * n, sizeof(long double ) );

    for( k = 0; k < n; ++k )
    {
  parg = M_PIl * (long double )(2*k+1)/(long double )(2*n);
  sparg = sinl(parg);
  cparg = cosl(parg);
  a = 1.0 + s2t*sparg;
  rcof[2*k] = c2t/a;
  rcof[2*k+1] = -s2t*cparg/a;
  tcof[2*k] = -2.0*cp*(ct+st*sparg)/a;
  tcof[2*k+1] = 2.0*cp*st*cparg/a;
    }

    dcof = trinomial_mult( n, tcof, rcof );
    free( tcof );
    free( rcof );

    dcof[1] = dcof[0];
    dcof[0] = 1.0;
    for( k = 3; k <= 2*n; ++k )
        dcof[k] = dcof[2*k-2];
    return( dcof );
}

/**********************************************************************
  ccof_bwlp - calculates the c coefficients for a butterworth lowpass
  filter. The coefficients are returned as an array of integers.

*/

int *ccof_bwlp( int n )
{
    int *ccof;
    int m;
    int i;

    ccof = (int *)calloc( n+1, sizeof(int) );
    if( ccof == NULL ) return( NULL );

    ccof[0] = 1;
    ccof[1] = n;
    m = n/2;
    for( i=2; i <= m; ++i)
    {
        ccof[i] = (n-i+1)*ccof[i-1]/i;
        ccof[n-i]= ccof[i];
    }
    ccof[n-1] = n;
    ccof[n] = 1;

    return( ccof );
}

/**********************************************************************
  ccof_bwhp - calculates the c coefficients for a butterworth highpass
  filter. The coefficients are returned as an array of integers.

*/

int *ccof_bwhp( int n )
{
    int *ccof;
    int i;

    ccof = ccof_bwlp( n );
    if( ccof == NULL ) return( NULL );

    for( i = 0; i <= n; ++i)
        if( i % 2 ) ccof[i] = -ccof[i];

    return( ccof );
}

/**********************************************************************
  ccof_bwbp - calculates the c coefficients for a butterworth bandpass
  filter. The coefficients are returned as an array of integers.

*/

int *ccof_bwbp( int n )
{
    int *tcof;
    int *ccof;
    int i;

    ccof = (int *)calloc( 2*n+1, sizeof(int) );
    if( ccof == NULL ) return( NULL );

    tcof = ccof_bwhp(n);
    if( tcof == NULL ) return( NULL );

    for( i = 0; i < n; ++i)
    {
        ccof[2*i] = tcof[i];
        ccof[2*i+1] = 0.0;
    }
    ccof[2*n] = tcof[n];

    free( tcof );
    return( ccof );
}

/**********************************************************************
  ccof_bwbs - calculates the c coefficients for a butterworth bandstop
  filter. The coefficients are returned as an array of integers.

*/

long double  *ccof_bwbs( int n, long double  f1f, long double  f2f )
{
    long double  alpha;
    long double  *ccof;
    int i, j;

    alpha = -2.0 * cosl(M_PIl * (f2f + f1f) / 2.0) / cos(M_PIl * (f2f - f1f) / 2.0);

    ccof = (long double  *)calloc( 2*n+1, sizeof(long double ) );

    ccof[0] = 1.0;

    ccof[2] = 1.0;
    ccof[1] = alpha;

    for( i = 1; i < n; ++i )
    {
  ccof[2*i+2] += ccof[2*i];
  for( j = 2*i; j > 1; --j )
      ccof[j+1] += alpha * ccof[j] + ccof[j-1];

  ccof[2] += alpha * ccof[1] + 1.0;
  ccof[1] += alpha;
    }

    return( ccof );
}

/**********************************************************************
  sf_bwlp - calculates the scaling factor for a butterworth lowpass filter.
  The scaling factor is what the c coefficients must be multiplied by so
  that the filter response has a maximum value of 1.

*/

long double  sf_bwlp( int n, long double  fcf )
{
    int m, k;         // loop variables
    long double  omega;     // M_PIl * fcf
    long double  fomega;    // function of omega
    long double  parg0;     // zeroth pole angle
    long double  sf;        // scaling factor

    omega = M_PIl * fcf;
    fomega = sinl(omega);
    parg0 = M_PIl / (long double )(2*n);

    m = n / 2;
    sf = 1.0;
    for( k = 0; k < n/2; ++k )
        sf *= 1.0 + fomega * sinl((long double )(2*k+1)*parg0);

    fomega = sinl(omega / 2.0);

    if( n % 2 ) sf *= fomega + cosl(omega / 2.0);
    sf = pow( fomega, n ) / sf;

    return(sf);
}

/**********************************************************************
  sf_bwhp - calculates the scaling factor for a butterworth highpass filter.
  The scaling factor is what the c coefficients must be multiplied by so
  that the filter response has a maximum value of 1.

*/

long double  sf_bwhp( int n, long double  fcf )
{
    int m, k;         // loop variables
    long double  omega;     // M_PIl * fcf
    long double  fomega;    // function of omega
    long double  parg0;     // zeroth pole angle
    long double  sf;        // scaling factor

    omega = M_PIl * fcf;
    fomega = sinl(omega);
    parg0 = M_PIl / (long double )(2*n);

    m = n / 2;
    sf = 1.0;
    for( k = 0; k < n/2; ++k )
        sf *= 1.0 + fomega * sinl((long double )(2*k+1)*parg0);

    fomega = cosl(omega / 2.0);

    if( n % 2 ) sf *= fomega + sinl(omega / 2.0);
    sf = pow( fomega, n ) / sf;

    return(sf);
}

/**********************************************************************
  sf_bwbp - calculates the scaling factor for a butterworth bandpass filter.
  The scaling factor is what the c coefficients must be multiplied by so
  that the filter response has a maximum value of 1.

*/

long double  sf_bwbp( int n, long double  f1f, long double  f2f )
{
    int k;            // loop variables
    long double  ctt;       // cotangent of theta
    long double  sfr, sfi;  // real and imaginary parts of the scaling factor
    long double  parg;      // pole angle
    long double  sparg;     // sine of pole angle
    long double  cparg;     // cosine of pole angle
    long double  a, b, c;   // workspace variables

    ctt = 1.0 / tanl(M_PIl * (f2f - f1f) / 2.0);
    sfr = 1.0;
    sfi = 0.0;

    for( k = 0; k < n; ++k )
    {
  parg = M_PIl * (long double )(2*k+1)/(long double )(2*n);
  sparg = ctt + sinl(parg);
  cparg = cosl(parg);
  a = (sfr + sfi)*(sparg - cparg);
  b = sfr * sparg;
  c = -sfi * cparg;
  sfr = b - c;
  sfi = a - b - c;
    }

    return( 1.0 / sfr );
}

/**********************************************************************
  sf_bwbs - calculates the scaling factor for a butterworth bandstop filter.
  The scaling factor is what the c coefficients must be multiplied by so
  that the filter response has a maximum value of 1.

*/

long double  sf_bwbs( int n, long double  f1f, long double  f2f )
{
    int k;            // loop variables
    long double  tt;        // tangent of theta
    long double  sfr, sfi;  // real and imaginary parts of the scaling factor
    long double  parg;      // pole angle
    long double  sparg;     // sine of pole angle
    long double  cparg;     // cosine of pole angle
    long double  a, b, c;   // workspace variables

    tt = tanl(M_PIl * (f2f - f1f) / 2.0);
    sfr = 1.0;
    sfi = 0.0;

    for( k = 0; k < n; ++k )
    {
  parg = M_PIl * (long double )(2*k+1)/(long double )(2*n);
  sparg = tt + sinl(parg);
  cparg = cosl(parg);
  a = (sfr + sfi)*(sparg - cparg);
  b = sfr * sparg;
  c = -sfi * cparg;
  sfr = b - c;
  sfi = a - b - c;
    }

    return( 1.0 / sfr );
}