Function to design a 4th order BiQuad filter
Dependents: Project_EMGtosetpoint emg_calibration emg_calibration demo_with_emg ... more
BiQuad4.cpp
- Committer:
- Mirjam
- Date:
- 2018-10-22
- Revision:
- 0:caa20b96f300
- Child:
- 1:ac4bef72d6af
File content as of revision 0:caa20b96f300:
#include "BiQuad4.h" #include <stdlib.h> #include <stddef.h> BiQuad4::BiQuad4(double b0, double b1, double b2, double b3, double b4, double a1, double a2, double a3, double a4) { set( b0, b1, b2, b3, b4, a1, a2, a3, a4 ); resetStateOnGainChange = true; } void BiQuad4::set(double b0, double b1, double b2, double b3, double b4, double a1, double a2, double a3, double a4) { B[0] = b0; B[1] = b1; B[2] = b2; B[3] = b3; B[4] = b4; A[0] = a1; A[1] = a2; A[2] = a3; A[3] = a4; if( resetStateOnGainChange ) wz[0] = 0; wz[1] = 0; } double BiQuad4::step(double x) { double y,w; /* Direct form II */ w = x - A[0]*wz[0] - A[1]*wz[1]- A[2]*wz[2] - A[3]*wz[3]; y = B[0]*w + B[1]*wz[0] + B[2]*wz[1] + B[3]*wz[2] + B[4]*wz[3]; /* Shift */ wz[3] = wz[2]; wz[2] = wz[1]; wz[1] = wz[0]; wz[0] = w; return y; }