cuboid strong
Dependencies: mbed
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 }
Generated on Fri Jul 15 2022 09:11:08 by
1.7.2