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?
User | Revision | Line number | New 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 | } |