david/libpasada
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: SSF step detector

Browse Source
This commit is contained in:
David Madl
2026-03-03 00:33:03 +01:00
parent c5acce5699
commit 40e10c1718
4 changed files with 268 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

157
google-tests/test2.cpp
View File

@@ -5,15 +5,30 @@
#include "npy.hpp"
#include <vector>
#include "iir_filter.h"
#include "ssf_filter.h"
#include <cmath>
#include <limits>
TEST(HelloTest, Zong_SSF_Stage1) {
npy::npy_data acc = npy::read_npy<double>("test2/ssf_t2_acc.npy");
npy::npy_data y_ref = npy::read_npy<double>("test2/ssf_t2_y_ref.npy");
#define FPS 60
#define MAX_BPM 300
//
// fetch y axis
//
template <typename T> static std::vector<double> apply_filter(T& filter, std::vector<double>& x) {
std::vector<double> y;
y.resize(x.size());
for (int i = 0; i < x.size(); i++) {
y[i] = filter.filter(x[i]);
}
return y;
}
static void npy_save(std::string path, std::vector<double>& x) {
npy::npy_data_ptr<double> d;
d.data_ptr = x.data();
d.shape = {(unsigned long) x.size()};
npy::write_npy(path, d);
}
static std::vector<double> fetch_y_axis(npy::npy_data<double>& acc) {
// TODO: later on, we should use a vector projection towards gravity
std::vector<double> signal;
const size_t rows_real = acc.shape[0];
@@ -38,6 +53,16 @@ TEST(HelloTest, Zong_SSF_Stage1) {
for (int i = 0; i < rows; i++) {
signal[i] = acc.data[i * stride + offset];
}
return signal;
}
TEST(HelloTest, Zong_SSF_Stage1) {
npy::npy_data acc = npy::read_npy<double>("test2/ssf_t2_acc.npy");
npy::npy_data y_ref = npy::read_npy<double>("test2/ssf_t2_y_ref.npy");
std::vector<double> signal = fetch_y_axis(acc);
const size_t N = signal.size();
ASSERT_NEAR(1.7, signal[0], 1e-5);
ASSERT_NEAR(3.6, signal[1], 1e-5);
ASSERT_NEAR(4.3, signal[2], 1e-5);
@@ -46,6 +71,10 @@ TEST(HelloTest, Zong_SSF_Stage1) {
// # but it adds delay...
// # <- we try reducing that to 100 ms delay.
#if (FPS != 60)
#error "FPS must currently be 60, as highpass taps are pre-computed for that value"
#endif
// Butterworth filter: order=5, fc=0.5, fs=60, btype='highpass'
std::vector b {0.91875845, -4.59379227, 9.18758454, -9.18758454, 4.59379227, -0.91875845};
std::vector a {1. , -4.83056552, 9.33652742, -9.02545247, 4.36360803, -0.8441171};
@@ -55,14 +84,14 @@ TEST(HelloTest, Zong_SSF_Stage1) {
// apply high-pass filter
//
std::vector<double> y;
y.resize(rows);
for (int i = 0; i < rows; i++) {
y.resize(N);
for (int i = 0; i < N; i++) {
y[i] = filter.filter(signal[i]);
}
// see: http://localhost:8888/notebooks/2026-02-25%20Accelero1/2026-03-01%20SSF3.ipynb
// check results
for (int i = 0; i < rows; i++) {
for (int i = 0; i < N; i++) {
//double rel_error = std::abs((y[i] - y_ref.data[i]) / y_ref.data[i]);
//ASSERT_NEAR(0, rel_error, 1e-2 + ((double) i) * 1e-9); // hack: put in the index into error msg
double abs_error = (y[i] - y_ref.data[i]);
@@ -72,7 +101,115 @@ TEST(HelloTest, Zong_SSF_Stage1) {
npy::npy_data_ptr<double> d;
d.data_ptr = y.data();
d.shape = {(unsigned long) rows};
d.shape = {(unsigned long) N};
const std::string path{"test2/ssf_t2_y_out.npy"};
npy::write_npy(path, d);
}
TEST(HelloTest, Filter_Delta_U) {
Filt f_delta_u(2, 0, 0, std::vector {1.0, -1.0});
std::vector x { 1.0, 3.0, 2.0 };
std::vector y_ref { 1.0, 2.0, -1.0 };
std::vector<double> y;
y.resize(x.size());
for (int i = 0; i < x.size(); i++) {
y[i] = f_delta_u.filter(x[i]);
}
for (int i = 0; i < x.size(); i++) {
ASSERT_NEAR(y_ref[i], y[i], 1e-5);
}
}
// NOTE: later SSF must be fed -u, not u
TEST(HelloTest, Filter_SSF) {
SsfFilter f_ssf(3);
std::vector x { 1.0, 3.0, 2.0, 5.0, 1.0, 1.5 };
// du { 1.0, 2.0, -1.0, 3.0, -4.0, 0.5 }
// duc { 1.0, 2.0, 0.0, 3.0, 0.0, 0.5 }
// ssf { 1.0, 3.0, 3.0, 5.0, 3.0, 3.5 }
std::vector ssf_ref { 1.0, 3.0, 3.0, 5.0, 3.0, 3.5 };
std::vector<double> ssf;
ssf.resize(x.size());
for (int i = 0; i < x.size(); i++) {
ssf[i] = f_ssf.filter(x[i]);
}
for (int i = 0; i < x.size(); i++) {
ASSERT_NEAR(ssf_ref[i], ssf[i], 1e-5);
}
}
TEST(HelloTest, Zong_SSF_Stage2) {
npy::npy_data acc = npy::read_npy<double>("test2/ssf_t2_acc.npy");
std::vector<double> signal = fetch_y_axis(acc);
#if (FPS != 60)
#error "FPS must currently be 60, as highpass taps are pre-computed for that value"
#endif
// Butterworth filter: order=5, fc=0.5, fs=60, btype='highpass'
std::vector b {0.91875845, -4.59379227, 9.18758454, -9.18758454, 4.59379227, -0.91875845};
std::vector a {1. , -4.83056552, 9.33652742, -9.02545247, 4.36360803, -0.8441171};
IirFilter filter(b, a);
// Stage 1: high-pass
auto y = apply_filter(filter, signal);
Filt f_neg(1, 0, 0, std::vector {-1.0});
auto y_neg = apply_filter(f_neg, y);
// Stage 2: sum slope function
const size_t upslope_width = 4;
SsfFilter f_ssf(upslope_width);
auto ssf = apply_filter(f_ssf, y_neg);
npy_save("test2/ssf_t2_ssf.npy", ssf);
}
/** Returns the ssf_threshold as the filter output for debugging. */
class DebugSsfStepDetectorThreshold : public SsfStepDetector {
public:
DebugSsfStepDetectorThreshold(size_t len_refr) : SsfStepDetector(len_refr) {}
double filter(double val) {
this->SsfStepDetector::filter(val);
return peek_threshold();
}
};
TEST(HelloTest, Zong_SSF_Stage3) {
npy::npy_data acc = npy::read_npy<double>("test2/ssf_t2_acc.npy");
std::vector<double> signal = fetch_y_axis(acc);
#if (FPS != 60)
#error "FPS must currently be 60, as highpass taps are pre-computed for that value"
#endif
// Butterworth filter: order=5, fc=0.5, fs=60, btype='highpass'
std::vector b {0.91875845, -4.59379227, 9.18758454, -9.18758454, 4.59379227, -0.91875845};
std::vector a {1. , -4.83056552, 9.33652742, -9.02545247, 4.36360803, -0.8441171};
IirFilter filter(b, a);
// Stage 1: high-pass
auto y = apply_filter(filter, signal);
Filt f_neg(1, 0, 0, std::vector {-1.0});
auto y_neg = apply_filter(f_neg, y);
// Stage 2: sum slope function
const size_t upslope_width = 4;
SsfFilter f_ssf(upslope_width);
auto ssf = apply_filter(f_ssf, y_neg);
// Stage 3: threshold detection
const size_t len_refr = (size_t) (FPS / (MAX_BPM / 60));
DebugSsfStepDetectorThreshold f_ssd_thr(len_refr);
auto ssf_threshold = apply_filter(f_ssd_thr, ssf);
npy_save("test2/ssf_t2_ssf_threshold.npy", ssf_threshold);
SsfStepDetector f_ssd(len_refr);
auto steps = apply_filter(f_ssd, ssf);
npy_save("test2/ssf_t2_steps.npy", steps);
}

2
pasada-lib/CMakeLists.txt
View File

@@ -3,6 +3,8 @@ project(Pasada_Lib)
SET(PASADA_SRC
library.cpp
iir_filter.cpp
ssf_filter.cpp
include/ssf_filter.h
)
if(PASADA_BUILD_TESTS)

54
pasada-lib/include/ssf_filter.h Normal file
View File

@@ -0,0 +1,54 @@
//
// Created by david on 03.03.2026.
//
#ifndef PASADASUPERPROJECT_SSF_FILTER_H
#define PASADASUPERPROJECT_SSF_FILTER_H
#include "iir_filter.h"
#define FPS 60
#define MAX_BPM 300
/**
* Sum-Slope Function filter.
*
* See Zong et al, 2003: An open-source algorithm to detect onset of arterial blood pressure pulses.
*/
class SsfFilter {
protected:
size_t sw;
Filt f_delta_u;
Filt f_window;
public:
SsfFilter(size_t upslope_width);
double filter(double val);
};
/**
* Provides dirac pulses upon steps, based on SSF input.
*
* Settling time is LEN_INIT (currently 3.0 sec),
* no steps are detected before.
*/
class SsfStepDetector {
protected:
const size_t LEN_INIT;
const size_t LEN_TH_WIN;
size_t num_samples;
double ssf_threshold;
size_t len_refr;
size_t n_refr;
bool is_refr;
double nm1_ssf;
Filt f_ssf_mean;
public:
/**
* @param len_refr duration of refractory period, in samples
*/
SsfStepDetector(size_t len_refr);
double filter(double val);
double peek_threshold();
};
#endif //PASADASUPERPROJECT_SSF_FILTER_H

65
pasada-lib/ssf_filter.cpp Normal file
View File

@@ -0,0 +1,65 @@
//
// Created by david on 03.03.2026.
//
#include "include/ssf_filter.h"
#include <limits>
#include <cmath>
#include <cassert>
static std::vector<double> make_ones(size_t sw) {
std::vector<double> ones;
ones.resize(sw);
ones.assign(sw, 1.0);
return ones;
}
SsfFilter::SsfFilter(size_t upslope_width) :
sw(upslope_width),
// Filt(N, shift, offset, taps)
f_delta_u(2, 0, 0, std::vector {1.0, -1.0}),
f_window(upslope_width, 0, 0, make_ones(upslope_width))
{}
double SsfFilter::filter(double val) {
double du = f_delta_u.filter(val);
double duc = std::max(0.0, du);
double ssf = f_window.filter(duc);
return ssf;
}
SsfStepDetector::SsfStepDetector(size_t len_refr) :
LEN_INIT((size_t) (3.0 * FPS)), // initial window length for ssf_threshold
LEN_TH_WIN((size_t) (3.0 * FPS)), // subsequent window length for ssf_threshold
num_samples(0),
ssf_threshold(std::numeric_limits<double>::infinity()),
len_refr(len_refr), n_refr(0), is_refr(false),
nm1_ssf(0.0),
f_ssf_mean(LEN_TH_WIN, 0, 0, make_ones(LEN_TH_WIN))
{
assert (LEN_INIT >= LEN_TH_WIN && "LEN_INIT < LEN_TH_WIN, check normalization of initial ssf_threshold");
}
double SsfStepDetector::filter(double val) {
double ssf_mean = f_ssf_mean.filter(val) / ((double) LEN_TH_WIN);
double rv = 0.0;
if (num_samples >= LEN_INIT) {
// initial and subsequent threshold setting.
ssf_threshold = 3.0 * ssf_mean * 0.99; // see Zong 2003 for the magic numbers
}
// threshold crossing detection
bool is_txing = nm1_ssf < ssf_threshold && val >= ssf_threshold;
// refractory period reset
if (num_samples - n_refr >= len_refr) is_refr = false;
// transition and not in refractory period? detected a step.
if (is_txing && !is_refr) {
rv = 1.0;
is_refr = true;
n_refr = num_samples;
}
nm1_ssf = val;
num_samples++;
return rv;
}
double SsfStepDetector::peek_threshold() {
return ssf_threshold;
}