32 template <
typename VAL_T,
typename TS_T=
unsigned long,
typename INTERNAL_T=
double>
46 #define ITN(x) ((INTERNAL_T)(x)) 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 57 in_ptr = (IN_T
const *
const) input;
61 this->
out_val.value.position = in_ptr->value;
62 this->
out_val.timestamp = in_ptr->timestamp;
66 else if (!seen_second) {
69 if (this->
out_val.timestamp != in_ptr->timestamp) {
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;
76 aft_ts += ITN(this->
out_val.timestamp);
84 if (next_ts == in_ptr->timestamp) {
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;
95 aft_ts = ITN(next_ts - this->
out_val.timestamp);
96 aft_spd = ITN(next_pos - this->
out_val.value.position) / aft_ts;
98 aft_ts += ITN(this->
out_val.timestamp);
100 this->
out_val.value.speed = INTERPOLATE(bef_ts, bef_spd, this->
out_val.timestamp, aft_ts, aft_spd);
102 this->
out_val.value.acceleration = (aft_spd - bef_spd) / (aft_ts - bef_ts);
111 INTERNAL_T bef_spd, bef_ts;
112 INTERNAL_T aft_spd, aft_ts;
132 template <
typename VAL_T,
typename TS_T=
unsigned long, TS_T (*time_fn)()=micros>
134 template <
typename VAL_T,
typename TS_T=
unsigned long, TS_T (*time_fn)()>
139 virtual bool update(
void const *
const input)
override 141 this->
out_val.value = *((VAL_T
const *
const) input);
142 this->
out_val.timestamp = time_fn();
156 template <
typename IN_T,
typename OUT_T>
160 CachedFilter(
size_t cap) : capacity(cap), size(0), end(0)
162 this->buffer =
new IN_T[this->capacity];
166 virtual bool update(
void const *
const input)
override 168 if (this->size < this->capacity) {
170 this->buffer[end++] = *(IN_T
const *
const) input;
172 return refresh((IN_T
const *
const) input, NULL, this->
out_val);
175 cached_val = this->buffer[end];
176 this->buffer[end++] = *(IN_T
const *
const) input;
178 return refresh((IN_T
const *
const) input, &cached_val, this->
out_val);
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; }
217 template <
typename IN_T,
typename OUT_T,
typename INTERNAL_T=
double>
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 232 sum += (INTERNAL_T) *new_val - (old_val == NULL ? 0 : (INTERNAL_T) *old_val);
233 output = internal_result = sum / (INTERNAL_T) this->get_size();
244 template <
typename IN_T,
typename OUT_T,
typename INTERNAL_T=
double>
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 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;
282 template <
typename IN_T,
typename OUT_T,
typename INTERNAL_T=
double>
287 virtual bool update(
void const *
const input)
override 290 this->
out_val = internal_result = innertia * internal_result + sensitivity * (INTERNAL_T) *(IN_T
const *
const)input;
293 this->
out_val = internal_result = *(IN_T
const *
const)input;
300 INTERNAL_T sensitivity;
311 template <
typename VAL_T>
316 virtual bool update(
void const *
const input)
override 318 in_val = *(VAL_T
const *
const) input;
320 maximum = minimum = in_val;
325 if (in_val >= maximum) {
329 else if (in_val <= minimum) {
334 this->
out_val = (in_val - minimum) / (
double) (maximum - minimum);
349 template <
typename VAL_T,
typename TS_T>
353 OneEuroFilter(
double _freq, VAL_T _mincutoff, VAL_T _beta, VAL_T _dcutoff)
358 return filter.mincutoff;
360 VAL_T mincutoff(VAL_T v)
362 filter.mincutoff = v;
376 return filter.dcutoff;
378 VAL_T dcutoff(VAL_T v)
384 virtual bool update(
void const *
const input)
override 402 template <
typename IN_T,
typename OUT_T>
418 virtual bool update(
void const *
const input)
override 420 return lambda(*(IN_T
const *
const) input, this->
out_val);
423 bool (*lambda)(IN_T
const &, OUT_T &);
441 template <
typename T,
typename TS_T=
unsigned long, TS_T (*time_fun)()=micros, TS_T TICKS_PER_SEC=(
int)1e6>
443 template <
typename T,
typename TS_T, TS_T (*time_fun)(), TS_T TICKS_PER_SEC>
449 virtual bool update(
void const *
const input)
override 452 if (++total_count == 1) {
453 first_ts = last_ts = time_fun();
456 last_ts = time_fun();
471 if (first_ts == last_ts) {
475 return total_count / get_duration_in_seconds();
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
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