Вот как я реализовал встроенную в MATLAB функцию filter
в C ++ некоторое время назад, если кому-то это нужно.В Zi
вы проходите начальные условия и собираете конечные условия, если это необходимо.
#include <vector>
#include <exception>
#include <algorithm>
typedef vector<double> vectord;
void filter(vectord B, vectord A, const vectord &X, vectord &Y, vectord &Zi)
{
if (A.empty())
throw std::domain_error("The feedback filter coefficients are empty.");
if (std::all_of(A.begin(), A.end(), [](double coef){ return coef == 0; }))
throw std::domain_error("At least one of the feedback filter coefficients has to be non-zero.");
if (A[0] == 0)
throw std::domain_error("First feedback coefficient has to be non-zero.");
// Normalize feedback coefficients if a[0] != 1;
auto a0 = A[0];
if (a0 != 1.0)
{
std::transform(A.begin(), A.end(), A.begin(), [a0](double v) { return v / a0; });
std::transform(B.begin(), B.end(), B.begin(), [a0](double v) { return v / a0; });
}
size_t input_size = X.size();
size_t filter_order = std::max(A.size(), B.size());
B.resize(filter_order, 0);
A.resize(filter_order, 0);
Zi.resize(filter_order, 0);
Y.resize(input_size);
for (size_t i = 0; i < input_size; ++i)
{
size_t order = filter_order - 1;
while (order)
{
if (i >= order)
Zi[order - 1] = B[order] * X[i - order] - A[order] * Y[i - order] + Zi[order];
--order;
}
Y[i] = B[0] * X[i] + Zi[0];
}
Zi.resize(filter_order - 1);
}
Не стесняйтесь проверить это с помощью этого кода:
TEST_METHOD(TestFilter)
{
vectord b_coeff = { /* Initialise your feedforward coefficients here */ };
vectord a_coeff = { /* Initialise your feedback coefficients here */ };
vectord input_signal = { /* Some input data to be filtered */ };
vectord y_filter_ori = { /* MATLAB output from calling y_filter_ori = filter(b_coeff, a_coeff, input_signal); */ };
vectord y_filter_out; vectord zi = { 0 }; // Set empty initial conditions
filter(b_coeff, a_coeff, input_signal, y_filter_out, zi);
Assert::IsTrue(compare(y_filter_out, y_filter_ori, 0.0001));
}
bool compare(const vectord &original, const vectord &expected, double tolerance = DBL_EPSILON)
{
if (original.size() != expected.size())
return false;
size_t size = original.size();
for (size_t i = 0; i < size; ++i)
{
auto diff = abs(original[i] - expected[i]);
if (diff >= tolerance)
return false;
}
return true;
}