From ed9e0fa975ce0e38d3240d05979bc4104a0b0cdf Mon Sep 17 00:00:00 2001 From: Miroslav Lichvar Date: Tue, 29 Oct 2013 11:58:19 +0100 Subject: [PATCH] Add median filter. 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 --- filter.c | 3 ++ filter.h | 1 + makefile | 2 +- mmedian.c | 110 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ mmedian.h | 27 ++++++++++++++ 5 files changed, 142 insertions(+), 1 deletion(-) create mode 100644 mmedian.c create mode 100644 mmedian.h diff --git a/filter.c b/filter.c index 65eb003..fceb29a 100644 --- a/filter.c +++ b/filter.c @@ -19,12 +19,15 @@ #include "filter_private.h" #include "mave.h" +#include "mmedian.h" struct filter *filter_create(enum filter_type type, int length) { switch (type) { case FILTER_MOVING_AVERAGE: return mave_create(length); + case FILTER_MOVING_MEDIAN: + return mmedian_create(length); default: return NULL; } diff --git a/filter.h b/filter.h index 4d7b370..5a196bc 100644 --- a/filter.h +++ b/filter.h @@ -30,6 +30,7 @@ struct filter; */ enum filter_type { FILTER_MOVING_AVERAGE, + FILTER_MOVING_MEDIAN, }; /** diff --git a/makefile b/makefile index 5e9b009..37cbaf9 100644 --- a/makefile +++ b/makefile @@ -24,7 +24,7 @@ CFLAGS = -Wall $(VER) $(incdefs) $(DEBUG) $(EXTRA_CFLAGS) LDLIBS = -lm -lrt $(EXTRA_LDFLAGS) PRG = ptp4l pmc phc2sys hwstamp_ctl OBJ = bmc.o clock.o clockadj.o clockcheck.o config.o fault.o \ - filter.o fsm.o mave.o msg.o phc.o pi.o port.o print.o \ + filter.o fsm.o mave.o mmedian.o msg.o phc.o pi.o port.o print.o \ ptp4l.o raw.o servo.o sk.o stats.o tlv.o tmtab.o transport.o udp.o \ udp6.o uds.o util.o version.o diff --git a/mmedian.c b/mmedian.c new file mode 100644 index 0000000..1d15789 --- /dev/null +++ b/mmedian.c @@ -0,0 +1,110 @@ +/** + * @file mmedian.c + * @note Copyright (C) 2013 Miroslav Lichvar + * + * 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 +#include + +#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; +} diff --git a/mmedian.h b/mmedian.h new file mode 100644 index 0000000..8fee0dc --- /dev/null +++ b/mmedian.h @@ -0,0 +1,27 @@ +/** + * @file mmedian.h + * @brief Implements a moving median. + * @note Copyright (C) 2013 Miroslav Lichvar + * + * 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. + */ +#ifndef HAVE_MMEDIAN_H +#define HAVE_MMEDIAN_H + +#include "filter.h" + +struct filter *mmedian_create(int length); + +#endif