IIR 滤波器的实现（C++）

IIR 滤波器的实现（C++）

最近在写的一个程序需要用到IIR滤波器，而且IIR滤波器的系数需要动态调整。因此就花了点时间研究IIR 滤波器的实现。

以前用到的IIR滤波器的参数都是事先确定好的，有个网站，只要把滤波器的参数特性输进去，直接就能生成需要的C代码。

http://www-users.cs.york.ac.uk/~fisher/mkfilter/trad.html

一直都偷懒直接用这个网站的结果，所以手上也没积累什么有用的代码。这次就需要自己从头做起。

我面临的问题有两个：

1. 根据滤波器的参数（滤波器的类型、截止频率、滤波器的阶数等），计算出滤波器对应的差分方程的系数。

2. 利用得到的差分方程的系数构造一个可以工作的滤波器。

其中第一个问题，对于不同类型的滤波器，比如Butterworth型、Bessel型等，滤波器系数的计算方法都不同。这部分工作我还没做完全，等我把常见的几种滤波器类型的系数计算方法都实现后再来写一篇文章。

这里就先写写第二个问题。IIR 滤波器对应的差分方程为：

相应的系统函数为：

这里默认a[0] = 1。实际上，总可以通过调整a[k] 与 b[k] 的值使得a[0] = 1，所以这个条件时总能满足的。

按照奥本海默写的《离散时间信号处理》上面的介绍，IIR 滤波器有两种基本的实现形式，分别成为直接I型和直接II型。我分别写了两个类，实现这两种形式。

直接I型

class IIR_I
{
private:
    double *m_pNum;
    double *m_pDen;
    double *m_px;
    double *m_py;
    int m_num_order;
    int m_den_order;
public:
    IIR_I();
    void reset();
    void setPara(double num[], int num_order, double den[], int den_order);
    void resp(double data_in[], int m, double data_out[], int n);
    double filter(double data);
    void filter(double data[], int len);
    void filter(double data_in[], double data_out[], int len);
};

其中 m_px 存放x[n-k] 的值（m_px[0]存放x[n-0]、 m_px[1] 存放x[n-1]，以此类推），m_py存放y[n-k] 的值（m_py[0]存放y[n-0]、 m_py[1] 存放y[n-1]，以此类推）。

三个filter函数用来做实际的滤波操作。在这之前，需要用setPara函数初始化滤波器的系数。

下面是实现代码：

/** \brief 将滤波器的内部状态清零，滤波器的系数保留
 * \return
 */
void IIR_I::reset()
{
    for(int i = 0; i <= m_num_order; i++)
    {
        m_pNum[i] = 0.0;
    }
    for(int i = 0; i <= m_den_order; i++)
    {
        m_pDen[i] = 0.0;
    }
}
IIR_I::IIR_I()
{
    m_pNum = NULL;
    m_pDen = NULL;
    m_px = NULL;
    m_py = NULL;
    m_num_order = -1;
    m_den_order = -1;
};
/** \brief
 *
 * \param num 分子多项式的系数，升序排列,num[0] 为常数项
 * \param m 分子多项式的阶数
 * \param den 分母多项式的系数，升序排列,den[0] 为常数项
 * \param m 分母多项式的阶数
 * \return
 */
void IIR_I::setPara(double num[], int num_order, double den[], int den_order)
{
    delete[] m_pNum;
    delete[] m_pDen;
    delete[] m_px;
    delete[] m_py;
    m_pNum = new double[num_order + 1];
    m_pDen = new double[den_order + 1];
    m_num_order = num_order;
    m_den_order = den_order;
    m_px = new double[num_order + 1];
    m_py = new double[den_order + 1];
    for(int i = 0; i <= m_num_order; i++)
    {
        m_pNum[i] = num[i];
        m_px[i] = 0.0;
    }
    for(int i = 0; i <= m_den_order; i++)
    {
        m_pDen[i] = den[i];
        m_py[i] = 0.0;
    }
}

/** \brief 滤波函数，采用直接I型结构
 *
 * \param data 传入输入数据
 * \return 滤波后的结果
 */
double IIR_I::filter(double data)
{
    m_py[0] = 0.0; // 存放滤波后的结果
    m_px[0] = data;
    for(int i = 0; i <= m_num_order; i++)
    {
        m_py[0] = m_py[0] + m_pNum[i] * m_px[i];
    }
    for(int i = 1; i <= m_den_order; i++)
    {
        m_py[0] = m_py[0] - m_pDen[i] * m_py[i];
    }
    for(int i = m_num_order; i >= 1; i--)
    {
        m_px[i] = m_px[i-1];
    }
    for(int i = m_den_order; i >= 1; i--)
    {
        m_py[i] = m_py[i-1];
    }
    return m_py[0];
}


/** \brief 滤波函数，采用直接I型结构
 *
 * \param data[] 传入输入数据，返回时给出滤波后的结果
 * \param len data[] 数组的长度
 * \return
 */
void IIR_I::filter(double data[], int len)
{
    int i, k;
    for(k = 0; k < len; k++)
    {
        m_px[0] = data[k];
        data[k] = 0.0;
        for(i = 0; i <= m_num_order; i++)
        {
            data[k] = data[k] + m_pNum[i] * m_px[i];
        }
        for(i = 1; i <= m_den_order; i++)
        {
            data[k] = data[k] - m_pDen[i] * m_py[i];
        }
        // we get the y value now
        m_py[0] = data[k];
        for(i = m_num_order; i >= 1; i--)
        {
            m_px[i] = m_px[i-1];
        }
        for(i = m_den_order; i >= 1; i--)
        {
            m_py[i] = m_py[i-1];
        }
    }
}
/** \brief 滤波函数，采用直接I型结构
 *
 * \param data_in[] 输入数据
 * \param data_out[] 保存滤波后的数据
 * \param len 数组的长度
 * \return
 */
void IIR_I::filter(double data_in[], double data_out[], int len)
{
    int i, k;
    for(k = 0; k < len; k++)
    {
        m_px[0] = data_in[k];
        m_py[0] = 0.0;
        for(i = 0; i <= m_num_order; i++)
        {
            m_py[0] = m_py[0] + m_pNum[i] * m_px[i];
        }
        for(i = 1; i <= m_den_order; i++)
        {
            m_py[0] = m_py[0] - m_pDen[i] * m_py[i];
        }
        for(i = m_num_order; i >= 1; i--)
        {
            m_px[i] = m_px[i-1];
        }
        for(i = m_den_order; i >= 1; i--)
        {
            m_py[i] = m_py[i-1];
        }
        data_out[k] = m_py[0];
    }
}

除此之外，还提供了个resp函数，这个函数可以计算滤波器的时域响应。并且不要求data_in与data_out 的数组长度相同。默认data_in[0] 之前的数据全为0，data_in[M-1]之后的数字全部为data_in[M-1]。因此，用这个函数计算阶跃响应输入数据只需要提供一个数据点就行了。并且这个函数不改变滤波器的内部状态。

/** \brief 计算 IIR 滤波器的时域响应，不影响滤波器的内部状态
 * \param data_in 为滤波器的输入，0 时刻之前的输入默认为 0，data_in[M] 及之后的输入默认为data_in[M-1]
 * \param data_out 滤波器的输出
 * \param M 输入数据的长度
 * \param N 输出数据的长度
 * \return
 */
void IIR_I::resp(double data_in[], int M, double data_out[], int N)
{
    int i, k, il;
    for(k = 0; k < N; k++)
    {
        data_out[k] = 0.0;
        for(i = 0; i <= m_num_order; i++)
        {
            if( k - i >= 0)
            {
                il = ((k - i) < M) ? (k - i) : (M - 1);
                data_out[k] = data_out[k] + m_pNum[i] * data_in[il];
            }
        }
        for(i = 1; i <= m_den_order; i++)
        {
            if( k - i >= 0)
            {
                data_out[k] = data_out[k] - m_pDen[i] * data_out[k - i];
            }
        }
    }
}

直接II型

/**< IIR 滤波器直接II型实现 */
class IIR_II
{
public:
    IIR_II();
    void reset();
    void setPara(double num[], int num_order, double den[], int den_order);
    void resp(double data_in[], int m, double data_out[], int n);
    double filter(double data);
    void filter(double data[], int len);
    void filter(double data_in[], double data_out[], int len);
protected:
    private:
    double *m_pNum;
    double *m_pDen;
    double *m_pW;
    int m_num_order;
    int m_den_order;
    int m_N;
};

class IIR_BODE
{
private:
    double *m_pNum;
    double *m_pDen;
    int m_num_order;
    int m_den_order;
    complex<double> poly_val(double p[], int order, double omega);
public:
    IIR_BODE();
    void setPara(double num[], int num_order, double den[], int den_order);
    complex<double> bode(double omega);
    void bode(double omega[], int n, complex<double> resp[]);
};

直接II型实现中所需的存储单元少了很多。另外，这两种实现的外部接口是完全相同的。

IIR_II::IIR_II()
{
    //ctor
    m_pNum = NULL;
    m_pDen = NULL;
    m_pW = NULL;
    m_num_order = -1;
    m_den_order = -1;
    m_N = 0;
};

/** \brief 将滤波器的内部状态清零，滤波器的系数保留
 * \return
 */
void IIR_II::reset()
{
    for(int i = 0; i < m_N; i++)
    {
        m_pW[i] = 0.0;
    }
}
/** \brief
 *
 * \param num 分子多项式的系数，升序排列,num[0] 为常数项
 * \param m 分子多项式的阶数
 * \param den 分母多项式的系数，升序排列,den[0] 为常数项
 * \param m 分母多项式的阶数
 * \return
 */
void IIR_II::setPara(double num[], int num_order, double den[], int den_order)
{
    delete[] m_pNum;
    delete[] m_pDen;
    delete[] m_pW;
    m_num_order = num_order;
    m_den_order = den_order;
    m_N = max(num_order, den_order) + 1;
    m_pNum = new double[m_N];
    m_pDen = new double[m_N];
    m_pW = new double[m_N];
    for(int i = 0; i < m_N; i++)
    {
        m_pNum[i] = 0.0;
        m_pDen[i] = 0.0;
        m_pW[i] = 0.0;
    }
    for(int i = 0; i <= num_order; i++)
    {
         m_pNum[i] = num[i];
    }
    for(int i = 0; i <= den_order; i++)
    {
        m_pDen[i] = den[i];
    }
}
/** \brief 计算 IIR 滤波器的时域响应，不影响滤波器的内部状态
 * \param data_in 为滤波器的输入，0 时刻之前的输入默认为 0，data_in[M] 及之后的输入默认为data_in[M-1]
 * \param data_out 滤波器的输出
 * \param M 输入数据的长度
 * \param N 输出数据的长度
 * \return
 */
void IIR_II::resp(double data_in[], int M, double data_out[], int N)
{
    int i, k, il;
    for(k = 0; k < N; k++)
    {
        data_out[k] = 0.0;
        for(i = 0; i <= m_num_order; i++)
        {
            if( k - i >= 0)
            {
                il = ((k - i) < M) ? (k - i) : (M - 1);
                data_out[k] = data_out[k] + m_pNum[i] * data_in[il];
            }
        }
        for(i = 1; i <= m_den_order; i++)
        {
            if( k - i >= 0)
            {
                data_out[k] = data_out[k] - m_pDen[i] * data_out[k - i];
            }
        }
    }
}
/** \brief 滤波函数，采用直接II型结构
 *
 * \param data 输入数据
 * \return 滤波后的结果
 */
double IIR_II::filter(double data)
{
    m_pW[0] = data;
    for(int i = 1; i <= m_den_order; i++) // 先更新 w[n] 的状态
    {
        m_pW[0] = m_pW[0] - m_pDen[i] * m_pW[i];
    }
    data = 0.0;
    for(int i = 0; i <= m_num_order; i++)
    {
        data = data + m_pNum[i] * m_pW[i];
    }
    for(int i = m_N - 1; i >= 1; i--)
    {
        m_pW[i] = m_pW[i-1];
    }
    return data;
}
/** \brief 滤波函数，采用直接II型结构
 *
 * \param data[] 传入输入数据，返回时给出滤波后的结果
 * \param len data[] 数组的长度
 * \return
 */
void IIR_II::filter(double data[], int len)
{
    int i, k;
    for(k = 0; k < len; k++)
    {
        m_pW[0] = data[k];
        for(i = 1; i <= m_den_order; i++) // 先更新 w[n] 的状态
        {
            m_pW[0] = m_pW[0] - m_pDen[i] * m_pW[i];
        }
        data[k] = 0.0;
        for(i = 0; i <= m_num_order; i++)
        {
            data[k] = data[k] + m_pNum[i] * m_pW[i];
        }

        for(i = m_N - 1; i >= 1; i--)
        {
            m_pW[i] = m_pW[i-1];
        }
    }
}
/** \brief 滤波函数，采用直接II型结构
 *
 * \param data_in[] 输入数据
 * \param data_out[] 保存滤波后的数据
 * \param len 数组的长度
 * \return
 */
void IIR_II::filter(double data_in[], double data_out[], int len)
{
    int i, k;
    for(k = 0; k < len; k++)
    {
        m_pW[0] = data_in[k];
        for(i = 1; i <= m_den_order; i++) // 先更新 w[n] 的状态
        {
            m_pW[0] = m_pW[0] - m_pDen[i] * m_pW[i];
        }
        data_out[k] = 0.0;
        for(i = 0; i <= m_num_order; i++)
        {
            data_out[k] = data_out[k] + m_pNum[i] * m_pW[i];
        }

        for(i = m_N - 1; i >= 1; i--)
        {
            m_pW[i] = m_pW[i-1];
        }
    }
}

测试

下面是几个测试例子，首先计算一个4阶切比雪夫低通滤波器的阶跃响应。

void resp_test(void)
{
    double b[5] = {0.001836, 0.007344, 0.011016, 0.007344, 0.001836};
    double a[5] = {1.0, -3.0544, 3.8291, -2.2925, 0.55075};
    double x[2] = {1.0, 1.0};
    double y[100];

    IIR_II filter;
    filter.setPara(b, 4, a, 4);
    filter.resp(x, 2, y, 100);

    for(int i= 0; i< 100; i++)
    {
        cout << y[i] << endl;
    }
}

得到的结果如下：

同样是这个滤波器，计算输入信号为 delta 函数时的结果。

void filter_test(void)
{
    double b[5] = {0.001836, 0.007344, 0.011016, 0.007344, 0.001836};
    double a[5] = {1.0, -3.0544, 3.8291, -2.2925, 0.55075};
    double x[100], y[100];
    int len = 100;
    IIR_I filter;
    //IIR_II filter;
    filter.setPara(b, 4, a, 4);

    for (int i = 0; i < len; i++)
    {
        x[i] = 0.0;
        y[i] = 0.0;
    }
    x[0] = 1.0;
    filter.filter(x, y, 100);
    filter.reset();
    filter.filter(x, 100);

    for (int i = 0; i < len; i++)
    {
        cout << x[i] <<", " << y[i]<<  endl;
    }
}

得到的结果如下：