ed9e0fa975
Median filter has an advantage over moving average that it is much less sensitive to outliers. For instance, it allows much faster recovery from an external clock time step which happened between receiving sync message and sending delay_req message. The measured delay includes a large error, but the median is still a good estimate of the delay and the first step correction applied by the servo is right. In this implementation the median update has linear time complexity. Signed-off-by: Miroslav Lichvar <mlichvar@redhat.com>
111 lines
2.7 KiB
C
111 lines
2.7 KiB
C
/**
|
|
* @file mmedian.c
|
|
* @note Copyright (C) 2013 Miroslav Lichvar <mlichvar@redhat.com>
|
|
*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 2 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
|
*/
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
#include "mmedian.h"
|
|
#include "filter_private.h"
|
|
|
|
struct mmedian {
|
|
struct filter filter;
|
|
int cnt;
|
|
int len;
|
|
int index;
|
|
/* Indices sorted by value. */
|
|
int *order;
|
|
/* Values stored in circular buffer. */
|
|
tmv_t *samples;
|
|
};
|
|
|
|
static void mmedian_destroy(struct filter *filter)
|
|
{
|
|
struct mmedian *m = container_of(filter, struct mmedian, filter);
|
|
free(m->order);
|
|
free(m->samples);
|
|
free(m);
|
|
}
|
|
|
|
static tmv_t mmedian_sample(struct filter *filter, tmv_t sample)
|
|
{
|
|
struct mmedian *m = container_of(filter, struct mmedian, filter);
|
|
int i;
|
|
|
|
m->samples[m->index] = sample;
|
|
if (m->cnt < m->len) {
|
|
m->cnt++;
|
|
} else {
|
|
/* Remove index of the replaced value from order. */
|
|
for (i = 0; i < m->cnt; i++)
|
|
if (m->order[i] == m->index)
|
|
break;
|
|
for (; i + 1 < m->cnt; i++)
|
|
m->order[i] = m->order[i + 1];
|
|
}
|
|
|
|
/* Insert index of the new value to order. */
|
|
for (i = m->cnt - 1; i > 0; i--) {
|
|
if (m->samples[m->order[i - 1]] <= m->samples[m->index])
|
|
break;
|
|
m->order[i] = m->order[i - 1];
|
|
}
|
|
m->order[i] = m->index;
|
|
|
|
m->index = (1 + m->index) % m->len;
|
|
|
|
if (m->cnt % 2)
|
|
return m->samples[m->order[m->cnt / 2]];
|
|
else
|
|
return tmv_div(tmv_add(m->samples[m->order[m->cnt / 2 - 1]],
|
|
m->samples[m->order[m->cnt / 2]]), 2);
|
|
}
|
|
|
|
static void mmedian_reset(struct filter *filter)
|
|
{
|
|
struct mmedian *m = container_of(filter, struct mmedian, filter);
|
|
m->cnt = 0;
|
|
m->index = 0;
|
|
}
|
|
|
|
struct filter *mmedian_create(int length)
|
|
{
|
|
struct mmedian *m;
|
|
|
|
if (length < 1)
|
|
return NULL;
|
|
m = calloc(1, sizeof(*m));
|
|
if (!m)
|
|
return NULL;
|
|
m->filter.destroy = mmedian_destroy;
|
|
m->filter.sample = mmedian_sample;
|
|
m->filter.reset = mmedian_reset;
|
|
m->order = calloc(1, length * sizeof(*m->order));
|
|
if (!m->order) {
|
|
free(m);
|
|
return NULL;
|
|
}
|
|
m->samples = calloc(1, length * sizeof(*m->samples));
|
|
if (!m->samples) {
|
|
free(m->order);
|
|
free(m);
|
|
return NULL;
|
|
}
|
|
m->len = length;
|
|
return &m->filter;
|
|
}
|