Ruprecht Altenburger / Mbed OS mirror_actuator_V1

Mirror actuator for RT2 lab

Dependencies:   FastPWM

Library_Misc/IIR_filter.cpp@12:8ea956a5c1d8, 2021-04-28 (annotated)

Committer:
altb2
Date:
Wed Apr 28 13:10:16 2021 +0000
Revision:
12:8ea956a5c1d8
Template for students

Who changed what in which revision?

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