git: add submodule libnpy

move StepDetector from lockstep android to libpasada

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "google-tests/libnpy"]
+	path = google-tests/libnpy
+	url = https://github.com/llohse/libnpy.git

google-tests/CMakeLists.txt

@@ -9,10 +9,11 @@ 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
+        test_helpers.cpp
         test1.cpp
         test2.cpp
         test3.cpp
-        test_helpers.cpp
+        test4.cpp
 )
 
 file(COPY test1/data1.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)
@@ -26,6 +27,8 @@ file(COPY test2/ssf_t2_y_ref.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test2)
 
 file(COPY test3/ssf_t3_acc.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test3)
 
+file(COPY test4/step_150a.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test4)
+
 target_link_libraries(Google_Tests_run pasada)
 #target_include_directories(Google_Tests_run PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/pasada-lib/include")
 

google-tests/test3.cpp

@@ -81,19 +81,26 @@ TEST(SignalTest, ranges) {
 class DebugRunningQuality : public RunningQuality {
 protected:
     virtual void dispatchLocked() { locked = true; }
-    virtual void dispatchBeat(int idx, bool good, double posCorr) { goods.push_back(good); corrs.push_back(posCorr); }
+    virtual void dispatchBeat(int idx, bool good, double posCorr) {
+        if (locked && lockedAt == -1)
+            lockedAt = idx;
+        goods.push_back(good);
+        corrs.push_back(posCorr);
+    }
 
+    int lockedAt;
     bool locked;
     std::vector<double> corrs;
     std::vector<bool> goods;
 
 public:
-    DebugRunningQuality(): locked(false) {}
+    DebugRunningQuality(): lockedAt(-1), locked(false) {}
     explicit DebugRunningQuality(bool disableSsf): RunningQuality(disableSsf), locked(false) {}
     virtual ~DebugRunningQuality() {}
     bool isLocked() { return locked; }
     std::vector<double> getCorrs() { return corrs; }
     std::vector<bool> getGoods() { return goods; }
+    int getLockedAt() { return lockedAt; }
     std::vector<double> getBeatTemplate() { return this->beatTemplate; }
 };
 
@@ -210,6 +217,9 @@ TEST(SignalTest, RunningQuality_t2) {
     EXPECT_TRUE(sqi.isLocked());
     EXPECT_TRUE(sqi.getCorrs().size() > 50);
 
+    EXPECT_TRUE(sqi.getLockedAt() < 10);
+    std::cout << "lockedAt=" << sqi.getLockedAt() << std::endl;
+
     std::vector<double> corrs(sqi.getCorrs());
     npy_save("test3/ssf_t3_sqi_corrs.npy", corrs);
 

google-tests/test4.cpp

@@ -0,0 +1,38 @@
+//
+// Created by david on 15.03.2026.
+//
+
+#include <gtest/gtest.h>
+
+#include "step_detector.h"
+#include "npy.hpp"
+#include "test_helpers.h"
+
+TEST(StepDetector, t1_sub_sample_resolution) {
+    npy::npy_data s = npy::read_npy<double>("test4/step_150a.npy");
+
+    std::vector<double> signal = fetch_y_axis(s);
+    const size_t N = signal.size();
+    const size_t N_INIT = SsfStepDetector::initial_samples();
+
+    StepDetector det(nullptr, true);
+
+    // initialize: feed for priming the filters
+    det.primeFilters(signal);
+
+    // feed for actual test
+    for (size_t i = 0; i < N; i++) {
+        const auto a_i = static_cast<float>(signal[i]);
+        det.filter(std::vector<float> {0.0f, a_i, 0.0f});
+    }
+
+    std::vector<double> ssd = det.getBufSsd(); // raw SsfStepDetector
+    std::vector<double> sqi = det.getBufSqi(); // SQI - RunningQuality beat correlations
+    std::vector<double> out = det.getBufOut(); // steps where SQI is OK
+
+    npy_save("test4/t1_ssd.npy", ssd);
+    npy_save("test4/t1_sqi.npy", sqi);
+    npy_save("test4/t1_out.npy", out);
+
+    // http://localhost:8888/notebooks/2026-03-10%20step%20interpolate%2F2026-03-15%20synth.ipynb
+}

pasada-lib/CMakeLists.txt

@@ -5,6 +5,7 @@ SET(PASADA_SRC
         iir_filter.cpp
         ssf_filter.cpp
         pd_signal.cpp
+        step_detector.cpp
 )
 
 if(PASADA_BUILD_TESTS)

pasada-lib/iir_filter.cpp

@@ -5,7 +5,9 @@
 #include "iir_filter.h"
 #include <iostream>
 
+#ifndef DEBUG_IIR
 #define DEBUG_IIR 0
+#endif
 
 #if (DEBUG_IIR == 1)
 #define DEBUG_PRINT(expr) do { expr; } while (0)

pasada-lib/include/ssf_filter.h

@@ -52,6 +52,8 @@ public:
     SsfStepDetector(size_t len_refr);
     double filter(double val);
     double peek_threshold();
+
+    static size_t initial_samples();
 };
 
 /**
@@ -98,8 +100,23 @@ public:
     // note: arg should be an iterator really, but can do later
     /**
      * @param beat individual beat accelero signal
+     * @return true if it is good beat
      */
-    void append(std::vector<double> &rawBeat, std::vector<double> &rawSsf);
+    bool append(std::vector<double> &rawBeat, std::vector<double> &rawSsf);
+};
+
+/**
+ * Signal quality indicator.
+ */
+class RunningQualityFilter {
+protected:
+    RunningQuality f_sqi;
+    std::vector<double> beat_buf;
+    std::vector<double> ssf_buf;
+    double sqi;
+public:
+    RunningQualityFilter(size_t upslope_width);
+    double filter(double y, double ssf, double step);
 };
 
 #endif //PASADASUPERPROJECT_SSF_FILTER_H

pasada-lib/include/step_detector.h

@@ -0,0 +1,52 @@
+//
+// Created by david on 15.03.2026.
+//
+
+#ifndef PASADASUPERPROJECT_STEP_DETECTOR_H
+#define PASADASUPERPROJECT_STEP_DETECTOR_H
+
+#include "iir_filter.h"
+#include "ssf_filter.h"
+#include <vector>
+
+class StepListener {
+public:
+    virtual ~StepListener() {}
+    virtual void playBeat() = 0;
+};
+
+/**
+ * Step detector from accelerometer signal.
+ *
+ * Settling time is 3.0 sec (defined in SsfStepDetector.LEN_INIT),
+ * no steps are detected before.
+ */
+class StepDetector {
+protected:
+    StepListener *listener;
+    IirFilter f_highpass;
+    Filt f_neg;
+    SsfFilter f_ssf;
+    SsfStepDetector f_ssd;
+    RunningQualityFilter f_sqi;
+
+    bool debug;
+    std::vector<double> buf_ssd;
+    std::vector<double> buf_sqi;
+    std::vector<double> buf_out;
+
+public:
+    StepDetector(StepListener *listener, bool debug = false);
+    void filter(std::vector<float> values);
+    std::vector<double> getBufSsd();
+    std::vector<double> getBufSqi();
+    std::vector<double> getBufOut();
+
+    /**
+     * Prime the filters using the given input signal.
+     * Used for debugging (non-realtime processing) to align the signal.
+     */
+    void primeFilters(std::vector<double> sig);
+};
+
+#endif //PASADASUPERPROJECT_STEP_DETECTOR_H

pasada-lib/ssf_filter.cpp

@@ -9,6 +9,17 @@
 #include <iomanip>
 #include <iostream>
 
+#ifndef DEBUG_SSF
+#define DEBUG_SSF 0
+#endif
+
+#if (DEBUG_SSF == 1)
+#define DEBUG_PRINT(expr) do { expr; } while (0)
+#else
+#define DEBUG_PRINT(expr) while(0) { expr; }
+#endif
+
+
 static std::vector<double> make_ones(size_t sw) {
     std::vector<double> ones;
     ones.resize(sw);
@@ -29,8 +40,10 @@ double SsfFilter::filter(double val) {
     return ssf;
 }
 
+size_t SsfStepDetector::initial_samples() { return (size_t) (3.0 * FPS); }
 
 SsfStepDetector::SsfStepDetector(size_t len_refr) :
+    // note: also change above, in initial_samples()
     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),
@@ -66,15 +79,15 @@ double SsfStepDetector::filter(double ssf) {
     if (num_samples == LEN_INIT) {
         // initial threshold setting
         ssf_threshold = 3.0 * ssf_mean * 0.99; // see Zong 2003 for the magic numbers
-        //std::cerr << "before prime()" << std::endl;
+        //DEBUG_PRINT(std::cerr << "before prime()" << std::endl);
         f_ssf_threshold_smoothing.prime(ssf_threshold);
     } else if (num_samples > LEN_TH_WIN) {
-        //std::cerr << "adaptive threshold setting" << std::endl;
+        //DEBUG_PRINT(std::cerr << "adaptive threshold setting" << std::endl);
         // adaptive threshold setting
         // +2 is half the window size
         // TODO: param upon SsfFilter.upslope_width/2 instead of hardcoding -- also f_ssf_threshold_smoothing(), nb. should be even number
         if (num_samples == n_refr + 2) {
-            //std::cerr << "setting adaptive threshold setting" << std::endl;
+            //DEBUG_PRINT(std::cerr << "setting adaptive threshold setting" << std::endl);
             ssf_threshold_nm1 = ssf_threshold;
             // the ssf peak comes 3 samples (half-window + 1 sample) after the crossing
             ssf_threshold = f_ssf_threshold_smoothing.filter(ssf) / ((double) f_ssf_threshold_smoothing.size()) * 0.6;
@@ -113,7 +126,7 @@ RunningQuality::RunningQuality(bool disableSsf): beatCorrThr2(BEAT_CORR_THR_2),
 RunningQuality::~RunningQuality() {}
 
 // note: arg should be an iterator really, but can do later
-void RunningQuality::append(std::vector<double> &rawBeat, std::vector<double> &rawSsf) {
+bool RunningQuality::append(std::vector<double> &rawBeat, std::vector<double> &rawSsf) {
     // TODO: should ignore crazy-long and very short beats here. (filter up on beat detector)
 
     std::vector<double> beat;
@@ -138,19 +151,19 @@ void RunningQuality::append(std::vector<double> &rawBeat, std::vector<double> &r
 
     if (beatTemplates.size() == 0) {
         // cannot correlate the first beat, no template yet
-        std::cerr << "(0) first beat -> addTemplate()" << std::endl;
+        DEBUG_PRINT(std::cerr << "(0) first beat -> addTemplate()" << std::endl);
         addTemplate(ssf);
         justLocked = false;
     } else if (beatTemplates.size() <= 2) {
         // restart if there is no clear correlation between beats
         if (goodBeat) {
-            std::cerr << "(2) good initial beat -> addTemplate()" << std::endl;
+            DEBUG_PRINT(std::cerr << "(2) good initial beat -> addTemplate()" << std::endl);
             addTemplate(ssf);
             if (beatTemplates.size() > 2)
-                justLocked = true; // TODO why not set? wrong compiler optimization? (is it unaware of member change?)
-            //std::cerr << "  (2) beatTemplates.size()=" << beatTemplates.size() << " justLocked=" << ((int) justLocked) << std::endl;
+                justLocked = true;
+            //DEBUG_PRINT(std::cerr << "  (2) beatTemplates.size()=" << beatTemplates.size() << " justLocked=" << ((int) justLocked) << std::endl);
         } else {
-            std::cerr << "(2) bad initial beat idx=" << idx << " -> replaceTemplate() corr=" << std::fixed << std::setw(7) << std::setprecision(4) << corr << " checkedSsf=" << checkedSsf << std::endl;
+            DEBUG_PRINT(std::cerr << "(2) bad initial beat idx=" << idx << " -> replaceTemplate() corr=" << std::fixed << std::setw(7) << std::setprecision(4) << corr << " checkedSsf=" << checkedSsf << std::endl);
             replaceTemplate(ssf);
             //badBeatRanges.clear();
             justLocked = false;
@@ -158,7 +171,7 @@ void RunningQuality::append(std::vector<double> &rawBeat, std::vector<double> &r
     } else {
         // running mode: collect bad beats, but may be OK not to restart immediately
 
-        std::cerr << "(3) running mode, good=" << ((int) goodBeat) << " justLocked=" << ((int) justLocked) << std::endl;
+        DEBUG_PRINT(std::cerr << "(3) running mode, good=" << ((int) goodBeat) << " justLocked=" << ((int) justLocked) << std::endl);
         if (goodBeat) {
             addTemplate(ssf);
         } else {
@@ -170,4 +183,20 @@ void RunningQuality::append(std::vector<double> &rawBeat, std::vector<double> &r
         dispatchBeat(idx, goodBeat, posCorr);
     }
     idx++;
+    if (!goodBeat) return 0.0;
+    return posCorr;
+}
+
+
+RunningQualityFilter::RunningQualityFilter(size_t upslope_width) : sqi(0.0) {}
+
+double RunningQualityFilter::filter(double y, double ssf, double step) {
+    if (step == 1.0) {
+        sqi = f_sqi.append(beat_buf, ssf_buf);
+        beat_buf.clear();
+        ssf_buf.clear();
+    }
+    beat_buf.push_back(y);
+    ssf_buf.push_back(ssf);
+    return sqi;
 }

pasada-lib/step_detector.cpp

@@ -0,0 +1,70 @@
+//
+// Created by david on 15.03.2026.
+//
+
+#include "step_detector.h"
+
+// TODO: we are hardcoding filter coefficients for 60 Hz
+// TODO: this is tolerable for 50 Hz
+
+// TODO: check if we can do with floats instead of doubles
+// (check how much the [already bad] accuracy of filtering suffers)
+
+// TODO: in Java, check if delta timestamps effectively match FPS
+// TODO: FPS constant should be passed as argument to C++ (but we keep an FPS define to validate the coefficients)
+
+// Butterworth filter: order=5, fc=0.5, fs=60, btype='highpass'
+static std::vector<double> hpf_taps_b {0.91875845, -4.59379227,  9.18758454, -9.18758454,  4.59379227, -0.91875845};
+static std::vector<double> hpf_taps_a {1.        , -4.83056552,  9.33652742, -9.02545247,  4.36360803, -0.8441171};
+static size_t upslope_width = 4;
+const size_t len_refr = (size_t) (FPS / (MAX_BPM / 60));
+
+StepDetector::StepDetector(StepListener *listener, bool debug) :
+    listener(listener),
+    f_highpass(hpf_taps_b, hpf_taps_a),
+    f_neg(1, 0, 0, std::vector<double> {-1.0}),
+    f_ssf(upslope_width),
+    f_ssd(len_refr),
+    f_sqi(upslope_width),
+    debug(debug)
+{}
+
+#if (FPS != 60)
+#error "FPS must currently be 60, as highpass taps are pre-computed for that value"
+#endif
+
+void StepDetector::filter(std::vector<float> values) {
+    // TODO: later on, we should use a vector projection towards gravity
+    auto s0 = (double) values[1]; // take y-axis value for now
+    auto s1 = f_highpass.filter(s0);
+    auto s2 = f_neg.filter(s1);
+    auto s3 = f_ssf.filter(s2);
+    auto s4 = f_ssd.filter(s3);
+    auto q5 = f_sqi.filter(s2, s3, s4);
+    if (debug) {
+        buf_ssd.push_back(s4);
+        buf_sqi.push_back(q5);
+        buf_out.push_back(s4 * (q5 > 0.0 ? 1.0 : 0.0));
+    }
+    // is step, step quality is OK, and we have a listener?
+    if(s4 > 0.0 && q5 > 0.0 && listener != nullptr) {
+        listener->playBeat();
+    }
+}
+
+std::vector<double> StepDetector::getBufSsd() { return buf_ssd; }
+std::vector<double> StepDetector::getBufSqi() { return buf_sqi; }
+std::vector<double> StepDetector::getBufOut() { return buf_out; }
+
+void StepDetector::primeFilters(std::vector<double> sig) {
+    const size_t N_INIT = SsfStepDetector::initial_samples();
+    // initialize: feed for priming the filters
+    for (size_t i = 0; i < N_INIT; i++) {
+        const auto a_i = static_cast<float>(sig[i]);
+        filter(std::vector<float> {0.0f, a_i, 0.0f});
+    }
+    // clear debug buffers
+    buf_ssd.clear();
+    buf_sqi.clear();
+    buf_out.clear();
+}