52 lines
1.5 KiB
C++
52 lines
1.5 KiB
C++
|
//
|
||
|
|
// Created by david on 02.03.2026.
|
||
|
|
//
|
||
|
|
|
||
|
|
#include "iir_filter.h"
|
||
|
|
|
||
|
|
Buf::Buf(size_t N): size(N), n(0) {
|
||
|
|
data.resize(N);
|
||
|
|
data.assign(N, 0.0);
|
||
|
|
}
|
||
|
|
void Buf::push(double val) {
|
||
|
|
data[n] = val;
|
||
|
|
n = (n+1) % size;
|
||
|
|
}
|
||
|
|
|
||
|
|
Filt::Filt(size_t N, size_t shift, size_t offset, std::vector<double> taps): Buf(N), shift(shift), offset(offset), taps(taps) {
|
||
|
|
if (taps.size() != N) throw std::invalid_argument("taps.size() != N");
|
||
|
|
}
|
||
|
|
double Filt::filter(double val) {
|
||
|
|
this->push(val);
|
||
|
|
return this->peek();
|
||
|
|
}
|
||
|
|
double Filt::peek() {
|
||
|
|
double sum = 0;
|
||
|
|
for (size_t i = offset; i < this->size; i++) {
|
||
|
|
//size_t n = (this->n - i + shift - 1) % this->size; // unsigned % size ... bad if u is negative
|
||
|
|
size_t n = (this->size + this->n - i + shift - 1) % this->size;
|
||
|
|
//std::cout << " t[" << i << "] * v[" << n << "]" << std::endl;
|
||
|
|
sum += this->data[n] * this->taps[i];
|
||
|
|
}
|
||
|
|
return sum;
|
||
|
|
}
|
||
|
|
void Filt::push(double val) {
|
||
|
|
Buf::push(val);
|
||
|
|
}
|
||
|
|
|
||
|
|
IirFilter::IirFilter(std::vector<double> b, std::vector<double> a) : x(b.size(), 0, 0, b), y(a.size(), 1, 1, a) {
|
||
|
|
if (b.size() != a.size()) throw std::invalid_argument("b.size() != a.size()");
|
||
|
|
}
|
||
|
|
double IirFilter::filter(double val) {
|
||
|
|
//std::cout << "x.filter(" << val << ")" << std::endl;
|
||
|
|
double xv = x.filter(val);
|
||
|
|
//std::cout << "xv=" << xv << std::endl;
|
||
|
|
//std::cout << "y.peek()" << std::endl;
|
||
|
|
double yv = y.peek();
|
||
|
|
//std::cout << "yv=" << yv << std::endl;
|
||
|
|
//std::cout << "---" << std::endl;
|
||
|
|
double yo = xv - yv;
|
||
|
|
y.push(yo);
|
||
|
|
return yo;
|
||
|
|
}
|