[C++知识库]Matlab中filter.m和filtfilt.m函数C语言实现

一些基础知识

filter.m函数是依据z变换的一些知识进行的滤波方法。
filtfilt.m则还有另一个名字是零相位滤波，顾名思义，通过filtfilt函数滤波后的信号，幅值会发生变化，但相位不会改变。
如何使用的零相位滤波呢，首先对原始信号进行普通的iir滤波(filter)，滤波后将得到的输出信号进行翻转，再次使用filter进行滤波，最后再将得到的结果进行翻转，这样便实现了零相位滤波。

filter.m函数实现

static void filter(const float* b, const float* a, const float*zi, const float* signal, const int signal_len, float* filter_result)
{
    int i = 0;
    int j = 0;
    float state_signal[FILTER_LEN + 1] = { 0 };
    float state_result[FILTER_LEN] = { 0 };
    
    for (i = 0; i < signal_len; i++)
    {
        state_signal[0] = signal[i];
        if (i == 0)
        {
            state_result[0] = 0;
        }
        else
        {
            state_result[0] = iir_result[i - 1];
        }

        for (j = 0; j < FILTER_LEN; j++)
        {
            iir_result[i] += b[j] * state_signal[j];
        }
        for (j = 1; j < FILTER_LEN; j++)
        {
            iir_result[i] -= a[j] * state_result[j - 1];
        }
        //
        if (i < FILTER_LEN - 1)
        {
            iir_result[i] += zi[i];
        }

        for (j = FILTER_LEN - 1; j > -1; j--)
        {
            state_signal[j + 1] = state_signal[j];
        }
        for (j = FILTER_LEN - 2; j > -1; j--)
        {
            state_result[j + 1] = state_result[j];
        }
        filter_result[i] /= a[0];
    }
}

filtfilt.m函数实现

static void zero_phase_iir(const float* b, const float* a, const float* zi, const float* signal, const int signal_len, float* iir_result)
{
    int i = 0;
    int add_num = 3 * (FILTER_LEN - 1);//matlab延拓长度是3 * (FILTER_LEN - 1)
    float tmp_signal[SIG_NUM + 2 * 3 * (FILTER_LEN - 1)] = {0};
    float tmp_result[SIG_NUM + 2 * 3 * (FILTER_LEN - 1)] = {0};//初始化缓存，用于iir滤波
    float zi_temp[FILTER_LEN - 1] = { 0 };
    //进行数据延拓
    for (i = 0; i < signal_len + add_num * 2; i++)
    {
        if (i < add_num)
        {
            tmp_signal[i] = 2 * signal[0] - signal[add_num - i]; //前延拓
        }
        else if (i < signal_len + add_num)
        {
            tmp_signal[i] = signal[i - add_num]; //中间数据不变
        }
        else
        {
            tmp_signal[i] = 2 * signal[signal_len - 1] - signal[signal_len - 2 - (i - signal_len - add_num)]; //后延拓
        }
    }

    //正向滤波
    for (i = 0; i < FILTER_LEN - 1; i++)
    {
        zi_temp[i] = zi[i] * tmp_signal[0];//计算初始状态
    }
    iir(b, a, zi_temp, tmp_signal, signal_len + add_num * 2, tmp_result);

    //数据翻转
    for (i = 0; i < signal_len + add_num * 2; i++)
    {
        tmp_signal[i] = tmp_result[signal_len + add_num * 2 - 1 - i];
    }
    //初始化缓存
    for (i = 0; i < signal_len + add_num * 2; i++)
    {
        tmp_result[i] = 0;
    }
    //逆向滤波
    for (i = 0; i < FILTER_LEN - 1; i++)
    {
        zi_temp[i] = zi[i] * tmp_signal[0];
    }
    iir(b, a, zi_temp, tmp_signal, signal_len + add_num * 2, tmp_result);
    
    //数据翻转
    for (i = 0; i < signal_len + add_num * 2; i++)
    {
        tmp_signal[i] = tmp_result[signal_len + add_num * 2 - 1 - i];
    }
    //数据截取
    for (i = 0; i < signal_len; i++)
    {
        iir_result[i] = tmp_signal[i + add_num];
    }

}