Nicolas Borla / Mbed OS BBR_1Ebene

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

IIR_filter.cpp@0:fbdae7e6d805, 2018-05-14 (annotated)

Committer:
borlanic
Date:
Mon May 14 11:29:06 2018 +0000
Revision:
0:fbdae7e6d805
BBR

Who changed what in which revision?

UserRevisionLine numberNew contents of line
borlanic 0:fbdae7e6d805 1 #include "IIR_filter.h"
borlanic 0:fbdae7e6d805 2 #include "mbed.h"
borlanic 0:fbdae7e6d805 3 using namespace std;
borlanic 0:fbdae7e6d805 4
borlanic 0:fbdae7e6d805 5 /*
borlanic 0:fbdae7e6d805 6 IIR filter implemention for the following filter types:
borlanic 0:fbdae7e6d805 7 init for: first order differentiatior: G(s) = s/(T*s + 1)
borlanic 0:fbdae7e6d805 8 first order lowpass with gain G(s) = K/(T*s + 1)
borlanic 0:fbdae7e6d805 9 second order lowpass with gain G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0*w0)
borlanic 0:fbdae7e6d805 10 nth order, with arbitrary values
borlanic 0:fbdae7e6d805 11 the billinear transformation is used for s -> z
borlanic 0:fbdae7e6d805 12 reseting the filter only makes sence for static signals, whatch out if you're using the differnetiator
borlanic 0:fbdae7e6d805 13 */
borlanic 0:fbdae7e6d805 14
borlanic 0:fbdae7e6d805 15 // G(s) = s/(T*s + 1)
borlanic 0:fbdae7e6d805 16 IIR_filter::IIR_filter(float T, float Ts){
borlanic 0:fbdae7e6d805 17
borlanic 0:fbdae7e6d805 18 // filter orders
borlanic 0:fbdae7e6d805 19 nb = 1; // Filter Order
borlanic 0:fbdae7e6d805 20 na = 1; // Filter Order
borlanic 0:fbdae7e6d805 21
borlanic 0:fbdae7e6d805 22 // filter coefficients
borlanic 0:fbdae7e6d805 23 B = (double*)malloc((nb+1)*sizeof(double));
borlanic 0:fbdae7e6d805 24 A = (double*)malloc(na*sizeof(double));
borlanic 0:fbdae7e6d805 25 B[0] = 2.0/(2.0*(double)T + (double)Ts);
borlanic 0:fbdae7e6d805 26 B[1] = -B[0];
borlanic 0:fbdae7e6d805 27 A[0] = -(2.0*(double)T - (double)Ts)/(2.0*(double)T + (double)Ts);
borlanic 0:fbdae7e6d805 28
borlanic 0:fbdae7e6d805 29 // signal arrays
borlanic 0:fbdae7e6d805 30 uk = (double*)malloc((nb+1)*sizeof(double));
borlanic 0:fbdae7e6d805 31 yk = (double*)malloc(na*sizeof(double));
borlanic 0:fbdae7e6d805 32 uk[0]= uk[1] = 0.0;
borlanic 0:fbdae7e6d805 33 yk[0] = 0.0;
borlanic 0:fbdae7e6d805 34
borlanic 0:fbdae7e6d805 35 // dc-gain
borlanic 0:fbdae7e6d805 36 this->K = 0.0;
borlanic 0:fbdae7e6d805 37 }
borlanic 0:fbdae7e6d805 38
borlanic 0:fbdae7e6d805 39 // G(s) = K/(T*s + 1)
borlanic 0:fbdae7e6d805 40 IIR_filter::IIR_filter(float T, float Ts, float K){
borlanic 0:fbdae7e6d805 41
borlanic 0:fbdae7e6d805 42 // filter orders
borlanic 0:fbdae7e6d805 43 nb = 1; // Filter Order
borlanic 0:fbdae7e6d805 44 na = 1; // Filter Order
borlanic 0:fbdae7e6d805 45
borlanic 0:fbdae7e6d805 46 // filter coefficients
borlanic 0:fbdae7e6d805 47 B = (double*)malloc((nb+1)*sizeof(double));
borlanic 0:fbdae7e6d805 48 A = (double*)malloc(na*sizeof(double));
borlanic 0:fbdae7e6d805 49 B[0] = (double)Ts/((double)Ts + 2.0*(double)T);
borlanic 0:fbdae7e6d805 50 B[1] = B[0];
borlanic 0:fbdae7e6d805 51 A[0] = ((double)Ts - 2.0*(double)T)/((double)Ts + 2.0*(double)T);
borlanic 0:fbdae7e6d805 52
borlanic 0:fbdae7e6d805 53 // signal arrays
borlanic 0:fbdae7e6d805 54 uk = (double*)malloc((nb+1)*sizeof(double));
borlanic 0:fbdae7e6d805 55 yk = (double*)malloc(na*sizeof(double));
borlanic 0:fbdae7e6d805 56 uk[0]= uk[1] = 0.0;
borlanic 0:fbdae7e6d805 57 yk[0] = 0.0;
borlanic 0:fbdae7e6d805 58
borlanic 0:fbdae7e6d805 59 // dc-gain
borlanic 0:fbdae7e6d805 60 this->K = (double)K;
borlanic 0:fbdae7e6d805 61 }
borlanic 0:fbdae7e6d805 62
borlanic 0:fbdae7e6d805 63 // G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0^2)
borlanic 0:fbdae7e6d805 64 IIR_filter::IIR_filter(float w0, float D, float Ts, float K){
borlanic 0:fbdae7e6d805 65
borlanic 0:fbdae7e6d805 66 // filter orders
borlanic 0:fbdae7e6d805 67 nb = 2; // Filter Order
borlanic 0:fbdae7e6d805 68 na = 2; // Filter Order
borlanic 0:fbdae7e6d805 69
borlanic 0:fbdae7e6d805 70 // filter coefficients
borlanic 0:fbdae7e6d805 71 B = (double*)malloc((nb+1)*sizeof(double));
borlanic 0:fbdae7e6d805 72 A = (double*)malloc(na*sizeof(double));
borlanic 0:fbdae7e6d805 73 double k0 = (double)Ts*(double)Ts*(double)w0*(double)w0;
borlanic 0:fbdae7e6d805 74 double k1 = 4.0*(double)D*(double)Ts*(double)w0;
borlanic 0:fbdae7e6d805 75 double k2 = k0 + k1 + 4.0;
borlanic 0:fbdae7e6d805 76 B[0] = (double)K*k0/k2;
borlanic 0:fbdae7e6d805 77 B[1] = 2.0*B[0];
borlanic 0:fbdae7e6d805 78 B[2] = B[0];
borlanic 0:fbdae7e6d805 79 A[0] = (2.0*k0 - 8.0)/k2;
borlanic 0:fbdae7e6d805 80 A[1] = (k0 - k1 + 4.0)/k2;
borlanic 0:fbdae7e6d805 81
borlanic 0:fbdae7e6d805 82 // signal arrays
borlanic 0:fbdae7e6d805 83 uk = (double*)malloc((nb+1)*sizeof(double));
borlanic 0:fbdae7e6d805 84 yk = (double*)malloc(na*sizeof(double));
borlanic 0:fbdae7e6d805 85 uk[0]= uk[1] = uk[2] = 0.0;
borlanic 0:fbdae7e6d805 86 yk[0] = yk[1] = 0.0;
borlanic 0:fbdae7e6d805 87
borlanic 0:fbdae7e6d805 88 // dc-gain
borlanic 0:fbdae7e6d805 89 this->K = (double)K;
borlanic 0:fbdae7e6d805 90 }
borlanic 0:fbdae7e6d805 91
borlanic 0:fbdae7e6d805 92 IIR_filter::IIR_filter(float *b, float *a, int nb_, int na_){
borlanic 0:fbdae7e6d805 93
borlanic 0:fbdae7e6d805 94 // filter orders
borlanic 0:fbdae7e6d805 95 this->nb = nb_-1; // Filter Order
borlanic 0:fbdae7e6d805 96 this->na = na_; // Filter Order
borlanic 0:fbdae7e6d805 97
borlanic 0:fbdae7e6d805 98 // filter coefficients
borlanic 0:fbdae7e6d805 99 B = (double*)malloc((nb+1)*sizeof(double));
borlanic 0:fbdae7e6d805 100 A = (double*)malloc(na*sizeof(double));
borlanic 0:fbdae7e6d805 101 uk = (double*)malloc((nb+1)*sizeof(double));
borlanic 0:fbdae7e6d805 102 yk = (double*)malloc(na*sizeof(double));
borlanic 0:fbdae7e6d805 103
borlanic 0:fbdae7e6d805 104 for(int k=0;k<=nb;k++){
borlanic 0:fbdae7e6d805 105 B[k]=b[k];
borlanic 0:fbdae7e6d805 106 uk[k]=0.0;
borlanic 0:fbdae7e6d805 107 }
borlanic 0:fbdae7e6d805 108 for(int k=0;k<na;k++){
borlanic 0:fbdae7e6d805 109 A[k] = a[k];
borlanic 0:fbdae7e6d805 110 yk[k] = 0.0;
borlanic 0:fbdae7e6d805 111 }
borlanic 0:fbdae7e6d805 112
borlanic 0:fbdae7e6d805 113 // dc-gain
borlanic 0:fbdae7e6d805 114 this->K = 1.0;
borlanic 0:fbdae7e6d805 115 }
borlanic 0:fbdae7e6d805 116
borlanic 0:fbdae7e6d805 117
borlanic 0:fbdae7e6d805 118 IIR_filter::~IIR_filter() {}
borlanic 0:fbdae7e6d805 119
borlanic 0:fbdae7e6d805 120 void IIR_filter::reset(float val) {
borlanic 0:fbdae7e6d805 121 for(int k=0;k < nb;k++)
borlanic 0:fbdae7e6d805 122 uk[k] = (double)val;
borlanic 0:fbdae7e6d805 123 for(int k=0;k < na;k++)
borlanic 0:fbdae7e6d805 124 yk[k] = (double)val*K;
borlanic 0:fbdae7e6d805 125
borlanic 0:fbdae7e6d805 126 }
borlanic 0:fbdae7e6d805 127
borlanic 0:fbdae7e6d805 128 /*
borlanic 0:fbdae7e6d805 129 the filter is operating as follows:
borlanic 0:fbdae7e6d805 130 (B[0] + B[1]*z^-1 + ... + B[nb]*z^-nb)*U(z) = (1 + A[0]*z^-1 + ... + A[na-1]*z^-na))*Y(z)
borlanic 0:fbdae7e6d805 131 y(n) = B[0]*u(k) + B[1]*u(k-1) + ... + B[nb]*u(k-nb) + ...
borlanic 0:fbdae7e6d805 132 - A[0]*y(k-1) - A[1]*y(k-2) - ... - A[na]*y(n-na)
borlanic 0:fbdae7e6d805 133 */
borlanic 0:fbdae7e6d805 134 float IIR_filter::filter(double input){
borlanic 0:fbdae7e6d805 135 for(int k = nb;k > 0;k--) // shift input values back
borlanic 0:fbdae7e6d805 136 uk[k] = uk[k-1];
borlanic 0:fbdae7e6d805 137 uk[0] = input;
borlanic 0:fbdae7e6d805 138 double ret = 0.0;
borlanic 0:fbdae7e6d805 139 for(int k = 0;k <= nb;k++)
borlanic 0:fbdae7e6d805 140 ret += B[k] * uk[k];
borlanic 0:fbdae7e6d805 141 for(int k = 0;k < na;k++)
borlanic 0:fbdae7e6d805 142 ret -= A[k] * yk[k];
borlanic 0:fbdae7e6d805 143 for(int k = na;k > 1;k--)
borlanic 0:fbdae7e6d805 144 yk[k-1] = yk[k-2];
borlanic 0:fbdae7e6d805 145 yk[0] = ret;
borlanic 0:fbdae7e6d805 146 return (float)ret;
borlanic 0:fbdae7e6d805 147 }
borlanic 0:fbdae7e6d805 148
borlanic 0:fbdae7e6d805 149