Boulus Abu Joudom / Mbed 2 deprecated Cube_Mini_Solution

Cube Mini Solution

Dependencies:   mbed QEI MPU6050 BLE_API nRF51822 MCP4725 eMPL_MPU6050

IIR_filter.cpp@24:c953b74ed88b, 2020-08-28 (annotated)

Committer:
BoulusAJ
Date:
Fri Aug 28 09:23:40 2020 +0000
Revision:
24:c953b74ed88b
Parent:
0:8e87cdf07037 
Prototype 2 - August 28, 2020.; Controller working well with updated parameters; State machine still needs updates

Who changed what in which revision?

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