From 9a87889dd3bca2dcb7b74b8f9deeab6526fdf795 Mon Sep 17 00:00:00 2001
From: Laurence Withers <l@lwithers.me.uk>
Date: Fri, 15 Apr 2011 17:37:22 +0000
Subject: [PATCH] Improve initial conditions
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Compute the DC gain of the filter (assuming 0 if unstable) and use that to
set the initial conditions of a filter as follows:
  x(t<0)=x(0)
  y(t<0)=x(0) × dc_gain
---
 src/docs/iir_structure.dox |  6 ++++--
 src/libiir/200_iir.c       | 29 +++++++++++++++++++++++++++--
 src/libiir/200_iir.h       |  2 +-
 3 files changed, 32 insertions(+), 5 deletions(-)

diff --git a/src/docs/iir_structure.dox b/src/docs/iir_structure.dox
index 08af02e..cb3a852 100644
--- a/src/docs/iir_structure.dox
+++ b/src/docs/iir_structure.dox
@@ -18,8 +18,10 @@ This leads to a general IIR filter equation:
 
 <code>y(t) = x(t).c[0] + x(t-1).c[1] + … + x(t-N).c[N] - y(t-1).d[0] - y(t-2).d[1] - … - y(t-1-M).d[M]</code>
 
-For initial conditions, the library sets <code>y(t)</code> for t &lt; 0 to 0,
-and <code>x(t)</code> for t &lt; 0 to <code>x(0)</code>.
+For initial conditions, the library sets <code>y(t)</code> for t &lt; 0 to
+<code>x(0).G</code> (where <code>G</code> is the DC gain of the filter, or 0 if
+it looks like the filter is unstable at DC), and <code>x(t)</code> for t &lt; 0
+to <code>x(0)</code>.
 
 */
 
diff --git a/src/libiir/200_iir.c b/src/libiir/200_iir.c
index 91db516..a730171 100644
--- a/src/libiir/200_iir.c
+++ b/src/libiir/200_iir.c
@@ -104,7 +104,6 @@ iir_filter_new(const struct iir_coeff_t* coeff)
     /* allocate space for state */
     fi->x = malloc(sizeof(double) * fi->nc);
     fi->y = malloc(sizeof(double) * fi->nd);
-    memset(fi->y, 0, sizeof(double) * fi->nd);
 
     return fi;
 }
@@ -173,7 +172,6 @@ struct iir_filter_t* iir_filter_copy(const struct iir_filter_t* fi, int state)
             copy->xpos = fi->xpos;
             copy->ypos = fi->ypos;
         } else {
-            memset(copy->y, 0, sizeof(double) * fi->nd);
             copy->ready = copy->xpos = copy->ypos = 0;
         }
 
@@ -203,15 +201,42 @@ iir_get_xy(const double* xy, int pos, int max, int step)
     return xy[pos];
 }
 
+static double
+iir_dc_gain(const struct iir_filter_t* fi)
+{
+    int i;
+    double sum, gain;
+
+    /* If the forward path DC gain is less than a threshold then we assume ‘fi’
+     * has high pass characteristics.
+     */
+    sum = 0;
+    for(i = 0; i < fi->nc; ++i) sum += fi->c[i];
+    gain = sum;
+
+    sum = 0;
+    for(i = 0; i < fi->nd; ++i) sum += fi->d[i];
+    if(fabs(sum - 1) < 0.001) {
+        /* ergh, looks unstable at DC; pretend DC gain is 0 */
+        return 0;
+    }
+
+    gain /= (1 + sum);
+    return gain;
+}
+
 double
 iir_filter(struct iir_filter_t* fi, double samp)
 {
     int i;
+    double gain;
 
     while(fi) {
         if(!fi->ready) {
             /* initial conditions */
+            gain = iir_dc_gain(fi);
             for(i = 0; i < fi->nc; ++i) fi->x[i] = samp;
+            for(i = 0; i < fi->nd; ++i) fi->y[i] = samp * gain;
             fi->ready = 1;
         }
 
diff --git a/src/libiir/200_iir.h b/src/libiir/200_iir.h
index 53e2629..b313a60 100644
--- a/src/libiir/200_iir.h
+++ b/src/libiir/200_iir.h
@@ -84,7 +84,7 @@ is copied into the returned structure, meaning the coefficients can be freed
 after this function returns if they will not be needed again.
 
 The first sample passed through \ref iir_filter() will be used to set initial
-conditions.
+conditions (see \ref iir_structure). 
 
 An arbitrary number of further filters may be chained on to the end of this
 instance through \ref iir_filter_chain().