低通滤波器

本个低通滤波器，就是前面的数据影响后面的数据，达到均匀滤波的效果。

参数设置为1000/500 = 2；时，跟随采样信号，意思就是     没有滤波   效果

设置为 1000/10 = 100时，采样了100个点还未达到预期采样效果，延时非常长了。

参考1000/30 = 33.33,滤波效果可以，延时也不太高。

1000/50 - 1000/15 = 20 - 66 之间选择。说白了值小了没有滤波效果=====值大了滤波稳定，如果达到滤波效果需要的时间长。比如0-5000会计算很多个点后才能达到目标值。

######################################################################################

######################################################################################

lpfilter.cpp

######################################################################################

#include "lpfilter.h"
#include <math.h>
#include <stdio.h>

#define M_PI_F 3.14159265358979323846f


void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
{
    _cutoff_freq = cutoff_freq;
    if (_cutoff_freq <= 0.0f) {
        // no filtering
        return;
    }
    float fr = sample_freq/_cutoff_freq;
    float ohm = tanf(M_PI_F/fr);
    float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
    _b0 = ohm*ohm/c;
    _b1 = 2.0f*_b0;
    _b2 = _b0;
    _a1 = 2.0f*(ohm*ohm-1.0f)/c;
    _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
}

float LowPassFilter2p::apply(float sample)
{
    if (_cutoff_freq <= 0.0f) {
        // no filtering
        return sample;
    }

    // do the filtering
    float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
    if (!isfinite(delay_element_0)) {
        // don't allow bad values to propagate via the filter
        delay_element_0 = sample;
    }
    float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
    
    _delay_element_2 = _delay_element_1;
    _delay_element_1 = delay_element_0;

    // return the value.  Should be no need to check limits
    return output;
}

float LowPassFilter2p::reset(float sample) {
    float dval = sample / (_b0 + _b1 + _b2);
    _delay_element_1 = dval;
    _delay_element_2 = dval;
    return apply(sample);
}

######################################################################################

######################################################################################

lpfilter.h

######################################################################################

class LowPassFilter2p
{
public:
    // constructor
    LowPassFilter2p(float sample_freq, float cutoff_freq) :
        _cutoff_freq(cutoff_freq),
        _a1(0.0f),
        _a2(0.0f),
        _b0(0.0f),
        _b1(0.0f),
        _b2(0.0f),
        _delay_element_1(0.0f),
        _delay_element_2(0.0f)
    {
        // set initial parameters
        set_cutoff_frequency(sample_freq, cutoff_freq);
    }

    /**
     * Change filter parameters
     */
    void set_cutoff_frequency(float sample_freq, float cutoff_freq);

    /**
     * Add a new raw value to the filter
     *
     * @return retrieve the filtered result
     */
    float apply(float sample);

    /**
     * Return the cutoff frequency
     */
    float get_cutoff_freq(void) const {
        return _cutoff_freq;
    }

    /**
     * Reset the filter state to this value
     */
    float reset(float sample);

private:
    float           _cutoff_freq;
    float           _a1;
    float           _a2;
    float           _b0;
    float           _b1;
    float           _b2;
    float           _delay_element_1;        // buffered sample -1
    float           _delay_element_2;        // buffered sample -2
};

######################################################################################

######################################################################################

main.cpp

######################################################################################

#include "lpfilter.h"
#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#define SAMPLE_RATE            1000
#define CUTOFF_RATE    30


int main(int argc, char *argv[])
{
    srandom( time(NULL) );  

    float sample_f = SAMPLE_RATE;
    float cutoff_f = CUTOFF_RATE;
    if(argc > 2)
    {
        sample_f = (float)atoi(argv[1]);
        cutoff_f = (float)atoi(argv[2]);
        printf("input ok!sf = %.5f, cf=%.5f\n", sample_f, cutoff_f);
    }
    LowPassFilter2p myf(sample_f, cutoff_f);
    int sample = 5000;
    int noise = 0;
    int i;
    for(i = 0; i < 100; i++)
    {
        int data = sample + random()%200;
        printf("num=%5d: in=%5d,out=%8.3f\n", i, data, myf.apply((float)data));
    }
    return 1;
}

#include "lpfilter.h"
#include 
   
   
    
    
#include 
    
    
     
     

#define M_PI_F 3.14159265358979323846f


void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
{
    _cutoff_freq = cutoff_freq;
    if (_cutoff_freq <= 0.0f) {
        // no filtering
        return;
    }
    float fr = sample_freq/_cutoff_freq;
    float ohm = tanf(M_PI_F/fr);
    float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
    _b0 = ohm*ohm/c;
    _b1 = 2.0f*_b0;
    _b2 = _b0;
    _a1 = 2.0f*(ohm*ohm-1.0f)/c;
    _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
}

float LowPassFilter2p::apply(float sample)
{
    if (_cutoff_freq <= 0.0f) {
        // no filtering
        return sample;
    }

    // do the filtering
    float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
    if (!isfinite(delay_element_0)) {
        // don't allow bad values to propagate via the filter
        delay_element_0 = sample;
    }
    float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
    
    _delay_element_2 = _delay_element_1;
    _delay_element_1 = delay_element_0;

    // return the value.  Should be no need to check limits
    return output;
}

float LowPassFilter2p::reset(float sample) {
	float dval = sample / (_b0 + _b1 + _b2);
    _delay_element_1 = dval;
    _delay_element_2 = dval;
    return apply(sample);
}
class LowPassFilter2p
{
public:
    // constructor
    LowPassFilter2p(float sample_freq, float cutoff_freq) : 
        _cutoff_freq(cutoff_freq),
        _a1(0.0f),
        _a2(0.0f),
        _b0(0.0f),
        _b1(0.0f),
        _b2(0.0f),
        _delay_element_1(0.0f),
        _delay_element_2(0.0f)
    {
        // set initial parameters
        set_cutoff_frequency(sample_freq, cutoff_freq);
    }

    /**
     * Change filter parameters
     */
    void set_cutoff_frequency(float sample_freq, float cutoff_freq);

    /**
     * Add a new raw value to the filter
     *
     * @return retrieve the filtered result
     */
    float apply(float sample);

    /**
     * Return the cutoff frequency
     */
    float get_cutoff_freq(void) const {
        return _cutoff_freq;
    }

    /**
     * Reset the filter state to this value
     */
    float reset(float sample);

private:
    float           _cutoff_freq; 
    float           _a1;
    float           _a2;
    float           _b0;
    float           _b1;
    float           _b2;
    float           _delay_element_1;        // buffered sample -1
    float           _delay_element_2;        // buffered sample -2
};
#include "lpfilter.h"
#include 
     
     
      
      
#include 
      
      
       
       
#include 
       
       
         #define SAMPLE_RATE 1000 #define CUTOFF_RATE 30 int main(int argc, char *argv[]) { srandom( time(NULL) ); float sample_f = SAMPLE_RATE; float cutoff_f = CUTOFF_RATE; if(argc > 2) { sample_f = (float)atoi(argv[1]); cutoff_f = (float)atoi(argv[2]); printf("input ok!sf = %.5f, cf=%.5f\n", sample_f, cutoff_f); } LowPassFilter2p myf(sample_f, cutoff_f); int sample = 5000; int noise = 0; int i; for(i = 0; i < 100; i++) { int data = sample + random()%200; printf("num=%5d: in=%5d,out=%8.3f\n", i, data, myf.apply((float)data)); } return 1; }