SoftFilters  0.1.0
Arduino framework and library of software data filters.
SoftFilters.h
1 #ifndef SOFTFILTERS_H
2 #define SOFTFILTERS_H
3 
4 #include "framework.h"
5 #include "types.h"
6 #include "OneEuro.h"
7 
32 template <typename VAL_T, typename TS_T=unsigned long, typename INTERNAL_T=double>
33 class DifferentialFilter : public BaseFilter<Reading<VAL_T, TS_T>, Reading<Differential<VAL_T>, TS_T> >
34 {
35 
36 public:
37  typedef Reading<VAL_T, TS_T> IN_T;
38  typedef Reading<Differential<VAL_T>, TS_T> OUT_T;
39  DifferentialFilter() : seen_first(false), seen_second(false) { }
40 
41 protected:
42 
46 #define ITN(x) ((INTERNAL_T)(x))
47 
54 #define INTERPOLATE(p0, v0, p1, p2, v2) (ITN(v2)*(ITN(p1)-ITN(p0))+ITN(v0)*(ITN(p2)-ITN(p1)))/(ITN(p2)-ITN(p0))
55  virtual bool update(void const * const input) override
56  {
57  in_ptr = (IN_T const * const) input;
58  if (!seen_first) {
59  // On the first observation, cache the value and timestamp
60  // in the internal storage for the output value.
61  this->out_val.value.position = in_ptr->value;
62  this->out_val.timestamp = in_ptr->timestamp;
63  seen_first = true;
64  return false;
65  }
66  else if (!seen_second) {
67  // Ignore the observation if the same timestamp,
68  // to avoid divide-by-zero.
69  if (this->out_val.timestamp != in_ptr->timestamp) {
70  // On the second observation, calculate the speed.
71  next_pos = in_ptr->value;
72  next_ts = in_ptr->timestamp;
73  aft_ts = ITN(next_ts - this->out_val.timestamp);
74  aft_spd = ITN(next_pos - this->out_val.value.position) / aft_ts;
75  aft_ts /= ITN(2);
76  aft_ts += ITN(this->out_val.timestamp);
77  seen_second = true;
78  }
79  return false;
80  }
81  else {
82  // Ignore the observation if the same timestamp,
83  // to avoid divide-by-zero.
84  if (next_ts == in_ptr->timestamp) {
85  return false;
86  }
87  // update internal data
88  bef_spd = aft_spd;
89  bef_ts = aft_ts;
90  this->out_val.value.position = next_pos;
91  this->out_val.timestamp = next_ts;
92  next_pos = in_ptr->value;
93  next_ts = in_ptr->timestamp;
94  // calculate the new speed
95  aft_ts = ITN(next_ts - this->out_val.timestamp);
96  aft_spd = ITN(next_pos - this->out_val.value.position) / aft_ts;
97  aft_ts /= ITN(2);
98  aft_ts += ITN(this->out_val.timestamp);
99  // interpolate the speed
100  this->out_val.value.speed = INTERPOLATE(bef_ts, bef_spd, this->out_val.timestamp, aft_ts, aft_spd);
101  // calculate the acceleration
102  this->out_val.value.acceleration = (aft_spd - bef_spd) / (aft_ts - bef_ts);
103  return true;
104  }
105  }
106 #undef INTERPOLATE
107 #undef ITN
108 
109 private:
110 
111  INTERNAL_T bef_spd, bef_ts;
112  INTERNAL_T aft_spd, aft_ts;
113  VAL_T next_pos;
114  TS_T next_ts;
115  bool seen_first;
116  bool seen_second;
117  IN_T const * in_ptr;
118 };
119 
131 #ifdef ARDUINO
132 template <typename VAL_T, typename TS_T=unsigned long, TS_T (*time_fn)()=micros>
133 #else
134 template <typename VAL_T, typename TS_T=unsigned long, TS_T (*time_fn)()>
135 #endif
136 class TimestampFilter : public BaseFilter<VAL_T, Reading<VAL_T, TS_T> >
137 {
138 public:
139  virtual bool update(void const * const input) override
140  {
141  this->out_val.value = *((VAL_T const * const) input);
142  this->out_val.timestamp = time_fn();
143  return true;
144  }
145 };
146 
156 template <typename IN_T, typename OUT_T>
157 class CachedFilter : public BaseFilter<IN_T, OUT_T>
158 {
159 public:
160  CachedFilter(size_t cap) : capacity(cap), size(0), end(0)
161  {
162  this->buffer = new IN_T[this->capacity];
163  }
164  ~CachedFilter() { delete[] this->buffer; }
165 protected:
166  virtual bool update(void const * const input) override
167  {
168  if (this->size < this->capacity) {
169  ++size;
170  this->buffer[end++] = *(IN_T const * const) input;
171  end %= capacity;
172  return refresh((IN_T const * const) input, NULL, this->out_val);
173  }
174  else {
175  cached_val = this->buffer[end];
176  this->buffer[end++] = *(IN_T const * const) input;
177  end %= capacity;
178  return refresh((IN_T const * const) input, &cached_val, this->out_val);
179  }
180  }
189  virtual bool refresh(IN_T const * const new_val, IN_T const * const old_val, OUT_T &output) = 0;
190  size_t get_capacity() { return capacity; }
191  size_t get_size() { return size; }
192 private:
193  size_t capacity;
194  size_t size;
195  IN_T *buffer;
196 
203  int end;
204  IN_T cached_val;
205 };
206 
217 template <typename IN_T, typename OUT_T, typename INTERNAL_T=double>
218 class MovingAverageFilter : public CachedFilter<IN_T, OUT_T>
219 {
220 public:
226  MovingAverageFilter(size_t w_sz) : CachedFilter<IN_T, OUT_T>(w_sz), sum(0) { }
227 protected:
228  INTERNAL_T get_sum() { return sum; }
229  virtual bool refresh(IN_T const * const new_val, IN_T const * const old_val, OUT_T &output) override
230  {
231  // Udpate the sum.
232  sum += (INTERNAL_T) *new_val - (old_val == NULL ? 0 : (INTERNAL_T) *old_val);
233  output = internal_result = sum / (INTERNAL_T) this->get_size();
234  return true;
235  }
236  INTERNAL_T internal_result;
237 private:
238  INTERNAL_T sum;
239 };
240 
244 template <typename IN_T, typename OUT_T, typename INTERNAL_T=double>
245 class MovingVarianceFilter : public MovingAverageFilter<IN_T, OUT_T, INTERNAL_T>
246 {
247 public:
248  MovingVarianceFilter(size_t w_sz) : MovingAverageFilter<IN_T, OUT_T, INTERNAL_T>(w_sz), squared_sum(0) { }
249 protected:
250  INTERNAL_T get_squared_sum() { return squared_sum; }
251  virtual bool refresh(IN_T const * const new_val, IN_T const * const old_val, OUT_T &output) override
252  {
253  // now the `internal_result` holds the mean value
254  MovingAverageFilter<IN_T, OUT_T>::refresh(new_val, old_val, output);
255  new_val_2 = *new_val;
256  new_val_2 *= new_val_2;
257  old_val_2 = old_val == NULL ? 0 : *old_val;
258  old_val_2 *= old_val_2;
259  squared_sum += new_val_2 - old_val_2;
260  output = this->internal_result = squared_sum / (INTERNAL_T) this->get_size() - this->internal_result * this->internal_result;
261  return true;
262  }
263 private:
264  INTERNAL_T new_val_2;
265  INTERNAL_T old_val_2;
266  INTERNAL_T squared_sum;
267 };
268 
282 template <typename IN_T, typename OUT_T, typename INTERNAL_T=double>
283 class WeightedUpdateFilter : public BaseFilter<IN_T, OUT_T>
284 {
285 public:
286  WeightedUpdateFilter(double w) : sensitivity(w), innertia(1 - w), seen_first(false) { }
287  virtual bool update(void const * const input) override
288  {
289  if (seen_first) {
290  this->out_val = internal_result = innertia * internal_result + sensitivity * (INTERNAL_T) *(IN_T const * const)input;
291  }
292  else {
293  this->out_val = internal_result = *(IN_T const * const)input;
294  }
295  return true;
296  }
297 protected:
298  INTERNAL_T internal_result;
299 private:
300  INTERNAL_T sensitivity; // sensitivity of the new observation, as a factor between 0 and 1
301  INTERNAL_T innertia; // 1 - sensitivity
302  bool seen_first;
303 };
304 
311 template <typename VAL_T>
312 class AdaptiveNormalizationFilter: public BaseFilter<VAL_T, double>
313 {
314 public:
315  AdaptiveNormalizationFilter() : seen_first(false) { }
316  virtual bool update(void const * const input) override
317  {
318  in_val = *(VAL_T const * const) input;
319  if (!seen_first) {
320  maximum = minimum = in_val;
321  this->out_val = 0;
322  seen_first = true;
323  }
324  else {
325  if (in_val >= maximum) {
326  maximum = in_val;
327  this->out_val = 1;
328  }
329  else if (in_val <= minimum) {
330  minimum = in_val;
331  this->out_val = 0;
332  }
333  else {
334  this->out_val = (in_val - minimum) / (double) (maximum - minimum);
335  }
336  }
337  return true;
338  }
339 private:
340  VAL_T maximum;
341  VAL_T minimum;
342  VAL_T in_val;
343  bool seen_first;
344 };
345 
349 template <typename VAL_T, typename TS_T>
350 class OneEuroFilter : public BaseFilter<Reading<VAL_T, TS_T>, Reading<VAL_T, TS_T>>
351 {
352 public:
353  OneEuroFilter(double _freq, VAL_T _mincutoff, VAL_T _beta, VAL_T _dcutoff)
354  : filter(one_euro_filter<VAL_T, TS_T>(_freq, _mincutoff, _beta, _dcutoff))
355  {}
356  VAL_T mincutoff()
357  {
358  return filter.mincutoff;
359  }
360  VAL_T mincutoff(VAL_T v)
361  {
362  filter.mincutoff = v;
363  return v;
364  }
365  VAL_T beta()
366  {
367  return filter.beta;
368  }
369  VAL_T beta(VAL_T v)
370  {
371  filter.beta = v;
372  return v;
373  }
374  VAL_T dcutoff()
375  {
376  return filter.dcutoff;
377  }
378  VAL_T dcutoff(VAL_T v)
379  {
380  filter.dcutoff = v;
381  return v;
382  }
383 protected:
384  virtual bool update(void const * const input) override
385  {
386  this->out_val.value = filter(
387  ((Reading<VAL_T, TS_T> const * const)input)->value,
388  ((Reading<VAL_T, TS_T> const * const)input)->timestamp);
389  this->out_val.timestamp = ((Reading<VAL_T, TS_T> const * const)input)->timestamp;
390  return true;
391  }
392 private:
394 };
395 
402 template <typename IN_T, typename OUT_T>
403 class LambdaFilter : public BaseFilter<IN_T, OUT_T>
404 {
405 public:
412  LambdaFilter(bool (*f)(IN_T const &, OUT_T &)): lambda(f) { }
413 protected:
418  virtual bool update(void const * const input) override
419  {
420  return lambda(*(IN_T const * const) input, this->out_val);
421  }
422 private:
423  bool (*lambda)(IN_T const &, OUT_T &);
424 };
425 
440 #ifdef ARDUINO
441 template <typename T, typename TS_T=unsigned long, TS_T (*time_fun)()=micros, TS_T TICKS_PER_SEC=(int)1e6>
442 #else
443 template <typename T, typename TS_T, TS_T (*time_fun)(), TS_T TICKS_PER_SEC>
444 #endif
446 {
447 public:
448  FlowRateFilter() : PassThroughFilter<T>(), total_count(0), first_ts(0), last_ts(0) { }
449  virtual bool update(void const * const input) override
450  {
452  if (++total_count == 1) {
453  first_ts = last_ts = time_fun();
454  }
455  else {
456  last_ts = time_fun();
457  }
458  return true;
459  }
469  double get_flow_rate()
470  {
471  if (first_ts == last_ts) {
472  // return an invalid value since the flow rate cannot be calculated
473  return -1;
474  }
475  return total_count / get_duration_in_seconds();
476  }
480  unsigned long get_count() { return total_count; }
485  TS_T get_duration_in_ticks() { return last_ts - first_ts; }
489  double get_duration_in_seconds() { return get_duration_in_ticks() / (double) TICKS_PER_SEC; }
490 private:
491  unsigned long total_count;
492  TS_T first_ts;
493  TS_T last_ts;
494 };
495 
496 #endif
INTERNAL_T new_val_2
square of the new data
Definition: SoftFilters.h:264
A moving variance filter.
Definition: SoftFilters.h:245
MovingAverageFilter(size_t w_sz)
Create a moving average filter with the specified window size.
Definition: SoftFilters.h:226
INTERNAL_T internal_result
representing the result in internal type in case the output type does not have the required precision...
Definition: SoftFilters.h:236
size_t capacity
The cache capacity, i.e., maximum data it can hold.
Definition: SoftFilters.h:193
A filter that outputs the average of a moving window.
Definition: SoftFilters.h:218
Filter framework.
virtual bool update(void const *const input) override
Internally update the filter output based on the given input.
Definition: SoftFilters.h:287
int end
The position in the internal buffer that points to the end of the cache.
Definition: SoftFilters.h:203
A flow rate filter measures the flow rate of incoming data.
Definition: SoftFilters.h:445
virtual bool refresh(IN_T const *const new_val, IN_T const *const old_val, OUT_T &output) override
Refresh the output value given the new value added to the cache and the old value removed from the ca...
Definition: SoftFilters.h:251
TS_T get_duration_in_ticks()
Definition: SoftFilters.h:485
INTERNAL_T old_val_2
square of the old data
Definition: SoftFilters.h:265
unsigned long get_count()
Definition: SoftFilters.h:480
virtual bool update(void const *const input) override
Internally update the filter output based on the given input.
Definition: SoftFilters.h:316
A filter with a data cache, which is suitable for output that depends on several previous input data...
Definition: SoftFilters.h:157
virtual bool update(void const *const input) override
Internally update the filter output based on the given input.
Definition: SoftFilters.h:166
IN_T * buffer
The internal buffer that holds the cached data.
Definition: SoftFilters.h:195
INTERNAL_T internal_result
representing the result in internal type in case the output type does not have the required precision...
Definition: SoftFilters.h:298
virtual bool update(void const *const input) override
In a lambda filter, the update function simply calls the client-supplied filter function.
Definition: SoftFilters.h:418
virtual bool update(void const *const input) override
Internally update the filter output based on the given input.
Definition: SoftFilters.h:384
virtual bool refresh(IN_T const *const new_val, IN_T const *const old_val, OUT_T &output) override
Refresh the output value given the new value added to the cache and the old value removed from the ca...
Definition: SoftFilters.h:229
A lambda filter that uses a client-supplied filter function.
Definition: SoftFilters.h:403
LambdaFilter(bool(*f)(IN_T const &, OUT_T &))
Create a lambda filter using the given function.
Definition: SoftFilters.h:412
A differential filter calculates the speed and acceleration from its raw scalar input.
Definition: SoftFilters.h:33
TS_T last_ts
timestamp of latest data
Definition: SoftFilters.h:493
A pass-through filter does nothing and is useful for derived classes to perform monitoring functional...
Definition: framework.h:167
The typed filter base class.
Definition: framework.h:112
unsigned long total_count
total data count
Definition: SoftFilters.h:491
Reading< Differential< VAL_T >, TS_T > out_val
Internally managed storage of the output value.
Definition: framework.h:157
An adaptive normalization filter.
Definition: SoftFilters.h:312
virtual bool update(void const *const input) override
Internally update the filter output based on the given input.
Definition: SoftFilters.h:139
INTERNAL_T squared_sum
squared sum
Definition: SoftFilters.h:266
INTERNAL_T sum
sum of the current cache content
Definition: SoftFilters.h:238
A filter that updates the output based on a weighted average between its previous output and the curr...
Definition: SoftFilters.h:283
double get_flow_rate()
Calculate the data rate in "frames per second".
Definition: SoftFilters.h:469
The 1-euro filter is based on the paper of the same name by Gery Casiez.
Definition: SoftFilters.h:350
A filter that adds timestamps to the input values.
Definition: SoftFilters.h:136
virtual bool update(void const *const input) override
Internally update the filter output based on the given input.
Definition: framework.h:173
A class that contains a <value, timestamp> tuple.
Definition: types.h:12
virtual bool update(void const *const input) override
Internally update the filter output based on the given input.
Definition: SoftFilters.h:449
IN_T cached_val
used to temporarily store the old data before overwritten by the new data
Definition: SoftFilters.h:204
TS_T first_ts
timestamp of first data
Definition: SoftFilters.h:492
double get_duration_in_seconds()
Definition: SoftFilters.h:489
size_t size
The current cache size, i.e., valid data.
Definition: SoftFilters.h:194
virtual bool update(void const *const input) override
Internally update the filter output based on the given input.
Definition: SoftFilters.h:55