feat: SSF step detector

google-tests/CMakeLists.txt

@@ -8,9 +8,19 @@ include_directories(${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR} libnpy/inclu
 
 # 'Google_Tests_run' is the target name
 # 'test1.cpp test2.cpp' are source files with tests
-add_executable(Google_Tests_run test1.cpp)
+add_executable(Google_Tests_run
+        test1.cpp
+        test2.cpp
+)
 
 file(COPY test1/data1.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)
+file(COPY test1/iir_t1_a.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)
+file(COPY test1/iir_t1_b.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)
+file(COPY test1/iir_t1_x.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)
+file(COPY test1/iir_t1_y.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)
+
+file(COPY test2/ssf_t2_acc.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test2)
+file(COPY test2/ssf_t2_y_ref.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test2)
 
 target_link_libraries(Google_Tests_run pasada)
 #target_include_directories(Google_Tests_run PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/pasada-lib/include")

google-tests/test1.cpp

@@ -3,6 +3,7 @@
 //
 #include <gtest/gtest.h>
 #include "library.h"
+#include "iir_filter.h"
 #include "npy.hpp"
 #include <utility>
 #include <vector>
@@ -51,76 +52,6 @@ TEST(HelloTest, Save_npy_matrix) {
     npy::write_npy(path, d);
 }
 
-/** Shift register implemented as a circular buffer. */
-class Buf {
-protected:
-    std::vector<double> data;
-    size_t size;
-    size_t n;
-public:
-    Buf(size_t N): size(N), n(0) {
-        data.resize(N);
-        data.assign(N, 0.0);
-    }
-    void push(double val) {
-        data[n] = val;
-        n = (n+1) % size;
-    }
-};
-
-/** Running filter base. */
-class Filt : Buf {
-protected:
-    std::vector<double> taps;
-    size_t shift;
-    size_t offset;
-public:
-    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 filter(double val) {
-        this->push(val);
-        return this->peek();
-    }
-    double 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 push(double val) {
-        Buf::push(val);
-    }
-};
-
-class IirFilter {
-protected:
-    Filt y;
-    Filt x;
-public:
-    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 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;
-    }
-};
-
-// TODO: copy npy files from CMake output folder
-// TODO: add npy files to CMakeLists.txt so they are copied into output folder
 TEST(HelloTest, Test_IIR_1_Apply_IIR) {
     npy::npy_data x = npy::read_npy<double>("test1/iir_t1_x.npy");
     npy::npy_data y_e = npy::read_npy<double>("test1/iir_t1_y.npy");

google-tests/test2.cpp

@@ -0,0 +1,215 @@
+//
+// Created by david on 02.03.2026.
+//
+#include <gtest/gtest.h>
+#include "npy.hpp"
+#include <vector>
+#include "iir_filter.h"
+#include "ssf_filter.h"
+#include <cmath>
+#include <limits>
+
+#define FPS 60
+#define MAX_BPM 300
+
+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];
+#if DEBUG_IIR == 1
+    const size_t rows = 5;
+#else
+    const size_t rows = acc.shape[0];
+#endif
+    int stride = 3;
+    int offset = 1; // [x,y,z] per row - fetch y
+    signal.resize(rows);
+    if (acc.fortran_order) {
+        stride = 1;
+        offset = (int) rows_real;
+    }
+    /*
+    std::cout << "is_fortran=" << acc.fortran_order << std::endl;
+    for (size_t i = 0; i < 10; i++) {
+        std::cout << "acc.data[" << i << "]=" << acc.data[i] << std::endl;
+    }
+    */
+    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);
+
+    // # de-trending using a high-pass has helped the SSF be less noisy!
+    // # 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};
+    IirFilter filter(b, a);
+
+    //
+    // apply high-pass filter
+    //
+    std::vector<double> y;
+    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 < 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]);
+        //ASSERT_NEAR(0, abs_error, 1e-2 + ((double) i) * 1e-9); // hack: put in the index into error msg
+        ASSERT_NEAR(0, abs_error, 0.1 + ((double) i) * 1e-9); // hack: put in the index into error msg
+    }
+
+    npy::npy_data_ptr<double> d;
+    d.data_ptr = y.data();
+    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);
+}

pasada-lib/CMakeLists.txt

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

pasada-lib/iir_filter.cpp

@@ -0,0 +1,60 @@
+//
+// 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): 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;
+        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);
+}
+
+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;
+}

pasada-lib/include/iir_filter.h

@@ -0,0 +1,46 @@
+//
+// Created by david on 02.03.2026.
+//
+
+#ifndef PASADASUPERPROJECT_IIR_FILTER_H
+#define PASADASUPERPROJECT_IIR_FILTER_H
+
+#include <utility>
+#include <vector>
+#include <stdexcept>
+
+/** Shift register implemented as a circular buffer. */
+class Buf {
+protected:
+    std::vector<double> data;
+    size_t size;
+    size_t n;
+public:
+    Buf(size_t N);
+    void push(double val);
+};
+
+/** Running filter base. */
+class Filt : Buf {
+protected:
+    std::vector<double> taps;
+    size_t shift;
+    size_t offset;
+public:
+    Filt(size_t N, size_t shift, size_t offset, std::vector<double> taps);
+    double filter(double val);
+    double peek();
+    void push(double val);
+};
+
+/** Running IIR filter. */
+class IirFilter {
+protected:
+    Filt y;
+    Filt x;
+public:
+    IirFilter(std::vector<double> b, std::vector<double> a);
+    double filter(double val);
+};
+
+#endif //PASADASUPERPROJECT_IIR_FILTER_H

pasada-lib/include/ssf_filter.h

@@ -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

pasada-lib/ssf_filter.cpp

@@ -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;
+}