Mirjam Bos / BiQuad4th_order

Function to design a 4th order BiQuad filter

Dependents:   Project_EMGtosetpoint emg_calibration emg_calibration demo_with_emg ... more

BiQuad4.cpp@0:caa20b96f300, 2018-10-22 (annotated)

Committer:
Mirjam
Date:
Mon Oct 22 09:11:21 2018 +0000
Revision:
0:caa20b96f300
Child:
1:ac4bef72d6af
Class to make a 4th order BiQuad

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mirjam 0:caa20b96f300 1 #include "BiQuad4.h"
Mirjam 0:caa20b96f300 2 #include <stdlib.h>
Mirjam 0:caa20b96f300 3 #include <stddef.h>
Mirjam 0:caa20b96f300 4
Mirjam 0:caa20b96f300 5 BiQuad4::BiQuad4(double b0, double b1, double b2, double b3, double b4, double a1, double a2, double a3, double a4) {
Mirjam 0:caa20b96f300 6 set( b0, b1, b2, b3, b4, a1, a2, a3, a4 );
Mirjam 0:caa20b96f300 7 resetStateOnGainChange = true;
Mirjam 0:caa20b96f300 8 }
Mirjam 0:caa20b96f300 9
Mirjam 0:caa20b96f300 10 void BiQuad4::set(double b0, double b1, double b2, double b3, double b4, double a1, double a2, double a3, double a4) {
Mirjam 0:caa20b96f300 11
Mirjam 0:caa20b96f300 12 B[0] = b0; B[1] = b1; B[2] = b2; B[3] = b3; B[4] = b4;
Mirjam 0:caa20b96f300 13 A[0] = a1; A[1] = a2; A[2] = a3; A[3] = a4;
Mirjam 0:caa20b96f300 14
Mirjam 0:caa20b96f300 15 if( resetStateOnGainChange )
Mirjam 0:caa20b96f300 16 wz[0] = 0; wz[1] = 0;
Mirjam 0:caa20b96f300 17 }
Mirjam 0:caa20b96f300 18
Mirjam 0:caa20b96f300 19 double BiQuad4::step(double x) {
Mirjam 0:caa20b96f300 20
Mirjam 0:caa20b96f300 21 double y,w;
Mirjam 0:caa20b96f300 22
Mirjam 0:caa20b96f300 23 /* Direct form II */
Mirjam 0:caa20b96f300 24 w = x - A[0]*wz[0] - A[1]*wz[1]- A[2]*wz[2] - A[3]*wz[3];
Mirjam 0:caa20b96f300 25 y = B[0]*w + B[1]*wz[0] + B[2]*wz[1] + B[3]*wz[2] + B[4]*wz[3];
Mirjam 0:caa20b96f300 26
Mirjam 0:caa20b96f300 27 /* Shift */
Mirjam 0:caa20b96f300 28 wz[3] = wz[2];
Mirjam 0:caa20b96f300 29 wz[2] = wz[1];
Mirjam 0:caa20b96f300 30 wz[1] = wz[0];
Mirjam 0:caa20b96f300 31 wz[0] = w;
Mirjam 0:caa20b96f300 32
Mirjam 0:caa20b96f300 33 return y;
Mirjam 0:caa20b96f300 34
Mirjam 0:caa20b96f300 35 }