altb_pmic / Lib_Cntrl

Control Library by altb

Dependents:   My_Libraries IndNav_QK3_T265

IIR_filter.cpp@15:c70cad2f4e64, 2020-01-16 (annotated)

Committer:
pmic
Date:
Thu Jan 16 09:12:50 2020 +0000
Revision:
15:c70cad2f4e64
Parent:
14:9184aa9fdac7 
Revisit IIR_filter.h and IIR_filter.cpp. Change internal double to float arithmetic.

Who changed what in which revision?

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