filter_util.c

Go to the documentation of this file.

#include "filter_util.h"
#include <assert.h>
#include <math.h>
#include <string.h>

#define MAX_Q       7

void recursive_filter_impulse(num *h, long N,
    const num *b, int p, const num *a, int q)
{
    long m, n;
    
    assert(h && N > 0 && b && p >= 0 && a && q > 0);
    
    for (n = 0; n < N; ++n)
    {
        h[n] = (n <= p) ? b[n] : 0;
        
        for (m = 1; m <= q && m <= n; ++m)
            h[n] -= a[m] * h[n - m];
    }
    
    return;
}

void init_recursive_filter(num *dest, const num *src, long N, long stride,
    const num *b, int p, const num *a, int q,
    num sum, num tol, long max_iter)
{
    num h[MAX_Q + 1];
    long n;
    int m;
    
    assert(dest && src && N > 0 && stride != 0
        && b && p >= 0 && a && 0 < q && q <= MAX_Q
        && tol > 0 && max_iter > 0);
    
    /* Compute the first q taps of the impulse response, h_0, ..., h_{q-1} */
    recursive_filter_impulse(h, q, b, p, a, q);
    
    /* Compute dest_m = sum_{n=1}^m h_{m-n} src_n, m = 0, ..., q-1 */
    for (m = 0; m < q; ++m)
        for (dest[m] = 0, n = 1; n <= m; ++n)
            dest[m] += h[m - n] * src[stride * extension(N, n)];

    for (n = 0; n < max_iter; ++n)
    {
        num cur = src[stride * extension(N, -n)];
        
        /* dest_m = dest_m + h_{n+m} src_{-n} */
        for (m = 0; m < q; ++m)
            dest[m] += h[m] * cur;
        
        sum -= fabs(h[0]);
        
        if (sum <= tol)
            break;
        
        /* Compute the next impulse response tap, h_{n+q} */
        h[q] = (n + q <= p) ? b[n + q] : 0;
        
        for (m = 1; m <= q; ++m)
            h[q] -= a[m] * h[q - m];
        
        /* Shift the h array for the next iteration */
        for (m = 0; m < q; ++m)
            h[m] = h[m + 1];
    }
    
    return;
}