一欧元滤波器(OneEuroFillter)C++实现

一欧元滤波器是一种通过可调抖动(tunable jitter)和延迟平衡(lag balance)来改善噪声信号质量的工具。它使用低通滤波器，但是截止频率根据速度变化：在低速时，低截止以延迟为代价减少抖动，但在高速时，增加截止值减少延迟而不是抖动。

从直觉上看，人们在缓慢移动时对抖动而不是延迟非常敏感，但随着移动速度的增加，人们对延迟而不是抖动变得非常敏感。

对于人类的运动，噪声通常在信号中形成高频，而实际的肢体运动频率较低。低通滤波器的设计目的是让这些所需的低频部分通过，同时衰减高于固定截止频率的高频信号。低通滤波器的阶数与它如何积极地衰减每个频率有关:一阶滤波器在频率每加倍时将信号振幅降低一半，而高阶变体以更大的速率降低信号振幅。

算法推导：
https://jaantollander.com/post/noise-filtering-using-one-euro-filter/#fn:1

简单来说：
有两个可以调节的参数，
最小截止频率fc↓，会减少抖动，但是增大延迟
参数β↑，会减小延迟，增大抖动

一个举例实现的网址：
https://gery.casiez.net/1euro/InteractiveDemo/

OneEuroFilter的各种语言实现搬运网址：
https://gery.casiez.net/1euro/

C++语言实现如下：

#include <iostream>
#include <stdexcept>
#include <cmath>
#include <ctime>

// -----------------------------------------------------------------
// Utilities

#define M_PI 3.1415926		 //原文档这里缺少了对M_PI的定义！

void randSeed(void) {
    srand(time(0));    		 //设置随机数种子
}

double unifRand(void) {
    return rand() / double(RAND_MAX);
}

typedef double TimeStamp; // in seconds

static const TimeStamp UndefinedTime = -1.0;

// -----------------------------------------------------------------

class LowPassFilter {           //低通滤波器    

    double y, a, s;
    bool initialized;

    void setAlpha(double alpha) {
        if (alpha <= 0.0 || alpha > 1.0)
            throw std::range_error("alpha should be in (0.0., 1.0]");
        a = alpha;
    }

public:

    LowPassFilter(double alpha, double initval = 0.0) {
        y = s = initval;        //初始时刻 y=s=0
        setAlpha(alpha);        //输入a=alpha
        initialized = false;
    }

    double filter(double value) {       //value为过滤的数据值
        double result;
        if (initialized)
            result = a * value + (1.0 - a) * s;     //s为上一时刻过滤后
        else {
            result = value;
            initialized = true;
        }
        y = value;
        s = result;
        return result;
    }

    double filterWithAlpha(double value, double alpha) {
        setAlpha(alpha);
        return filter(value);
    }

    bool hasLastRawValue(void) {
        return initialized;
    }

    double lastRawValue(void) {     //最后一个没有被过滤的数据
        return y;
    }

};

// -----------------------------------------------------------------

class OneEuroFilter {

    double freq;         //采样频率
    double mincutoff;    //最小截止频率
    double beta_;
    double dcutoff;
    LowPassFilter* x;
    LowPassFilter* dx;
    TimeStamp lasttime;

    double alpha(double cutoff) {                                        //计算alpha，输入截止频率fc
        double te = 1.0 / freq;
        double tau = 1.0 / (2 * M_PI * cutoff);
        return 1.0 / (1.0 + tau / te);
    }

    void setFrequency(double f) {                                        //设置采样频率，采样频率用来计算采样周期te
        if (f <= 0) throw std::range_error("freq should be >0");
        freq = f;
    }

    void setMinCutoff(double mc) {                                       //设置最小截止频率
        if (mc <= 0) throw std::range_error("mincutoff should be >0");  
        mincutoff = mc;
    }

    void setBeta(double b) {                                             //设置bata
        beta_ = b;
    }

    void setDerivateCutoff(double dc) {
        if (dc <= 0) throw std::range_error("dcutoff should be >0");
        dcutoff = dc;
    }

public:

    OneEuroFilter(double freq,                                                  //构造函数,会调用上述设置
        double mincutoff = 1.0, double beta_ = 0.0, double dcutoff = 1.0) {
        setFrequency(freq);
        setMinCutoff(mincutoff);
        setBeta(beta_);
        setDerivateCutoff(dcutoff);
        x = new LowPassFilter(alpha(mincutoff));
        dx = new LowPassFilter(alpha(dcutoff));
        lasttime = UndefinedTime;
    }

    double filter(double value, TimeStamp timestamp = UndefinedTime) {
        // update the sampling frequency based on timestamps
        if (lasttime != UndefinedTime && timestamp != UndefinedTime)
            freq = 1.0 / (timestamp - lasttime);
        lasttime = timestamp;
        // estimate the current variation per second    每秒估计当前的变化
        double dvalue = x->hasLastRawValue() ? (value - x->lastRawValue()) * freq : 0.0; // FIXME: 0.0 or value?
        double edvalue = dx->filterWithAlpha(dvalue, alpha(dcutoff));
        // use it to update the cutoff frequency
        double cutoff = mincutoff + beta_ * fabs(edvalue);
        // filter the given value
        return x->filterWithAlpha(value, alpha(cutoff));
    }

    ~OneEuroFilter(void) {
        delete x;
        delete dx;
    }

};

// -----------------------------------------------------------------

int main(int argc, char** argv) {

    randSeed();//设置随机数种子

    double duration = 1000000000;     // seconds

    double frequency = 120;     // Hz
    double mincutoff = 1.0;     // FIXME
    double beta = 1.0;          // FIXME
    double dcutoff = 1.0;       // this one should be ok

    std::cout << "#SRC OneEuroFilter.cc" << std::endl
        << "#CFG {'beta': " << beta << ", 'freq': " << frequency << ", 'dcutoff': " << dcutoff << ", 'mincutoff': " << mincutoff << "}" << std::endl
        << "#LOG timestamp, signal, noisy, filtered" << std::endl;

    OneEuroFilter f(frequency, mincutoff, beta, dcutoff);

    for (TimeStamp timestamp = 0.0; timestamp < duration; timestamp += 1.0 / frequency) {

        double signal = sin(timestamp);
        double noisy = signal + (unifRand() - 0.5) / 5.0;
        double filtered = f.filter(noisy, timestamp);			//这里实现的时候noisy为想传入的数据，timestamp = -1
        std::cout << timestamp << ", "
            << signal << ", "
            << noisy << ", "
            << filtered
            << std::endl;
    }

    system("pause");
    return 0;
}