feat: iterate on SsfStepDetector

* 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)

pasada-lib/iir_filter.cpp

@@ -13,13 +13,13 @@
 #define DEBUG_PRINT(expr) while(0) { expr; }
 #endif
 
-Buf::Buf(size_t N): size(N), n(0) {
+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) % size;
+    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) {
@@ -31,9 +31,9 @@ double Filt::filter(double val) {
 }
 double Filt::peek() {
     double sum = 0;
-    for (size_t i = offset; i < this->size; i++) {
+    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->size + this->n - i + shift - 1) % this->size;
+        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];
     }
@@ -42,6 +42,12 @@ double Filt::peek() {
 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()");

pasada-lib/include/iir_filter.h

@@ -13,7 +13,7 @@
 class Buf {
 protected:
     std::vector<double> data;
-    size_t size;
+    size_t N;
     size_t n;
 public:
     Buf(size_t N);
@@ -21,7 +21,7 @@ public:
 };
 
 /** Running filter base. */
-class Filt : Buf {
+class Filt : public Buf {
 protected:
     std::vector<double> taps;
     size_t shift;
@@ -31,6 +31,9 @@ public:
     double filter(double val);
     double peek();
     void push(double val);
+    /** prime the filter by overwriting the entire buffer with 'val' */
+    void prime(double val);
+    size_t size();
 };
 
 /** Running IIR filter. */

pasada-lib/include/pd_signal.h

@@ -6,6 +6,7 @@
 #define PASADASUPERPROJECT_SIGNAL_H
 
 #include <vector>
+#include <deque>
 
 namespace pd_signal {
     /** `num` evenly spaced numbers over interval [start,stop] */
@@ -33,6 +34,8 @@ namespace pd_signal {
 
     /** two-dimensional mean of a collection of signals */
     void mean(std::vector<double> &out, std::vector<std::vector<double> >& m);
+    /** two-dimensional mean of a collection of signals */
+    void mean(std::vector<double> &out, std::deque<std::vector<double> >& m);
 
 }
 

pasada-lib/include/ssf_filter.h

@@ -37,10 +37,12 @@ protected:
     const size_t LEN_TH_WIN;
     size_t num_samples;
     double ssf_threshold;
+    double ssf_threshold_nm1;
+    Filt f_ssf_threshold_smoothing;
     size_t len_refr;
     size_t n_refr;
     bool is_refr;
-    double nm1_ssf;
+    double ssf_nm1;
     Filt f_ssf_mean;
 public:
     /**

pasada-lib/pd_signal.cpp

@@ -113,7 +113,7 @@ double clip(double val, double a_min, double a_max) {
 }
 
 // two-dimensional mean of a collection of signals
-void mean(std::vector<double> &out, std::vector<std::vector<double> >& m) {
+template<class T> void mean_tpl(std::vector<double> &out, T& m) {
     if (m.empty()) {
         out.resize(0);
         return;
@@ -132,4 +132,11 @@ void mean(std::vector<double> &out, std::vector<std::vector<double> >& m) {
     }
 }
 
+void mean(std::vector<double> &out, std::vector<std::vector<double> >& m) {
+    mean_tpl(out, m);
+}
+void mean(std::vector<double> &out, std::deque<std::vector<double> >& m) {
+    mean_tpl(out, m);
+}
+
 }

pasada-lib/ssf_filter.cpp

@@ -6,6 +6,7 @@
 #include <limits>
 #include <cmath>
 #include <cassert>
+#include <iostream>
 
 static std::vector<double> make_ones(size_t sw) {
     std::vector<double> ones;
@@ -33,21 +34,26 @@ SsfStepDetector::SsfStepDetector(size_t len_refr) :
     LEN_TH_WIN((size_t) (3.0 * FPS)), // subsequent window length for ssf_threshold
     num_samples(0),
     ssf_threshold(std::numeric_limits<double>::infinity()),
+    ssf_threshold_nm1(std::numeric_limits<double>::infinity()),
+    f_ssf_threshold_smoothing(6, 0, 0, make_ones(6)),
     len_refr(len_refr), n_refr(0), is_refr(false),
-    nm1_ssf(0.0),
+    ssf_nm1(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 SsfStepDetector::filter(double ssf) {
+    double ssf_mean = f_ssf_mean.filter(ssf) / ((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;
+    // 'is_prev_lower' fixes a glitch where a falling threshold leads to undetected crossings
+    bool is_prev_lower = ssf_nm1 < ssf_threshold || ssf_nm1 < ssf_threshold_nm1;
+    bool is_cur_higher = ssf >= ssf_threshold;
+    bool is_txing = is_prev_lower && is_cur_higher;
     // refractory period reset
     if (num_samples - n_refr >= len_refr) is_refr = false;
     // transition and not in refractory period? detected a step.
@@ -56,7 +62,24 @@ double SsfStepDetector::filter(double val) {
         is_refr = true;
         n_refr = num_samples;
     }
-    nm1_ssf = val;
+    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;
+        f_ssf_threshold_smoothing.prime(ssf_threshold);
+    } else if (num_samples > LEN_TH_WIN) {
+        //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;
+            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;
+        }
+    }
+    ssf_nm1 = ssf;
     num_samples++;
     return rv;
 }