81 lines
1.7 KiB
C
81 lines
1.7 KiB
C
|
#ifndef ONEEURO_H
|
||
|
|
#define ONEEURO_H
|
||
|
|
/*
|
||
|
|
1-Euro Filter, template-compliant version
|
||
|
|
Jonathan Aceituno <join@oin.name>
|
||
|
|
|
||
|
|
25/04/14: fixed bug with last_time_ never updated on line 40
|
||
|
|
|
||
|
|
For details, see http://www.lifl.fr/~casiez/1euro
|
||
|
|
|
||
|
|
Updates:
|
||
|
|
|
||
|
|
- 23 May 2019 by Haimo Zhang <zh.hammer.dev@gmail.com>
|
||
|
|
- Included Arduino header
|
||
|
|
|
||
|
|
*/
|
||
|
|
|
||
|
|
#ifdef ARDUINO
|
||
|
|
/**
|
||
|
|
* cmath is not included in Arduino library, but we have the Arduino.h header
|
||
|
|
* which defines the abs macro.
|
||
|
|
*/
|
||
|
|
#include <Arduino.h>
|
||
|
|
#else
|
||
|
|
#include <cmath>
|
||
|
|
#endif
|
||
|
|
|
||
|
|
template <typename T = double>
|
||
|
|
struct low_pass_filter {
|
||
|
|
low_pass_filter() : hatxprev(0), xprev(0), hadprev(false) {}
|
||
|
|
T operator()(T x, T alpha) {
|
||
|
|
T hatx;
|
||
|
|
if(hadprev) {
|
||
|
|
hatx = alpha * x + (1-alpha) * hatxprev;
|
||
|
|
} else {
|
||
|
|
hatx = x;
|
||
|
|
hadprev = true;
|
||
|
|
}
|
||
|
|
hatxprev = hatx;
|
||
|
|
xprev = x;
|
||
|
|
return hatx;
|
||
|
|
}
|
||
|
|
T hatxprev;
|
||
|
|
T xprev;
|
||
|
|
bool hadprev;
|
||
|
|
};
|
||
|
|
|
||
|
|
template <typename T = double, typename timestamp_t = double>
|
||
|
|
struct one_euro_filter {
|
||
|
|
one_euro_filter(double _freq, T _mincutoff, T _beta, T _dcutoff) : freq(_freq), mincutoff(_mincutoff), beta(_beta), dcutoff(_dcutoff), last_time_(-1) {}
|
||
|
|
T operator()(T x, timestamp_t t = -1) {
|
||
|
|
T dx = 0;
|
||
|
|
|
||
|
|
if(last_time_ != -1 && t != -1 && t != last_time_) {
|
||
|
|
freq = 1.0 / (t - last_time_);
|
||
|
|
}
|
||
|
|
last_time_ = t;
|
||
|
|
|
||
|
|
if(xfilt_.hadprev)
|
||
|
|
dx = (x - xfilt_.xprev) * freq;
|
||
|
|
|
||
|
|
T edx = dxfilt_(dx, alpha(dcutoff));
|
||
|
|
T cutoff = mincutoff + beta * abs(static_cast<double>(edx));
|
||
|
|
return xfilt_(x, alpha(cutoff));
|
||
|
|
}
|
||
|
|
|
||
|
|
double freq;
|
||
|
|
T mincutoff, beta, dcutoff;
|
||
|
|
private:
|
||
|
|
T alpha(T cutoff) {
|
||
|
|
T tau = 1.0 / (2 * M_PI * cutoff);
|
||
|
|
T te = 1.0 / freq;
|
||
|
|
return 1.0 / (1.0 + tau / te);
|
||
|
|
}
|
||
|
|
|
||
|
|
timestamp_t last_time_;
|
||
|
|
low_pass_filter<T> xfilt_, dxfilt_;
|
||
|
|
};
|
||
|
|
|
||
|
|
#endif
|