improve lowpass filter with a higher order one. (#463)
* improve lowpass filter with a higer order one. * Update test_sampling.cc * remove the unneccerary throw in the code
This commit is contained in:
Wenbing Li
GitHub
Родитель
8f36cf3272
Коммит
507358545d
|
|
@ -6,52 +6,161 @@
|
|||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <complex>
|
||||
#include <algorithm>
|
||||
#include "narrow.h"
|
||||
|
||||
// https://en.wikipedia.org/wiki/Butterworth_filter
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
// http://www.dspguide.com/CH33.PDF, p625
|
||||
class ButterworthLowpass {
|
||||
private:
|
||||
static constexpr int kFilterOrder = 4;
|
||||
|
||||
// public for test
|
||||
public:
|
||||
ButterworthLowpass(float sample_rate, float cutoff_frequency)
|
||||
: x_prev_(0.0f), y_prev_(0.0f) {
|
||||
float RC = 1.0f / (2.0f * 3.14159265359f * cutoff_frequency);
|
||||
float dt = 1.0f / sample_rate;
|
||||
float alpha = dt / (RC + dt);
|
||||
a0_ = alpha;
|
||||
a1_ = alpha;
|
||||
b1_ = 1 - alpha;
|
||||
static void OnePoleCoefs(double pole_n, double np, double fc, double a[3], double b[3]) {
|
||||
double rp, ip, t, w, m, d, x0, x1, x2, y1, y2, k;
|
||||
|
||||
// Calculate pole location on unit circle
|
||||
rp = -cos(M_PI / (np * 2.0) + (pole_n - 1.0) * M_PI / np);
|
||||
ip = sin(M_PI / (np * 2.0) + (pole_n - 1.0) * M_PI / np);
|
||||
|
||||
// s-domain to z-domain conversion
|
||||
t = 2.0 * tan(0.5);
|
||||
w = 2.0 * M_PI * fc;
|
||||
m = rp * rp + ip * ip;
|
||||
d = 4.0 - 4.0 * rp * t + m * t * t;
|
||||
x0 = t * t / d;
|
||||
x1 = 2.0 * t * t / d;
|
||||
x2 = t * t / d;
|
||||
y1 = (8.0 - 2.0 * m * t * t) / d;
|
||||
y2 = (-4.0 - 4.0 * rp * t - m * t * t) / d;
|
||||
|
||||
// LP TO LP, or LP TO HP
|
||||
k = sin(0.5 - w / 2.0) / sin(0.5 + w / 2.0);
|
||||
|
||||
d = 1.0 + y1 * k - y2 * k * k;
|
||||
|
||||
a[0] = (x0 - x1 * k + x2 * k * k) / d;
|
||||
a[1] = (-2.0 * x0 * k + x1 + x1 * k * k - 2.0 * x2 * k) / d;
|
||||
a[2] = (x0 * k * k - x1 * k + x2) / d;
|
||||
b[1] = (2.0 * k + y1 + y1 * k * k - 2.0 * y2 * k) / d;
|
||||
b[2] = (-k * k - y1 * k + y2) / d;
|
||||
}
|
||||
|
||||
float Process(float input) {
|
||||
float output = a0_ * input + a1_ * x_prev_ - b1_ * y_prev_;
|
||||
x_prev_ = input;
|
||||
y_prev_ = output;
|
||||
void CalculateCoefs(std::vector<double>& num, std::vector<double>& den, size_t num_pole, double cutoff_freq) {
|
||||
const size_t POLE_DATA_SIZE = 3;
|
||||
num.resize(kFilterOrder + 1);
|
||||
den.resize(kFilterOrder + 1);
|
||||
|
||||
const size_t pole_buff_size = num_pole + POLE_DATA_SIZE;
|
||||
|
||||
std::vector<double> a(pole_buff_size), b(pole_buff_size), ta(pole_buff_size), tb(pole_buff_size);
|
||||
std::array<double, POLE_DATA_SIZE> ap{}, bp{};
|
||||
double sa{}, sb{}, gain{};
|
||||
|
||||
a[POLE_DATA_SIZE - 1] = 1.0;
|
||||
b[POLE_DATA_SIZE - 1] = 1.0;
|
||||
|
||||
for (auto p = 1; p <= num_pole / 2; ++p) {
|
||||
OnePoleCoefs(p, static_cast<double>(num_pole), cutoff_freq, ap.data(), bp.data());
|
||||
|
||||
std::copy(a.begin(), a.end(), ta.begin());
|
||||
std::copy(b.begin(), b.end(), tb.begin());
|
||||
|
||||
for (auto i = POLE_DATA_SIZE - 1; i <= num_pole + 2; ++i) {
|
||||
a[i] = ap[0] * ta[i] + ap[1] * ta[i - 1] + ap[2] * ta[i - 2];
|
||||
b[i] = tb[i] - bp[1] * tb[i - 1] - bp[2] * tb[i - 2];
|
||||
}
|
||||
}
|
||||
|
||||
b[POLE_DATA_SIZE - 1] = 0.0;
|
||||
|
||||
for (auto i = 0; i <= num_pole; ++i) {
|
||||
a[i] = a[i + 2];
|
||||
b[i] = -b[i + 2];
|
||||
}
|
||||
|
||||
for (auto i = 0; i <= num_pole; ++i) {
|
||||
sa += a[i];
|
||||
sb += b[i];
|
||||
}
|
||||
|
||||
gain = sa / (1.0 - sb);
|
||||
|
||||
for (auto i = 0; i <= num_pole; ++i)
|
||||
a[i] /= gain;
|
||||
|
||||
for (auto i = 0; i <= num_pole; ++i) {
|
||||
num[i] = a[i];
|
||||
den[i] = -b[i];
|
||||
}
|
||||
|
||||
den[0] = 1.0;
|
||||
}
|
||||
|
||||
|
||||
std::vector<double> coefs_a_;
|
||||
std::vector<double> coefs_b_;
|
||||
|
||||
public:
|
||||
ButterworthLowpass(double cutoff_freq, double sampling_rate) {
|
||||
auto normalized_cutoff = cutoff_freq / sampling_rate;
|
||||
CalculateCoefs(coefs_b_, coefs_a_, kFilterOrder, normalized_cutoff);
|
||||
}
|
||||
|
||||
const std::vector<double>& GetCoefs_A() {
|
||||
return coefs_a_;
|
||||
}
|
||||
|
||||
const std::vector<double>& GetCoefs_B() {
|
||||
return coefs_b_;
|
||||
}
|
||||
|
||||
std::vector<float> Process(const std::vector<float>& input) {
|
||||
std::vector<float> output(input.size(), 0.0);
|
||||
|
||||
// Initialize delay elements
|
||||
double x_n_1 = 0.0, x_n_2 = 0.0, x_n_3 = 0.0, x_n_4 = 0.0;
|
||||
double y_n_1 = 0.0, y_n_2 = 0.0, y_n_3 = 0.0, y_n_4 = 0.0;
|
||||
|
||||
for (size_t i = 0; i < input.size(); ++i) {
|
||||
double x = input[i];
|
||||
|
||||
// Compute the output
|
||||
double y = coefs_b_[0] * x + coefs_b_[1] * x_n_1 + coefs_b_[2] * x_n_2 + coefs_b_[3] * x_n_3 + coefs_b_[4] * x_n_4
|
||||
- coefs_a_[1] * y_n_1 - coefs_a_[2] * y_n_2 - coefs_a_[3] * y_n_3 - coefs_a_[4] * y_n_4;
|
||||
|
||||
// Shuffle old input and output values
|
||||
x_n_4 = x_n_3;
|
||||
x_n_3 = x_n_2;
|
||||
x_n_2 = x_n_1;
|
||||
x_n_1 = x;
|
||||
|
||||
y_n_4 = y_n_3;
|
||||
y_n_3 = y_n_2;
|
||||
y_n_2 = y_n_1;
|
||||
y_n_1 = y;
|
||||
|
||||
output[i] = static_cast<float>(y);
|
||||
}
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
std::vector<float> Process(const std::vector<float>& inputSignal) {
|
||||
std::vector<float> outputSignal(inputSignal.size());
|
||||
for (size_t i = 0; i < inputSignal.size(); ++i) {
|
||||
outputSignal[i] = Process(inputSignal[i]);
|
||||
}
|
||||
return outputSignal;
|
||||
}
|
||||
|
||||
private:
|
||||
float x_prev_, y_prev_;
|
||||
float a0_, a1_, b1_;
|
||||
};
|
||||
|
||||
// https://ccrma.stanford.edu/~jos/sasp/Kaiser_Window.html
|
||||
class KaiserWindowInterpolation {
|
||||
private:
|
||||
// Kaiser window parameters, empirically
|
||||
constexpr static double kBeta = 6.0; // Beta controls the width of the transition band
|
||||
static constexpr double kBeta = 6.0; // Beta controls the width of the transition band
|
||||
|
||||
public:
|
||||
static void Process(const std::vector<float>& input, std::vector<float>& output, float inputSampleRate, float outputSampleRate) {
|
||||
// Downsampling factor
|
||||
float factor = outputSampleRate / inputSampleRate;
|
||||
const double MY_PI = 3.14159265359;
|
||||
|
||||
// Calculate the number of output samples
|
||||
int outputSize = static_cast<int>(std::ceil(static_cast<float>(input.size()) * factor));
|
||||
|
|
@ -65,7 +174,7 @@ class KaiserWindowInterpolation {
|
|||
float fractionalPart = index - integerPart;
|
||||
|
||||
// Calculate the range of input samples for interpolation
|
||||
int range = static_cast<int>(std::ceil(kBeta / (2.0f * factor)));
|
||||
int range = static_cast<int>(std::ceil(kBeta / (2.0 * factor)));
|
||||
int startSample = std::max(0, integerPart - range);
|
||||
int endSample = std::min(static_cast<int>(input.size()) - 1, integerPart + range);
|
||||
|
||||
|
|
@ -73,7 +182,7 @@ class KaiserWindowInterpolation {
|
|||
std::vector<double> weights = KaiserWin(static_cast<size_t>(endSample - startSample + 1));
|
||||
for (int j = startSample; j <= endSample; j++) {
|
||||
double distance = std::abs(j - index);
|
||||
double sincValue = (distance < 1e-6f) ? 1.0f : std::sin(MY_PI * distance) / (MY_PI * distance);
|
||||
double sincValue = (distance < 1e-6f) ? 1.0f : std::sin(M_PI * distance) / (M_PI * distance);
|
||||
weights[j - startSample] *= sincValue;
|
||||
}
|
||||
|
||||
|
|
@ -93,7 +202,7 @@ class KaiserWindowInterpolation {
|
|||
double sum = 0.0;
|
||||
double term = 1.0;
|
||||
double x_squared = x * x / 4.0;
|
||||
int n = 0;
|
||||
size_t n = 0;
|
||||
double tolerance = 1e-8;
|
||||
|
||||
while (term > tolerance * sum) {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,44 @@
|
|||
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||||
// Licensed under the MIT License.
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "audio/sampling.h"
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
|
||||
// The expected test result was generated by the following Python library
|
||||
// b, a = scipy.signal.butter(4, 8000/(441000/2), btype='low', analog=False)
|
||||
TEST(ButterworthLowpassTest, CalculateCoefsTest) {
|
||||
std::vector<double> expected_b = {
|
||||
9.12788e-06,
|
||||
3.65115e-05,
|
||||
5.47673e-05,
|
||||
3.65115e-05,
|
||||
9.12788e-06};
|
||||
std::vector<double> expected_a = {1.0, -3.70223, 5.15023, -3.19013, 0.742275};
|
||||
|
||||
ButterworthLowpass filt(8000, 441000);
|
||||
ASSERT_EQ(filt.GetCoefs_B().size(), expected_b.size()) << "Coefs-B result and expected are of unequal length";
|
||||
ASSERT_EQ(filt.GetCoefs_A().size(), expected_a.size()) << "Coefs-A result and expected are of unequal length";
|
||||
for (size_t i = 0; i < expected_b.size(); ++i) {
|
||||
EXPECT_NEAR(expected_b[i], filt.GetCoefs_B().at(i), 1e-5) << "Coefs-B result and expected differ at index " << i;
|
||||
EXPECT_NEAR(expected_a[i], filt.GetCoefs_A().at(i), 1e-5) << "Coefs-A result and expected differ at index " << i;
|
||||
}
|
||||
}
|
||||
|
||||
// The expected test result was generated by the following Python library
|
||||
// data = np.asarray([1, 2, 3, 4, 5, 6, 7, 8, 9, 10], dtype=np.float32)
|
||||
// scipy.signal.lfilter(b, a, data)
|
||||
TEST(ButterworthLowpassTest, FilterTest) {
|
||||
std::vector<float> signal = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
|
||||
std::vector<double> expected = {9.1278764060962e-06, 8.856077402703055e-05, 0.00043603576325860837,
|
||||
0.0014794083160631888, 0.00394531110456504, 0.008896621026975655,
|
||||
0.01774416762114977, 0.032237572808636775, 0.05443959732833486, 0.08668788318758487};
|
||||
|
||||
ButterworthLowpass filt(8000, 441000);
|
||||
std::vector<float> actual = filt.Process(signal);
|
||||
EXPECT_EQ(expected.size(), actual.size());
|
||||
for (size_t i = 0; i < actual.size(); i++) {
|
||||
EXPECT_NEAR(expected[i], actual[i], 1e-05);
|
||||
}
|
||||
}
|
||||
Загрузка…
Ссылка в новой задаче