cuboid strong

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers IIR_filter.cpp Source File

IIR_filter.cpp

00001 #include "IIR_filter.h"
00002 #include "mbed.h"
00003 using namespace std;
00004 /*  IIR filter. implemention is for n-th order, init only for 1st order
00005     coeffissients based on bilinear transform if 1st order filter
00006     
00007               1                              Ts/(Ts+2*tau) * (z+1)
00008     G(s) = -------                  G(z) = ---------------------------
00009             tau*s+1                         z + (Ts-2*tau)/(Ts+2*tau)
00010     */
00011 IIR_filter::IIR_filter(float tau,float Ts){
00012     b = (float*)malloc( 2 * sizeof(float) );
00013     a = (float*)malloc( 1 * sizeof(float) );
00014     uk = (float*)malloc( 2 * sizeof(float) );
00015     yk = (float*)malloc( 1 * sizeof(float) );
00016     nb = 1; // Filter Order
00017     na = 1; // Filter Order
00018     b[0] = Ts/(Ts+2.0f*tau);
00019     b[1] = b[0];
00020     a[0] = (Ts-2.0f*tau)/(Ts+2.0f*tau);
00021     uk[0]= uk[1] = 0.0f;
00022     yk[0] = 0.0f;
00023     this->dc = 1.0f;
00024     }
00025     // the following filter has in addition a dc-gain ( dc/(tau*s+1)  )
00026 IIR_filter::IIR_filter(float tau,float Ts,float dc){
00027     b = (float*)malloc( 2 * sizeof(float) );
00028     a = (float*)malloc( 1 * sizeof(float) );
00029     uk = (float*)malloc( 2 * sizeof(float) );
00030     yk = (float*)malloc( 1 * sizeof(float) );
00031     nb = 1; // Filter Order
00032     na = 1; // Filter Order
00033     b[0] = dc * Ts/(Ts+2.0f*tau);
00034     b[1] = b[0];
00035     a[0] = (Ts-2.0f*tau)/(Ts+2.0f*tau);
00036     uk[0]= uk[1] = 0.0f;
00037     yk[0] = 0.0f;
00038     this->dc = dc;
00039     }
00040     
00041 IIR_filter::~IIR_filter() {} 
00042     
00043 void IIR_filter::reset(float val) {
00044     for(unsigned int k=0;k < nb;k++)
00045         uk[k] = val;
00046     for(unsigned int k=0;k < na;k++)
00047         yk[k] = val*dc;
00048         
00049 }
00050 /* the filter step: 
00051 y(n) =  b[0]*u_k   + b[1]*u_k-1 + ... + b[nb]*u_k-nb
00052       - a[0]*y_k-1 - ... - a[na]*y_n-na  // mind: a[0] corresponds to z^(na-1)
00053 */
00054 float IIR_filter::filter(float input){
00055     unsigned int k;
00056     for(k = nb;k > 0;k--)    // shift input values back
00057         uk[k] = uk[k-1];
00058     uk[0] = input;
00059     float ret = 0.0f;
00060     for(k = 0;k <= nb;k++)
00061         ret += b[k] * uk[k];
00062     for(k = 0;k < na;k++)
00063         ret -= a[k] * yk[k];
00064     for(k = na;k > 1;k--)
00065         yk[k-1] = yk[k-2];
00066     yk[0] = ret;
00067     return ret;
00068     }