first commit

.gitignore

@@ -0,0 +1,4 @@
+backup/
+*.o
+.depend
+filter_test

README

@@ -0,0 +1 @@
+IIR filter using Laplace transform

dumper.c

@@ -0,0 +1,41 @@
+#include <stdio.h>
+
+#include "dumper.h"
+
+void print_polynom(const double *p, const int np, const char v)
+{
+    int i, j = 0;
+    for (i = 0; i < np; ++i) {
+	double pi = p[i];
+	if (pi == 0)
+	    continue;
+	if (j == 0) {
+	    if (pi < 0)
+		printf("- ");
+	} else {
+	    if (pi < 0)
+		printf(" - ");
+	    else
+		printf(" + ");
+	}
+	if (pi < 0)
+	    pi = -pi;
+	printf("%f", pi);
+	if (i > 0)
+	    printf("%c", v);
+	if (i > 1)
+	    printf("^%d", i);
+	++j;
+    }
+    printf("\n");
+}
+
+void print_polynom_nd(const double *num, const int nnum, const double *den, const int nden, const char v, const char *title)
+{
+    if (title)
+	printf("\n%s:\n", title);
+    print_polynom(num, nnum, v);
+    printf("----------\n");
+    print_polynom(den, nden, v);
+    printf("\n");
+}

dumper.h

@@ -0,0 +1,2 @@
+void print_polynom(const double *p, const int np, const char v);
+void print_polynom_nd(const double *num, const int nnum, const double *den, const int nden, const char v, const char *title);

filter.c

@@ -0,0 +1,204 @@
+#include <string.h>
+#include <stdlib.h>
+
+#include "polynom.h"
+#include "filter.h"
+
+
+void bilinear_transform_n(const double *s, const int ns, const double t, double *znum, double *zden)
+{
+    const double z1num[2] = { 2 / t, - 2 / t };
+    const double z1den[2] = { 1, 1 };
+
+    double z[ns];
+    int nz;
+
+    double h = 1;
+    int i;
+
+    if (ns == 0)
+	return;
+
+    clear_polynom(znum, ns);
+    clear_polynom(zden, ns);
+    znum[0] = 1;
+    zden[0] = 1;
+    nz = 1;
+
+    for (i = ns - 1; i > 0; --i) {
+	h = s[i];
+	if (h != 0)
+	    break;
+    }
+
+    if (i == 0) {
+	znum[0] = s[0];
+	return;
+    }
+
+    for (i = i - 1; i >= 0; --i) {
+	expand_polynom(znum, nz, z1num, 2, z);
+	copy_polynom(z, nz + 1, znum);
+	expand_polynom(zden, nz, z1den, 2, z);
+	copy_polynom(z, nz + 1, zden);
+	++nz;
+	add_polynom(znum, nz, s[i] / h, zden);
+    }
+    mul_polynom(znum, ns, h);
+}
+
+
+int bilinear_transform_nd(const double *snum, const int nsnum, const double *sden, const int nsden, const double t, double *znum, double *zden)
+{
+    double znumnum[nsnum], znumden[nsnum], zdennum[nsden], zdenden[nsden];
+    int nz = (nsnum > nsden ? nsnum : nsden);
+    int nznumden, nzdenden;
+    double l;
+
+    const double z1[2] = { 1, 1 };
+
+    bilinear_transform_n(snum, nsnum, t, znumnum, znumden);
+    bilinear_transform_n(sden, nsden, t, zdennum, zdenden);
+
+/*
+    printf("zn:\n");
+    print_polynom_nd(znumnum, nsnum, znumden, nsnum, 'z');
+    printf("zd:\n");
+    print_polynom_nd(zdennum, nsden, zdenden, nsden, 'z');
+*/
+
+    clear_polynom(znumden, nsnum);
+    clear_polynom(zdenden, nsden);
+    nzdenden = 0;
+    nznumden = 0;
+    if (nsden > 0) {
+	zdenden[0] = 1;
+	nzdenden = 1;
+    }
+    if (nsnum > 0) {
+	znumden[0] = 1;
+	nznumden = 1;
+    }
+    if (nsnum > nsden) {
+	pow_polynom(z1, 2, nsnum - nsden, znumden);
+	nznumden = (nsnum - nsden) + 1;
+    }
+    if (nsden > nsnum) {
+	pow_polynom(z1, 2, nsden - nsnum, zdenden);
+	nzdenden = (nsden - nsnum) + 1;
+    }
+
+    expand_polynom(znumnum, nsnum, zdenden, nzdenden, znum);
+    expand_polynom(zdennum, nsden, znumden, nznumden, zden);
+
+    if (nz > 0) {
+	l = zden[0];
+	if (l != 0) {
+	    mul_polynom(zden, nz, 1 / l);
+	    mul_polynom(znum, nz, 1 / l);
+	}
+    }
+
+    return nz;
+}
+
+
+IIR_FILTER *iir_filter_init(const double *b, const int m, const double *a, const int n)
+{
+    IIR_FILTER *iir;
+
+    if ((iir = (IIR_FILTER *)malloc(sizeof(IIR_FILTER))) == NULL)
+	return NULL;
+
+    memset(iir, 0, sizeof(IIR_FILTER));
+
+    iir->b = (double *)calloc(m + 1, sizeof(double));
+    iir->m = m;
+    iir->a = (double *)calloc(n + 1, sizeof(double));
+    iir->n = n;
+
+    iir->x = (double *)calloc(m + 1, sizeof(double));
+    iir->y = (double *)calloc(n + 1, sizeof(double));
+
+    memcpy(iir->b, b, (m + 1) * sizeof(double));
+    memcpy(iir->a, a, (n + 1) * sizeof(double));
+
+    return iir;
+}
+
+
+void iir_filter_free(IIR_FILTER *iir)
+{
+    if (iir) {
+	free(iir->b);
+	free(iir->a);
+	free(iir->x);
+	free(iir->y);
+    }
+    free(iir);
+}
+
+
+double iir_filter_next(IIR_FILTER *iir, const double x)
+{
+    double sx = 0, sy = 0;
+    double y;
+    int i;
+
+    for (i = iir->m; i > 0; --i)
+        iir->x[i] = iir->x[i - 1];
+    iir->x[0] = x;
+    for (i = 0; i <= iir->m; ++i)
+        sx += iir->b[i] * iir->x[i];
+
+    for (i = iir->n; i > 0; --i)
+        iir->y[i] = iir->y[i - 1];
+    for (i = 1; i <= iir->n; ++i)
+        sy += iir->a[i] * iir->y[i];
+    y = sx - sy;
+    iir->y[0] = y;
+
+    return y;
+}
+
+
+IIR_FILTER *laplace_nd_filter(const double *num, const int nnum, const double *den, const int nden, const double dt, const int L)
+{
+    double interp_znum[] = { 0.0221824, -0.0134342, -0.0134342, 0.0221824 };
+    int ninterp_znum = sizeof(interp_znum) / sizeof(interp_znum[0]);
+    double interp_zden[] = { 1, -2.5885253, 2.3163762, -0.7103546 };
+    int ninterp_zden = sizeof(interp_zden) / sizeof(interp_zden[0]);
+
+    int nz = (nnum > nden ? nnum : nden);
+    double laplace_znum[nz];
+    double laplace_zden[nz];
+
+    int nznum = nz + ninterp_znum - 1;
+    int nzden = nz + ninterp_zden - 1;
+    double znum[nznum];
+    double zden[nzden];
+
+    clear_polynom(laplace_znum, nz);
+    clear_polynom(laplace_zden, nz);
+    bilinear_transform_nd(num, nnum, den, nden, dt, laplace_znum, laplace_zden);
+
+    if (L > 1) {
+	expand_polynom(laplace_znum, nz, interp_znum, ninterp_znum, znum);
+	expand_polynom(laplace_zden, nz, interp_zden, ninterp_zden, zden);
+    } else {
+	copy_polynom(laplace_znum, nz, znum);
+	copy_polynom(laplace_zden, nz, zden);
+	nznum = nz;
+	nzden = nz;
+    }
+
+    IIR_FILTER *iir = iir_filter_init(znum, nznum - 1, zden, nzden - 1);
+
+    return iir;
+}
+
+
+IIR_FILTER *laplace_zp_filter(const double *num[2], const int nnum, const double *den[2], const int nden, const double dt, const int L)
+{
+    return NULL;
+}

filter.h

@@ -0,0 +1,20 @@
+typedef struct {
+    double *b;
+    int m;
+
+    double *a;
+    int n;
+
+    double *x;
+    double *y;
+} IIR_FILTER;
+
+void bilinear_transform_n(const double *s, const int ns, const double t, double *znum, double *zden);
+int bilinear_transform_nd(const double *snum, const int nsnum, const double *sden, const int nsden, const double t, double *znum, double *zden);
+
+IIR_FILTER *iir_filter_init(const double *b, const int m, const double *a, const int n);
+void iir_filter_free(IIR_FILTER *iir);
+double iir_filter_next(IIR_FILTER *iir, const double x);
+
+IIR_FILTER *laplace_nd_filter(const double *num, const int nnum, const double *den, const int nden, const double dt, const int L);
+IIR_FILTER *laplace_zp_filter(const double *num[2], const int nnum, const double *den[2], const int nden, const double dt, const int L);

filter_test.c

@@ -0,0 +1,36 @@
+#include <stdio.h>
+
+#include "filter.h"
+#include "dumper.h"
+
+int main()
+{
+    double snum[] = { 1 };
+    double sden[] = { 1, 1 };
+
+    double dT = 0.01;
+    int L = 1;
+    double tmax = 0.2;
+
+    int nsnum = sizeof(snum) / sizeof(snum[0]);
+    int nsden = sizeof(sden) / sizeof(sden[0]);
+    double dt = dT / L;
+
+    IIR_FILTER *iir = laplace_nd_filter(snum, nsnum, sden, nsden, dt, L);
+
+    print_polynom_nd(snum, nsnum, sden, nsden, 's', "Laplace H(s)");
+    printf("dT: %f, L: %d, dt: %f\n", dT, L, dt);
+    print_polynom_nd(iir->b, iir->m + 1, iir->a, iir->n + 1, 'z', "IIR H(z)");
+
+    printf("t\tx\ty\n");
+    double t, x, y;
+    for (t = 0; t < tmax; t += dt) {
+	x = 1;
+	y = iir_filter_next(iir, x);
+	printf("%f\t%f\t%f\n", t, x, y);
+    }
+
+    iir_filter_free(iir);
+
+    return 0;
+}

makefile

@@ -0,0 +1,28 @@
+SRC=filter_test.c filter.c polynom.c dumper.c
+OBJ=$(SRC:.c=.o)
+EXE=filter_test
+
+CC=gcc
+CFLAGS=-Wall -O3
+#LDFLAGS=-static-libgcc -static-libstdc++ -static
+LDFLAGS=
+RM=rm
+
+all: $(EXE)
+
+%.o: %.c
+	$(CC) $(CFLAGS) -o $@ -c $<
+
+$(EXE): $(OBJ)
+	$(CC) $(OBJ) $(LDFLAGS) -o $@
+
+clean:
+	$(RM) $(OBJ) $(EXE)
+
+depend: $(SRCS)
+	rm -f ./.depend
+	$(CC) $(CFLAGS) -MM $(SRC) >> ./.depend
+
+-include .depend
+
+.PHONY: all clean