Skip to content

Commit

Permalink
add
Browse files Browse the repository at this point in the history
  • Loading branch information
@AlexGyver
AlexGyver committed Jul 27, 2019
1 parent 8387a31 commit fe4cfe8
Show file tree
Hide file tree
Showing 7 changed files with 261 additions and 134 deletions.
283 changes: 157 additions & 126 deletions GyverFilters/GyverFilters.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,167 +57,198 @@ uint16_t GMedian3::filtered(uint16_t value) {
int c = buffer[2];
int middle;

if ((a <= b) && (a <= c)) {
middle = (b <= c) ? b : c;
}
else {
if ((b <= a) && (b <= c)) {
middle = (a <= c) ? a : c;
}
else {
middle = (a <= b) ? a : b;
}
}
return middle;
if ((a <= b) && (a <= c)) {
middle = (b <= c) ? b : c;
}
else {
if ((b <= a) && (b <= c)) {
middle = (a <= c) ? a : c;
}
else {
middle = (a <= b) ? a : b;
}
}
return middle;
}

// ***************************** GMedian *****************************
GMedian::GMedian() {}
uint16_t GMedian::filtered(uint16_t value) {
struct pair
{
struct pair *point; /* Pointers forming list linked in sorted order */
uint16_t value; /* Values to sort */
};
static struct pair buffer[MEDIAN_FILTER_SIZE] = {0}; /* Buffer of nwidth pairs */
static struct pair *datpoint = buffer; /* Pointer into circular buffer of data */
static struct pair small = {NULL, STOPPER}; /* Chain stopper */
static struct pair big = {&small, 0}; /* Pointer to head (largest) of linked list.*/

struct pair *successor; /* Pointer to successor of replaced data item */
struct pair *scan; /* Pointer used to scan down the sorted list */
struct pair *scanold; /* Previous value of scan */
struct pair *median; /* Pointer to median */
uint16_t i;

if (value == STOPPER)
{
value = STOPPER + 1; /* No stoppers allowed. */
}

if ( (++datpoint - buffer) >= MEDIAN_FILTER_SIZE)
{
datpoint = buffer; /* Increment and wrap data in pointer.*/
}

datpoint->value = value; /* Copy in new value */
successor = datpoint->point; /* Save pointer to old value's successor */
median = &big; /* Median initially to first in chain */
scanold = NULL; /* Scanold initially null. */
scan = &big; /* Points to pointer to first (largest) value in chain */

/* Handle chain-out of first item in chain as special case */
if (scan->point == datpoint)
{
scan->point = successor;
}
scanold = scan; /* Save this pointer and */
scan = scan->point ; /* step down chain */

/* Loop through the chain, normal loop exit via break. */
for (i = 0 ; i < MEDIAN_FILTER_SIZE; ++i)
{
/* Handle odd-numbered item in chain */
if (scan->point == datpoint)
{
scan->point = successor; /* Chain out the old value.*/
}

if (scan->value < value) /* If value is larger than scanned value,*/
{
datpoint->point = scanold->point; /* Chain it in here. */
scanold->point = datpoint; /* Mark it chained in. */
value = STOPPER;
};

/* Step median pointer down chain after doing odd-numbered element */
median = median->point; /* Step median pointer. */
if (scan == &small)
{
break; /* Break at end of chain */
}
scanold = scan; /* Save this pointer and */
scan = scan->point; /* step down chain */

/* Handle even-numbered item in chain. */
if (scan->point == datpoint)
{
scan->point = successor;
}

if (scan->value < value)
{
datpoint->point = scanold->point;
scanold->point = datpoint;
value = STOPPER;
}

if (scan == &small)
{
break;
}

scanold = scan;
scan = scan->point;
}
return median->value;
{
struct pair *point; /* Pointers forming list linked in sorted order */
uint16_t value; /* Values to sort */
};
static struct pair buffer[MEDIAN_FILTER_SIZE] = {0}; /* Buffer of nwidth pairs */
static struct pair *datpoint = buffer; /* Pointer into circular buffer of data */
static struct pair small = {NULL, STOPPER}; /* Chain stopper */
static struct pair big = {&small, 0}; /* Pointer to head (largest) of linked list.*/

struct pair *successor; /* Pointer to successor of replaced data item */
struct pair *scan; /* Pointer used to scan down the sorted list */
struct pair *scanold; /* Previous value of scan */
struct pair *median; /* Pointer to median */
uint16_t i;

if (value == STOPPER)
{
value = STOPPER + 1; /* No stoppers allowed. */
}

if ( (++datpoint - buffer) >= MEDIAN_FILTER_SIZE)
{
datpoint = buffer; /* Increment and wrap data in pointer.*/
}

datpoint->value = value; /* Copy in new value */
successor = datpoint->point; /* Save pointer to old value's successor */
median = &big; /* Median initially to first in chain */
scanold = NULL; /* Scanold initially null. */
scan = &big; /* Points to pointer to first (largest) value in chain */

/* Handle chain-out of first item in chain as special case */
if (scan->point == datpoint)
{
scan->point = successor;
}
scanold = scan; /* Save this pointer and */
scan = scan->point ; /* step down chain */

/* Loop through the chain, normal loop exit via break. */
for (i = 0 ; i < MEDIAN_FILTER_SIZE; ++i)
{
/* Handle odd-numbered item in chain */
if (scan->point == datpoint)
{
scan->point = successor; /* Chain out the old value.*/
}

if (scan->value < value) /* If value is larger than scanned value,*/
{
datpoint->point = scanold->point; /* Chain it in here. */
scanold->point = datpoint; /* Mark it chained in. */
value = STOPPER;
};

/* Step median pointer down chain after doing odd-numbered element */
median = median->point; /* Step median pointer. */
if (scan == &small)
{
break; /* Break at end of chain */
}
scanold = scan; /* Save this pointer and */
scan = scan->point; /* step down chain */

/* Handle even-numbered item in chain. */
if (scan->point == datpoint)
{
scan->point = successor;
}

if (scan->value < value)
{
datpoint->point = scanold->point;
scanold->point = datpoint;
value = STOPPER;
}

if (scan == &small)
{
break;
}

scanold = scan;
scan = scan->point;
}
return median->value;
}

// ***************************** GABfilter *****************************
GABfilter::GABfilter(float delta, float sigma_process, float sigma_noise) {
dt = delta;
float lambda = (float)sigma_process * dt * dt / sigma_noise;
float r = (4 + lambda - (float)sqrt(8 * lambda + lambda * lambda)) / 4;
a = (float)1 - r * r;
b = (float)2 * (2 - a) - 4 * (float)sqrt(1 - a);
float lambda = (float)sigma_process * dt * dt / sigma_noise;
float r = (4 + lambda - (float)sqrt(8 * lambda + lambda * lambda)) / 4;
a = (float)1 - r * r;
b = (float)2 * (2 - a) - 4 * (float)sqrt(1 - a);
}
void GABfilter::setParameters(float delta, float sigma_process, float sigma_noise) {
dt = delta;
float lambda = (float)sigma_process * dt * dt / sigma_noise;
float r = (4 + lambda - (float)sqrt(8 * lambda + lambda * lambda)) / 4;
a = (float)1 - r * r;
b = (float)2 * (2 - a) - 4 * (float)sqrt(1 - a);
float lambda = (float)sigma_process * dt * dt / sigma_noise;
float r = (4 + lambda - (float)sqrt(8 * lambda + lambda * lambda)) / 4;
a = (float)1 - r * r;
b = (float)2 * (2 - a) - 4 * (float)sqrt(1 - a);
}
float GABfilter::filtered(float value) {
xm = value;
xk = xk_1 + ((float) vk_1 * dt );
vk = vk_1;
rk = xm - xk;
xk += (float)a * rk;
vk += (float)( b * rk ) / dt;
xk_1 = xk;
vk_1 = vk;
return xk_1;
xm = value;
xk = xk_1 + ((float) vk_1 * dt );
vk = vk_1;
rk = xm - xk;
xk += (float)a * rk;
vk += (float)( b * rk ) / dt;
xk_1 = xk;
vk_1 = vk;
return xk_1;
}

// ***************************** GKalman *****************************
#include <math.h>
GKalman::GKalman(float mea_e, float est_e, float q)
{
GKalman::setParameters(mea_e, est_e, q);
GKalman::setParameters(mea_e, est_e, q);
}
GKalman::GKalman(float mea_e, float q)
{
GKalman::setParameters(mea_e, mea_e, q);
GKalman::setParameters(mea_e, mea_e, q);
}

void GKalman::setParameters(float mea_e, float est_e, float q)
{
_err_measure = mea_e;
_err_estimate = est_e;
_q = q;
_err_measure = mea_e;
_err_estimate = est_e;
_q = q;
}
void GKalman::setParameters(float mea_e, float q)
{
GKalman::setParameters(mea_e, mea_e, q);
GKalman::setParameters(mea_e, mea_e, q);
}

float GKalman::filtered(float value)
{
_kalman_gain = _err_estimate / (_err_estimate + _err_measure);
_current_estimate = _last_estimate + _kalman_gain * (value - _last_estimate);
_err_estimate = (1.0 - _kalman_gain)*_err_estimate + fabs(_last_estimate-_current_estimate)*_q;
_last_estimate=_current_estimate;
_kalman_gain = _err_estimate / (_err_estimate + _err_measure);
_current_estimate = _last_estimate + _kalman_gain * (value - _last_estimate);
_err_estimate = (1.0 - _kalman_gain)*_err_estimate + fabs(_last_estimate-_current_estimate)*_q;
_last_estimate=_current_estimate;

return _current_estimate;
}

// ************************* GLinear ***************************
GLinear::GLinear() {}

void GLinear::compute(int *x_array, int *y_array, int arrSize) {
arrSize /= sizeof(int);

for (int i = 0; i < arrSize; i++) { // для всех элементов массива
sumX += x_array[i];
sumY += (long)y_array[i];
sumX2 += x_array[i] * x_array[i];
sumXY += (long)y_array[i] * x_array[i];
}
a = (long)arrSize * sumXY; // расчёт коэффициента наклона приямой
a = a - (long)sumX * sumY;
a = (float)a / (arrSize * sumX2 - sumX * sumX);
b = (float)(sumY - (float)a * sumX) / arrSize;
delta = a * arrSize; // расчёт изменения
}

float GLinear::getA() {
return a;
}

float GLinear::getB() {
return b;
}

return _current_estimate;
float GLinear::getDelta() {
return delta;
}
21 changes: 20 additions & 1 deletion GyverFilters/GyverFilters.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,15 +6,17 @@
#define MEDIAN_FILTER_SIZE (8) // размер медианного фильтра!

/*
Текущая версия: 1.3 от 24.09.2018
Текущая версия: 1.5 от 27.07.2019
GyverFilters - библиотека с некоторыми удобными фильтрами:
- GFilterRA - компактная альтернатива фильтра экспоненциальное бегущее среднее (Running Average)
- GMedian3 - быстрый медианный фильтр 3-го порядка (отсекает выбросы)
- GMedian - медианный фильтр N-го порядка. Порядок настраивается в GyverFilters.h - MEDIAN_FILTER_SIZE
- GABfilter - альфа-бета фильтр (разновидность Калмана для одномерного случая)
- GKalman - упрощённый Калман для одномерного случая (на мой взгляд лучший из фильтров)
- GLinear - линейная аппроксимация методом наименьших квадратов для двух массивов
*/

// компактная альтернатива фильтра экспоненциальное бегущее среднее
class GFilterRA
{
public:
Expand All @@ -36,6 +38,7 @@ class GFilterRA
uint16_t _filterInterval = 0;
};

// быстрый медианный фильтр 3-го порядка
class GMedian3
{
public:
Expand All @@ -47,13 +50,15 @@ class GMedian3
byte counter = 0;
};

// медианный фильтр N-го порядка
class GMedian
{
public:
GMedian();
uint16_t filtered(uint16_t value); // возвращает фильтрованное значение
};

// альфа-бета фильтр
class GABfilter
{
public:
Expand All @@ -72,6 +77,7 @@ class GABfilter
float xm;
};

// упрощённый Калман для одномерного случая
class GKalman
{
public:
Expand All @@ -98,4 +104,17 @@ class GKalman
float _kalman_gain = 0.0;
};

// линейная аппроксимация методом наименьших квадратов
class GLinear {
public:
GLinear();
void compute(int *x_array, int *y_array, int arrSize); // аппроксимировать
float getA(); // получить коэффициент А
float getB(); // получить коэффициент В
float getDelta(); // получить аппроксимированное изменение
private:
int32_t sumX, sumY, sumX2, sumXY;
float a, b, delta;
};

#endif
Loading

0 comments on commit fe4cfe8

Please sign in to comment.