frank
284c6b11a6
added new helper class for 3x3 matrices and vec3 added magnetometer data added compass detection refactored pose-estimation (single class) refactored debug plots (move to own class) minor changes
75 lines
1.4 KiB
C++
75 lines
1.4 KiB
C++
#ifndef MOVINGMEDIANTS_H
|
|
#define MOVINGMEDIANTS_H
|
|
|
|
#include <vector>
|
|
#include <algorithm>
|
|
#include "../data/Timestamp.h"
|
|
|
|
template <typename T> class MovingMedianTS {
|
|
|
|
private:
|
|
|
|
/** timestamp -> value combination */
|
|
struct Entry {
|
|
Timestamp ts;
|
|
T value;
|
|
Entry(const Timestamp ts, const T& value) : ts(ts), value(value) {;}
|
|
};
|
|
|
|
/** the regional window to use */
|
|
Timestamp window;
|
|
|
|
/** the value history for the window-size */
|
|
std::vector<Entry> history;
|
|
|
|
|
|
|
|
|
|
public:
|
|
|
|
/** ctor with the window-size to use */
|
|
MovingMedianTS(const Timestamp window) : window(window) {
|
|
|
|
}
|
|
|
|
/** add a new value */
|
|
void add(const Timestamp ts, const T data) {
|
|
|
|
// append to history
|
|
history.push_back(Entry(ts, data));
|
|
|
|
// remove too-old history entries
|
|
const Timestamp oldest = ts - window;
|
|
while(history.front().ts < oldest) {
|
|
|
|
// remove from history
|
|
history.erase(history.begin());
|
|
|
|
}
|
|
|
|
}
|
|
|
|
/** get the current median */
|
|
T get() const {
|
|
|
|
const auto comp = [] (const Entry& a, const Entry& b) {
|
|
return a.value < b.value;
|
|
};
|
|
|
|
// create a sorted copy (slow, but works)
|
|
std::vector<Entry> copy = history;
|
|
std::sort(copy.begin(), copy.end(), comp);
|
|
|
|
// get the median
|
|
if (copy.size() % 2 != 0) {
|
|
return copy[(copy.size() + 0) / 2].value;
|
|
} else {
|
|
return (copy[copy.size() / 2 - 1].value + copy[copy.size() / 2 + 0].value) / 2;
|
|
}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // MOVINGMEDIANTS_H
|