David Madl
90f8943930
* use SSF signal instead of accelerometer signal
* use higher BEAT_CORR_THR_{12} for SSF signal
* add absolute SSF_THRESHOLD to ignore small accelero bumps
* compute ssf_threshold according to detected SSF peaks, not the mean (more robust vs. noise)
67 lines
1.8 KiB
C++
67 lines
1.8 KiB
C++
//
|
|
// Created by david on 02.03.2026.
|
|
//
|
|
|
|
#include "iir_filter.h"
|
|
#include <iostream>
|
|
|
|
#define DEBUG_IIR 0
|
|
|
|
#if (DEBUG_IIR == 1)
|
|
#define DEBUG_PRINT(expr) do { expr; } while (0)
|
|
#else
|
|
#define DEBUG_PRINT(expr) while(0) { expr; }
|
|
#endif
|
|
|
|
Buf::Buf(size_t N): N(N), n(0) {
|
|
data.resize(N);
|
|
data.assign(N, 0.0);
|
|
}
|
|
void Buf::push(double val) {
|
|
data[n] = val;
|
|
n = (n+1) % N;
|
|
}
|
|
|
|
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->N; i++) {
|
|
//size_t n = (this->n - i + shift - 1) % this->size; // unsigned % size ... bad if u is negative
|
|
size_t n = (this->N + this->n - i + shift - 1) % this->N;
|
|
DEBUG_PRINT(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);
|
|
}
|
|
void Filt::prime(double val) {
|
|
data.assign(this->N, val);
|
|
}
|
|
size_t Filt::size() {
|
|
return this->N;
|
|
}
|
|
|
|
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) {
|
|
DEBUG_PRINT(std::cout << "x.filter(" << val << ")" << std::endl);
|
|
double xv = x.filter(val);
|
|
DEBUG_PRINT(std::cout << "xv=" << xv << std::endl);
|
|
DEBUG_PRINT(std::cout << "y.peek()" << std::endl);
|
|
double yv = y.peek();
|
|
DEBUG_PRINT(std::cout << "yv=" << yv << std::endl);
|
|
DEBUG_PRINT(std::cout << "---" << std::endl);
|
|
double yo = xv - yv;
|
|
y.push(yo);
|
|
return yo;
|
|
}
|