Diff: IIR_filter.cpp

Revision:

0:15be70d21d7c

Child:

5:d6c7ccbbce78

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IIR_filter.cpp	Wed Jan 10 16:08:07 2018 +0000
@@ -0,0 +1,68 @@
+#include "IIR_filter.h"
+#include "mbed.h"
+using namespace std;
+/*  IIR filter. implemention is for n-th order, init only for 1st order
+    coeffissients based on bilinear transform if 1st order filter
+    
+              1                              Ts/(Ts+2*tau) * (z+1)
+    G(s) = -------                  G(z) = ---------------------------
+            tau*s+1                         z + (Ts-2*tau)/(Ts+2*tau)
+    */
+IIR_filter::IIR_filter(float tau,float Ts){
+    b = (float*)malloc( 2 * sizeof(float) );
+    a = (float*)malloc( 1 * sizeof(float) );
+    uk = (float*)malloc( 2 * sizeof(float) );
+    yk = (float*)malloc( 1 * sizeof(float) );
+    nb = 1; // Filter Order
+    na = 1; // Filter Order
+    b[0] = Ts/(Ts+2.0f*tau);
+    b[1] = b[0];
+    a[0] = (Ts-2.0f*tau)/(Ts+2.0f*tau);
+    uk[0]= uk[1] = 0.0f;
+    yk[0] = 0.0f;
+    this->dc = 1.0f;
+    }
+    // the following filter has in addition a dc-gain ( dc/(tau*s+1)  )
+IIR_filter::IIR_filter(float tau,float Ts,float dc){
+    b = (float*)malloc( 2 * sizeof(float) );
+    a = (float*)malloc( 1 * sizeof(float) );
+    uk = (float*)malloc( 2 * sizeof(float) );
+    yk = (float*)malloc( 1 * sizeof(float) );
+    nb = 1; // Filter Order
+    na = 1; // Filter Order
+    b[0] = dc * Ts/(Ts+2.0f*tau);
+    b[1] = b[0];
+    a[0] = (Ts-2.0f*tau)/(Ts+2.0f*tau);
+    uk[0]= uk[1] = 0.0f;
+    yk[0] = 0.0f;
+    this->dc = dc;
+    }
+    
+IIR_filter::~IIR_filter() {} 
+    
+void IIR_filter::reset(float val) {
+    for(unsigned int k=0;k < nb;k++)
+        uk[k] = val;
+    for(unsigned int k=0;k < na;k++)
+        yk[k] = val*dc;
+        
+}
+/* the filter step: 
+y(n) =  b[0]*u_k   + b[1]*u_k-1 + ... + b[nb]*u_k-nb
+      - a[0]*y_k-1 - ... - a[na]*y_n-na  // mind: a[0] corresponds to z^(na-1)
+*/
+float IIR_filter::filter(float input){
+    unsigned int k;
+    for(k = nb;k > 0;k--)    // shift input values back
+        uk[k] = uk[k-1];
+    uk[0] = input;
+    float ret = 0.0f;
+    for(k = 0;k <= nb;k++)
+        ret += b[k] * uk[k];
+    for(k = 0;k < na;k++)
+        ret -= a[k] * yk[k];
+    for(k = na;k > 1;k--)
+        yk[k-1] = yk[k-2];
+    yk[0] = ret;
+    return ret;
+    }
\ No newline at end of file