Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Make the filter state instance data #4

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Make the filter state instance data #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
191 changes: 81 additions & 110 deletions EMGFilters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,131 +60,102 @@ static float ahf_denominator_coef_60Hz[2][6] = {
{1.0000, -1.8407, 0.9894, 1.0000, -1.8584, 0.9891}};
static float ahf_output_gain_coef_60Hz[2] = {1.3430, 1.4206};

enum FILTER_TYPE {
FILTER_TYPE_LOWPASS = 0,
FILTER_TYPE_HIGHPASS,
};

class FILTER_2nd {
private:
// second-order filter
float states[2];
float num[3];
float den[3];

public:
void init(FILTER_TYPE ftype, int sampleFreq) {
states[0] = 0;
states[1] = 0;
if (ftype == FILTER_TYPE_LOWPASS) {
// 2th order butterworth lowpass filter
// cutoff frequency 150Hz
if (sampleFreq == SAMPLE_FREQ_500HZ) {
for (int i = 0; i < 3; i++) {
num[i] = lpf_numerator_coef[0][i];
den[i] = lpf_denominator_coef[0][i];
}
} else if (sampleFreq == SAMPLE_FREQ_1000HZ) {
for (int i = 0; i < 3; i++) {
num[i] = lpf_numerator_coef[1][i];
den[i] = lpf_denominator_coef[1][i];
}
void FILTER_2nd::init(FILTER_TYPE ftype, int sampleFreq) {
states[0] = 0;
states[1] = 0;
if (ftype == FILTER_TYPE_LOWPASS) {
// 2th order butterworth lowpass filter
// cutoff frequency 150Hz
if (sampleFreq == SAMPLE_FREQ_500HZ) {
for (int i = 0; i < 3; i++) {
num[i] = lpf_numerator_coef[0][i];
den[i] = lpf_denominator_coef[0][i];
}
} else if (ftype == FILTER_TYPE_HIGHPASS) {
// 2th order butterworth
// cutoff frequency 20Hz
if (sampleFreq == SAMPLE_FREQ_500HZ) {
for (int i = 0; i < 3; i++) {
num[i] = hpf_numerator_coef[0][i];
den[i] = hpf_denominator_coef[0][i];
}
} else if (sampleFreq == SAMPLE_FREQ_1000HZ) {
for (int i = 0; i < 3; i++) {
num[i] = hpf_numerator_coef[1][i];
den[i] = hpf_denominator_coef[1][i];
}
} else if (sampleFreq == SAMPLE_FREQ_1000HZ) {
for (int i = 0; i < 3; i++) {
num[i] = lpf_numerator_coef[1][i];
den[i] = lpf_denominator_coef[1][i];
}
}
} else if (ftype == FILTER_TYPE_HIGHPASS) {
// 2th order butterworth
// cutoff frequency 20Hz
if (sampleFreq == SAMPLE_FREQ_500HZ) {
for (int i = 0; i < 3; i++) {
num[i] = hpf_numerator_coef[0][i];
den[i] = hpf_denominator_coef[0][i];
}
} else if (sampleFreq == SAMPLE_FREQ_1000HZ) {
for (int i = 0; i < 3; i++) {
num[i] = hpf_numerator_coef[1][i];
den[i] = hpf_denominator_coef[1][i];
}
}
}
}

float FILTER_2nd::update(float input) {
float tmp = (input - den[1] * states[0] - den[2] * states[1]) / den[0];
float output = num[0] * tmp + num[1] * states[0] + num[2] * states[1];
// save last states
states[1] = states[0];
states[0] = tmp;
return output;
}

float update(float input) {
float tmp = (input - den[1] * states[0] - den[2] * states[1]) / den[0];
float output = num[0] * tmp + num[1] * states[0] + num[2] * states[1];
// save last states
states[1] = states[0];
states[0] = tmp;
return output;
void FILTER_4th::init(int sampleFreq, int humFreq) {
gain = 0;
for (int i = 0; i < 4; i++) {
states[i] = 0;
}
};

class FILTER_4th {
private:
// fourth-order filter
// cascade two 2nd-order filters
float states[4];
float num[6];
float den[6];
float gain;

public:
void init(int sampleFreq, int humFreq) {
gain = 0;
for (int i = 0; i < 4; i++) {
states[i] = 0;
if (humFreq == NOTCH_FREQ_50HZ) {
if (sampleFreq == SAMPLE_FREQ_500HZ) {
for (int i = 0; i < 6; i++) {
num[i] = ahf_numerator_coef_50Hz[0][i];
den[i] = ahf_denominator_coef_50Hz[0][i];
}
gain = ahf_output_gain_coef_50Hz[0];
} else if (sampleFreq == SAMPLE_FREQ_1000HZ) {
for (int i = 0; i < 6; i++) {
num[i] = ahf_numerator_coef_50Hz[1][i];
den[i] = ahf_denominator_coef_50Hz[1][i];
}
gain = ahf_output_gain_coef_50Hz[1];
}
if (humFreq == NOTCH_FREQ_50HZ) {
if (sampleFreq == SAMPLE_FREQ_500HZ) {
for (int i = 0; i < 6; i++) {
num[i] = ahf_numerator_coef_50Hz[0][i];
den[i] = ahf_denominator_coef_50Hz[0][i];
}
gain = ahf_output_gain_coef_50Hz[0];
} else if (sampleFreq == SAMPLE_FREQ_1000HZ) {
for (int i = 0; i < 6; i++) {
num[i] = ahf_numerator_coef_50Hz[1][i];
den[i] = ahf_denominator_coef_50Hz[1][i];
}
gain = ahf_output_gain_coef_50Hz[1];
} else if (humFreq == NOTCH_FREQ_60HZ) {
if (sampleFreq == SAMPLE_FREQ_500HZ) {
for (int i = 0; i < 6; i++) {
num[i] = ahf_numerator_coef_60Hz[0][i];
den[i] = ahf_denominator_coef_60Hz[0][i];
}
} else if (humFreq == NOTCH_FREQ_60HZ) {
if (sampleFreq == SAMPLE_FREQ_500HZ) {
for (int i = 0; i < 6; i++) {
num[i] = ahf_numerator_coef_60Hz[0][i];
den[i] = ahf_denominator_coef_60Hz[0][i];
}
gain = ahf_output_gain_coef_60Hz[0];
} else if (sampleFreq == SAMPLE_FREQ_1000HZ) {
for (int i = 0; i < 6; i++) {
num[i] = ahf_numerator_coef_60Hz[1][i];
den[i] = ahf_denominator_coef_60Hz[1][i];
}
gain = ahf_output_gain_coef_60Hz[1];
gain = ahf_output_gain_coef_60Hz[0];
} else if (sampleFreq == SAMPLE_FREQ_1000HZ) {
for (int i = 0; i < 6; i++) {
num[i] = ahf_numerator_coef_60Hz[1][i];
den[i] = ahf_denominator_coef_60Hz[1][i];
}
gain = ahf_output_gain_coef_60Hz[1];
}
}
}

float update(float input) {
float output;
float stageIn;
float stageOut;
float FILTER_4th::update(float input) {
float output;
float stageIn;
float stageOut;

stageOut = num[0] * input + states[0];
states[0] = (num[1] * input + states[1]) - den[1] * stageOut;
states[1] = num[2] * input - den[2] * stageOut;
stageIn = stageOut;
stageOut = num[3] * stageOut + states[2];
states[2] = (num[4] * stageIn + states[3]) - den[4] * stageOut;
states[3] = num[5] * stageIn - den[5] * stageOut;
stageOut = num[0] * input + states[0];
states[0] = (num[1] * input + states[1]) - den[1] * stageOut;
states[1] = num[2] * input - den[2] * stageOut;
stageIn = stageOut;
stageOut = num[3] * stageOut + states[2];
states[2] = (num[4] * stageIn + states[3]) - den[4] * stageOut;
states[3] = num[5] * stageIn - den[5] * stageOut;

output = gain * stageOut;
output = gain * stageOut;

return output;
}
};

FILTER_2nd LPF;
FILTER_2nd HPF;
FILTER_4th AHF;
return output;
}

void EMGFilters::init(SAMPLE_FREQUENCY sampleFreq,
NOTCH_FREQUENCY notchFreq,
Expand Down
36 changes: 36 additions & 0 deletions EMGFilters.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,39 @@ enum NOTCH_FREQUENCY { NOTCH_FREQ_50HZ = 50, NOTCH_FREQ_60HZ = 60 };

enum SAMPLE_FREQUENCY { SAMPLE_FREQ_500HZ = 500, SAMPLE_FREQ_1000HZ = 1000 };

enum FILTER_TYPE {
FILTER_TYPE_LOWPASS = 0,
FILTER_TYPE_HIGHPASS,
};

class FILTER_2nd {
private:
// second-order filter
float states[2];
float num[3];
float den[3];

public:
void init(FILTER_TYPE ftype, int sampleFreq);

float update(float input);
};

class FILTER_4th {
private:
// fourth-order filter
// cascade two 2nd-order filters
float states[4];
float num[6];
float den[6];
float gain;

public:
void init(int sampleFreq, int humFreq);

float update(float input);
};

// \brief EMGFilter provides an anti-hum notch filter to filter out 50HZ or
// 60HZ power line noise, a lowpass filter to filter out signals above
// 150HZ, and a highpass filter to filter out noise below 20HZ;
Expand Down Expand Up @@ -71,6 +104,9 @@ class EMGFilters {
bool m_notchFilterEnabled;
bool m_lowpassFilterEnabled;
bool m_highpassFilterEnabled;
FILTER_2nd LPF;
FILTER_2nd HPF;
FILTER_4th AHF;
};

#endif