SoftFilters  0.1.0
Arduino framework and library of software data filters.
OneEuro.h
1 #ifndef ONEEURO_H
2 #define ONEEURO_H
3 /*
4 1-Euro Filter, template-compliant version
5 Jonathan Aceituno <join@oin.name>
6 
7 25/04/14: fixed bug with last_time_ never updated on line 40
8 
9 For details, see http://www.lifl.fr/~casiez/1euro
10 
11 Updates:
12 
13 - 23 May 2019 by Haimo Zhang <zh.hammer.dev@gmail.com>
14  - Included Arduino header
15 
16 */
17 
18 #ifdef ARDUINO
19 
23 #include <Arduino.h>
24 #else
25 #include <cmath>
26 #endif
27 
28 template <typename T = double>
30  low_pass_filter() : hatxprev(0), xprev(0), hadprev(false) {}
31  T operator()(T x, T alpha) {
32  T hatx;
33  if(hadprev) {
34  hatx = alpha * x + (1-alpha) * hatxprev;
35  } else {
36  hatx = x;
37  hadprev = true;
38  }
39  hatxprev = hatx;
40  xprev = x;
41  return hatx;
42  }
43  T hatxprev;
44  T xprev;
45  bool hadprev;
46 };
47 
48 template <typename T = double, typename timestamp_t = double>
50  one_euro_filter(double _freq, T _mincutoff, T _beta, T _dcutoff) : freq(_freq), mincutoff(_mincutoff), beta(_beta), dcutoff(_dcutoff), last_time_(-1) {}
51  T operator()(T x, timestamp_t t = -1) {
52  T dx = 0;
53 
54  if(last_time_ != -1 && t != -1 && t != last_time_) {
55  freq = 1.0 / (t - last_time_);
56  }
57  last_time_ = t;
58 
59  if(xfilt_.hadprev)
60  dx = (x - xfilt_.xprev) * freq;
61 
62  T edx = dxfilt_(dx, alpha(dcutoff));
63  T cutoff = mincutoff + beta * abs(static_cast<double>(edx));
64  return xfilt_(x, alpha(cutoff));
65  }
66 
67  double freq;
68  T mincutoff, beta, dcutoff;
69 private:
70  T alpha(T cutoff) {
71  T tau = 1.0 / (2 * M_PI * cutoff);
72  T te = 1.0 / freq;
73  return 1.0 / (1.0 + tau / te);
74  }
75 
76  timestamp_t last_time_;
77  low_pass_filter<T> xfilt_, dxfilt_;
78 };
79 
80 #endif
Definition: OneEuro.h:29
Definition: OneEuro.h:49