GY-63_MS5611/libraries/RunningMedian/RunningMedian.cpp
2023-07-13 14:27:45 +02:00

375 lines
7.5 KiB
C++

//
// FILE: RunningMedian.cpp
// AUTHOR: Rob Tillaart
// VERSION: 0.3.8
// PURPOSE: RunningMedian library for Arduino
//
// HISTORY: see changelog.md
#include "RunningMedian.h"
RunningMedian::RunningMedian(const uint8_t size)
{
_size = size;
if (_size < MEDIAN_MIN_SIZE) _size = MEDIAN_MIN_SIZE;
#if !RUNNING_MEDIAN_USE_MALLOC
if (_size > MEDIAN_MAX_SIZE) _size = MEDIAN_MAX_SIZE;
#endif
#if RUNNING_MEDIAN_USE_MALLOC
_values = (float *) malloc(_size * sizeof(float));
_sortIdx = (uint8_t *) malloc(_size * sizeof(uint8_t));
#endif
clear();
}
RunningMedian::~RunningMedian()
{
#if RUNNING_MEDIAN_USE_MALLOC
free(_values);
free(_sortIdx);
#endif
}
// resets all internal counters
void RunningMedian::clear()
{
_count = 0;
_index = 0;
_sorted = false;
for (uint8_t i = 0; i < _size; i++)
{
_sortIdx[i] = i;
}
}
// adds a new value to the data-set
// or overwrites the oldest if full.
void RunningMedian::add(float value)
{
_values[_index++] = value;
if (_index >= _size) _index = 0; // wrap around
if (_count < _size) _count++;
_sorted = false;
}
float RunningMedian::getMedian()
{
if (_count == 0) return NAN;
if (_sorted == false) sort();
if (_count & 0x01) // is it odd sized?
{
return _values[_sortIdx[_count / 2]];
}
return (_values[_sortIdx[_count / 2]] + _values[_sortIdx[_count / 2 - 1]]) / 2;
}
float RunningMedian::getQuantile(float quantile)
{
if (_count == 0) return NAN;
if ((quantile < 0) || (quantile > 1)) return NAN;
if (_sorted == false) sort();
const float index = (_count - 1) * quantile;
const uint8_t lo = floor(index);
const uint8_t hi = ceil(index);
const float qs = _values[_sortIdx[lo]];
const float h = (index - lo);
return (1.0 - h) * qs + h * _values[_sortIdx[hi]];
}
float RunningMedian::getAverage()
{
if (_count == 0) return NAN;
float sum = 0;
for (uint8_t i = 0; i < _count; i++)
{
sum += _values[i];
}
return sum / _count;
}
float RunningMedian::getAverage(uint8_t nMedians)
{
if ((_count == 0) || (nMedians == 0)) return NAN;
// when filling the array for first time
if (_count < nMedians) nMedians = _count;
uint8_t start = ((_count - nMedians) / 2);
uint8_t stop = start + nMedians;
if (_sorted == false) sort();
float sum = 0;
for (uint8_t i = start; i < stop; i++)
{
sum += _values[_sortIdx[i]];
}
return sum / nMedians;
}
// nMedians is the spread, or the middle N
// this version compensated for bias #22
float RunningMedian::getMedianAverage(uint8_t nMedians)
{
// handle special cases.
if ((_count == 0) || (nMedians == 0)) return NAN;
if (_count == 1) return _values[0];
if (_count == 2) return (_values[0] + _values[1]) * 0.5;
// nMedians can not be larger than current nr of elements.
if (_count <= nMedians) return getAverage();
// _count is at least 3 from here
// Eliminate the bias when the nMedians would fall slightly
// to the left or right of the centre.
// If count and nMedians are not both odd or both even reduce
// the spread by 1 to make them the same.
// If nMedians becomes 0 correct this. to 2.
if ((_count & 0x01) != (nMedians & 0x01))
{
--nMedians;
// nmedians can not become 0
if (nMedians == 0) nMedians = 2;
}
uint8_t start = (_count - nMedians) / 2;
uint8_t stop = start + nMedians;
if (_sorted == false) sort();
float sum = 0;
for (uint8_t i = start; i < stop; i++)
{
sum += _values[_sortIdx[i]];
}
return sum / nMedians;
}
float RunningMedian::getElement(const uint8_t n)
{
if ((_count == 0) || (n >= _count)) return NAN;
uint8_t pos = _index + n;
if (pos >= _count) // faster than %
{
pos -= _count;
}
return _values[pos];
}
float RunningMedian::getSortedElement(const uint8_t n)
{
if ((_count == 0) || (n >= _count)) return NAN;
if (_sorted == false) sort();
return _values[_sortIdx[n]];
}
// n can be max <= half the (filled) size
float RunningMedian::predict(const uint8_t n)
{
uint8_t mid = _count / 2;
if ((_count == 0) || (n >= mid)) return NAN;
float med = getMedian(); // takes care of sorting !
if (_count & 0x01) // odd # elements
{
return max(med - _values[_sortIdx[mid - n]], _values[_sortIdx[mid + n]] - med);
}
// even # elements
float f1 = (_values[_sortIdx[mid - n]] + _values[_sortIdx[mid - n - 1]]) / 2;
float f2 = (_values[_sortIdx[mid + n]] + _values[_sortIdx[mid + n - 1]]) / 2;
return max(med - f1, f2 - med) / 2;
}
void RunningMedian::setSearchMode(uint8_t searchMode)
{
if (searchMode == 1) _searchMode = 1;
else _searchMode = 0;
}
uint8_t RunningMedian::getSearchMode()
{
return _searchMode;
}
////////////////////////////////////////////////////////////
//
// PRIVATE
//
// insertion sort - _searchMode = linear or binary.
void RunningMedian::sort()
{
uint16_t lo = 0;
uint16_t hi = 0;
uint16_t mi = 0;
uint16_t temp = 0;
for (uint16_t i = 1; i < _count; i++)
{
temp = _sortIdx[i];
float f = _values[temp];
// handle special case f is smaller than all elements first.
// only one compare needed, improves linear search too.
if (f <= _values[_sortIdx[0]])
{
hi = 0;
}
else
{
if (_searchMode == 0)
{
hi = i;
// find insertion point with linear search
while ((hi > 0) && (f < _values[_sortIdx[hi - 1]]))
{
hi--;
}
}
else if (_searchMode == 1)
{
// find insertion point with binary search
lo = 0;
hi = i;
// be aware there might be duplicates
while (hi - lo > 1)
{
mi = (lo + hi) / 2;
if (f < _values[_sortIdx[mi]])
{
hi = mi;
}
else
{
lo = mi;
}
}
}
}
// move elements to make space
uint16_t k = i;
while (k > hi)
{
_sortIdx[k] = _sortIdx[k - 1];
k--;
}
// insert at right spot.
_sortIdx[k] = temp;
}
_sorted = true;
// // verify sorted
// for (int i = 0; i < _count; i++)
// {
// if (i%5 == 0) Serial.println();
// Serial.print("\t");
// Serial.print(_values[_sortIdx[i]]);
// }
// Serial.println("\n");
}
/*
split version of pre-0.3.7 sort - bit faster
void RunningMedian::sort()
{
// insertSort
for (uint16_t i = 1; i < _count; i++)
{
uint16_t hi = i;
uint16_t temp = _sortIdx[hi];
float f = _values[temp];
while ((hi > 0) && (f < _values[_sortIdx[hi - 1]]))
{
hi--;
}
// move elements to make space
uint16_t k = i;
while (k > hi)
{
_sortIdx[k] = _sortIdx[k - 1];
k--;
}
// insert at right spot.
_sortIdx[k] = temp;
}
_sorted = true;
// // verify sorted
// for (int i = 0; i < _count; i++)
// {
// if (i%5 == 0) Serial.println();
// Serial.print("\t");
// Serial.print(_values[_sortIdx[i]]);
// }
// Serial.println("\n");
}
*/
/*
// straightforward insertion sort - PRE-0.3.7
void RunningMedian::sort()
{
for (uint16_t i = 1; i < _count; i++)
{
uint16_t z = i;
uint16_t temp = _sortIdx[z];
while ((z > 0) && (_values[temp] < _values[_sortIdx[z - 1]]))
{
_sortIdx[z] = _sortIdx[z - 1];
z--;
}
_sortIdx[z] = temp;
}
_sorted = true;
// // verify sorted
// for (int i = 0; i < _count; i++)
// {
// if (i%5 == 0) Serial.println();
// Serial.print("\t");
// Serial.print(_values[_sortIdx[i]]);
// }
// Serial.println("\n");
}
*/
// -- END OF FILE --