branch for cuboid
Embed:
(wiki syntax)
Show/hide line numbers
IIR_filter.cpp
00001 #include "IIR_filter.h" 00002 #include "mbed.h" 00003 using namespace std; 00004 00005 00006 /* 00007 IIR filter implemention for the following filter types: 00008 init for: first order differentiatior: G(s) = s/(T*s + 1) 00009 first order lowpass with gain G(s) = K/(T*s + 1) 00010 second order lowpass with gain G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0*w0) 00011 nth order, with arbitrary values 00012 the billinear transformation is used for s -> z 00013 reseting the filter only makes sence for static signals, whatch out if you're using the differnetiator 00014 */ 00015 00016 // G(s) = s/(T*s + 1) 00017 // default constreuctor 00018 00019 00020 IIR_filter::IIR_filter(float T, float Ts){ 00021 00022 // filter orders 00023 nb = 1; // Filter Order 00024 na = 1; // Filter Order 00025 00026 // filter coefficients 00027 B = (double*)malloc((nb+1)*sizeof(double)); 00028 A = (double*)malloc(na*sizeof(double)); 00029 B[0] = 2.0/(2.0*(double)T + (double)Ts); 00030 B[1] = -B[0]; 00031 A[0] = -(2.0*(double)T - (double)Ts)/(2.0*(double)T + (double)Ts); 00032 00033 // signal arrays 00034 uk = (double*)malloc((nb+1)*sizeof(double)); 00035 yk = (double*)malloc(na*sizeof(double)); 00036 uk[0]= uk[1] = 0.0; 00037 yk[0] = 0.0; 00038 00039 // dc-gain 00040 this->K = 0.0; 00041 } 00042 00043 // G(s) = K/(T*s + 1) 00044 IIR_filter::IIR_filter(float T, float Ts, float K){ 00045 00046 // filter orders 00047 nb = 1; // Filter Order 00048 na = 1; // Filter Order 00049 00050 // filter coefficients 00051 B = (double*)malloc((nb+1)*sizeof(double)); 00052 A = (double*)malloc(na*sizeof(double)); 00053 B[0] = (double)Ts/((double)Ts + 2.0*(double)T); 00054 B[1] = B[0]; 00055 A[0] = ((double)Ts - 2.0*(double)T)/((double)Ts + 2.0*(double)T); 00056 00057 // signal arrays 00058 uk = (double*)malloc((nb+1)*sizeof(double)); 00059 yk = (double*)malloc(na*sizeof(double)); 00060 uk[0]= uk[1] = 0.0; 00061 yk[0] = 0.0; 00062 00063 // dc-gain 00064 this->K = (double)K; 00065 } 00066 void IIR_filter::setup(float T, float Ts, float K){ 00067 00068 // filter orders 00069 nb = 1; // Filter Order 00070 na = 1; // Filter Order 00071 00072 // filter coefficients 00073 B = (double*)malloc((nb+1)*sizeof(double)); 00074 A = (double*)malloc(na*sizeof(double)); 00075 B[0] = (double)Ts/((double)Ts + 2.0*(double)T); 00076 B[1] = B[0]; 00077 A[0] = ((double)Ts - 2.0*(double)T)/((double)Ts + 2.0*(double)T); 00078 00079 // signal arrays 00080 uk = (double*)malloc((nb+1)*sizeof(double)); 00081 yk = (double*)malloc(na*sizeof(double)); 00082 uk[0]= uk[1] = 0.0; 00083 yk[0] = 0.0; 00084 00085 // dc-gain 00086 this->K = (double)K; 00087 } 00088 00089 00090 00091 // G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0^2) 00092 IIR_filter::IIR_filter(float w0, float D, float Ts, float K){ 00093 00094 // filter orders 00095 nb = 2; // Filter Order 00096 na = 2; // Filter Order 00097 00098 // filter coefficients 00099 B = (double*)malloc((nb+1)*sizeof(double)); 00100 A = (double*)malloc(na*sizeof(double)); 00101 double k0 = (double)Ts*(double)Ts*(double)w0*(double)w0; 00102 double k1 = 4.0*(double)D*(double)Ts*(double)w0; 00103 double k2 = k0 + k1 + 4.0; 00104 B[0] = (double)K*k0/k2; 00105 B[1] = 2.0*B[0]; 00106 B[2] = B[0]; 00107 A[0] = (2.0*k0 - 8.0)/k2; 00108 A[1] = (k0 - k1 + 4.0)/k2; 00109 00110 // signal arrays 00111 uk = (double*)malloc((nb+1)*sizeof(double)); 00112 yk = (double*)malloc(na*sizeof(double)); 00113 uk[0]= uk[1] = uk[2] = 0.0; 00114 yk[0] = yk[1] = 0.0; 00115 00116 // dc-gain 00117 this->K = (double)K; 00118 } 00119 00120 IIR_filter::IIR_filter(float *b, float *a, int nb_, int na_){ 00121 00122 // filter orders 00123 this->nb = nb_-1; // Filter Order 00124 this->na = na_; // Filter Order 00125 00126 // filter coefficients 00127 B = (double*)malloc((nb+1)*sizeof(double)); 00128 A = (double*)malloc(na*sizeof(double)); 00129 uk = (double*)malloc((nb+1)*sizeof(double)); 00130 yk = (double*)malloc(na*sizeof(double)); 00131 00132 for(int k=0;k<=nb;k++){ 00133 B[k]=b[k]; 00134 uk[k]=0.0; 00135 } 00136 for(int k=0;k<na;k++){ 00137 A[k] = a[k]; 00138 yk[k] = 0.0; 00139 } 00140 00141 // dc-gain 00142 this->K = 1.0; 00143 } 00144 00145 00146 IIR_filter::~IIR_filter() {} 00147 00148 void IIR_filter::reset(float val) { 00149 for(int k=0;k < nb;k++) 00150 uk[k] = (double)val; 00151 for(int k=0;k < na;k++) 00152 yk[k] = (double)val*K; 00153 00154 } 00155 00156 /* 00157 the filter is operating as follows: 00158 (B[0] + B[1]*z^-1 + ... + B[nb]*z^-nb)*U(z) = (1 + A[0]*z^-1 + ... + A[na-1]*z^-na))*Y(z) 00159 y(n) = B[0]*u(k) + B[1]*u(k-1) + ... + B[nb]*u(k-nb) + ... 00160 - A[0]*y(k-1) - A[1]*y(k-2) - ... - A[na]*y(n-na) 00161 */ 00162 float IIR_filter::filter(double input){ 00163 for(int k = nb;k > 0;k--) // shift input values back 00164 uk[k] = uk[k-1]; 00165 uk[0] = input; 00166 double ret = 0.0; 00167 for(int k = 0;k <= nb;k++) 00168 ret += B[k] * uk[k]; 00169 for(int k = 0;k < na;k++) 00170 ret -= A[k] * yk[k]; 00171 for(int k = na;k > 1;k--) 00172 yk[k-1] = yk[k-2]; 00173 yk[0] = ret; 00174 return (float)ret; 00175 } 00176 00177 00178 00179 00180
Generated on Fri Jul 29 2022 15:49:12 by
1.7.2