Diff: IIR_filter.cpp

Revision:

5:d6c7ccbbce78

Parent:

0:15be70d21d7c

Child:

10:600d7cf652e7

--- a/IIR_filter.cpp	Thu Mar 01 13:48:32 2018 +0000
+++ b/IIR_filter.cpp	Fri Mar 09 12:53:45 2018 +0000
@@ -1,68 +1,139 @@
 #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
+
+/*
+  IIR filter implemention for the following filter types:
+  init for: first order differentiatior:   G(s) = s/(T*s + 1)
+            first order lowpass with gain  G(s) = K/(T*s + 1)
+            second order lowpass with gain G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0*w0)        
+  the billinear transformation is used for s -> z
+  reseting the filter only makes sence for static signals, whatch out if you're using the differnetiator
+*/
+
+// G(s) = s/(T*s + 1)
+IIR_filter::IIR_filter(float T,float Ts){
+           
+    // filter orders
+    nb = 1; // Filter Order
+    na = 1; // Filter Order
     
-              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) );
+    // filter coefficients
+    B = (float*)malloc((nb+1)*sizeof(float));
+    A = (float*)malloc(na*sizeof(float));    
+    B[0] = 2.0f/(2.0f*T + Ts);
+    B[1] = -B[0];
+    A[0] = -(2.0f*T - Ts)/(2.0f*T + Ts);
+    
+    // signal arrays
+    uk = (float*)malloc((nb+1)*sizeof(float));
+    yk = (float*)malloc(na*sizeof(float));
+    uk[0]= uk[1] = 0.0f;
+    yk[0] = 0.0f;
+    
+    // dc-gain
+    this->K = 0.0f;
+}
+
+// G(s) = K/(T*s + 1)
+IIR_filter::IIR_filter(float T,float Ts,float K){
+    
+    // filter orders
     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);
+    
+    // filter coefficients
+    B = (float*)malloc((nb+1)*sizeof(float));
+    A = (float*)malloc(na*sizeof(float));      
+    B[0] = Ts/(Ts + 2.0f*T);
+    B[1] = B[0];
+    A[0] = (Ts - 2.0f*T)/(Ts + 2.0f*T); 
+    
+    // signal arrays
+    uk = (float*)malloc((nb+1)*sizeof(float));
+    yk = (float*)malloc(na*sizeof(float));
     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;
-    }
+    
+    // dc-gain
+    this->K = K;
+}
+
+// G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0^2) 
+IIR_filter::IIR_filter(float w0,float D, float Ts, float K){
+    
+    // filter orders
+    nb = 2; // Filter Order
+    na = 2; // Filter Order
+    
+    // filter coefficients
+    B = (float*)malloc((nb+1)*sizeof(float));
+    A = (float*)malloc(na*sizeof(float));
+    float k0 = Ts*Ts*w0*w0;
+    float k1 = 4.0f*D*Ts*w0;
+    float k2 = k0 + k1 + 4.0f;    
+    B[0] = K*k0/k2;
+    B[1] = 2.0f*B[0];
+    B[2] = B[0]; 
+    A[0] = (2.0f*k0 - 8.0f)/k2;
+    A[1] = (k0 - k1 + 4.0f)/k2;
+    
+    // signal arrays
+    uk = (float*)malloc((nb+1)*sizeof(float));
+    yk = (float*)malloc(na*sizeof(float));
+    uk[0]= uk[1] = uk[2] = 0.0f;
+    yk[0] = yk[1] = 0.0f;
+    
+    // dc-gain
+    this->K = K;
+}
     
 IIR_filter::~IIR_filter() {} 
     
 void IIR_filter::reset(float val) {
-    for(unsigned int k=0;k < nb;k++)
+    for(int k=0;k < nb;k++)
         uk[k] = val;
-    for(unsigned int k=0;k < na;k++)
-        yk[k] = val*dc;
+    for(int k=0;k < na;k++)
+        yk[k] = val*K;
         
 }
-/* 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)
+
+/* 
+    the filter is operating as follows: 
+    (B[0] + B[1]*z^-1 + ... + B[nb]*z^-nb)*U(z) = (1 + A[0]*z^-1 + ... + A[na-1]*z^-na))*Y(z)
+    y(n) =  B[0]*u(k)   + B[1]*u(k-1) + ... + B[nb]*u(k-nb) + ...
+          - A[0]*y(k-1) - A[1]*y(k-2) - ... - A[na]*y(n-na)
 */
 float IIR_filter::filter(float input){
-    unsigned int k;
-    for(k = nb;k > 0;k--)    // shift input values back
+    for(int 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--)
+    for(int k = 0;k <= nb;k++)
+        ret += B[k] * uk[k];
+    for(int k = 0;k < na;k++)
+        ret -= A[k] * yk[k];
+    for(int k = na;k > 1;k--)
         yk[k-1] = yk[k-2];
     yk[0] = ret;
     return ret;
-    }
\ No newline at end of file
+}
+
+// (B[0] + B[1]*z^-1 + ... + B[nb]*z^-nb)*U(z) = (1 + A[0]*z^-1 + ... + A[na-1]*z^-na))*Y(z)
+/*
+IIR_filter::IIR_filter(float *a[], float *b[], float K){
+    
+    this->A = A[0];
+    this->B = B[0];   
+    nb = sizeof(B)/sizeof(B[0]);
+    na = sizeof(A)/sizeof(A[0]);
+    uk = (float*)malloc((nb+1)*sizeof(float));
+    yk = (float*)malloc(na*sizeof(float));
+    for(int ii=0; ii<nb; ii++){
+        uk[ii] = 0.0f;
+    }
+    for(int ii=0; ii<na; ii++){
+        yk[ii] = 0.0f;
+    }
+    this->K = K;       %%% THIS IMPLEMENTATION SUITS NOT THE RESET PROCESS %%%
+}*/
\ No newline at end of file