feat: add GravityFilter and Resampler to StepDetector

pasada-lib/include/step_detector.h

@@ -7,6 +7,7 @@
 
 #include "iir_filter.h"
 #include "ssf_filter.h"
+#include "pd_resamp.h"
 #include <vector>
 
 class StepListener {
@@ -15,39 +16,6 @@ public:
     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;
-    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(double fps, 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(double fps, std::vector<double> sig);
-};
-
 /** mean-filter the gravity vector, then take acceleration downwards */
 class GravityFilter {
     size_t N;
@@ -58,7 +26,45 @@ class GravityFilter {
 public:
     // 5 secs buffer, prime y with direction of gravity (for tests & faster init)
     GravityFilter(double fps);
-    double filter(std::vector<float> values);
+    double filter(std::vector<double> values);
+};
+
+/**
+ * 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;
+    GravityFilter f_grav;
+    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;
+
+    Resampler res_x;
+    Resampler res_y;
+    Resampler res_z;
+
+public:
+    StepDetector(double fps, StepListener *listener, bool debug = false);
+    void filter(double ts, std::vector<float> values);
+    void filter_a(double s1);
+    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.
+     */
+    double primeFilters(double fps, std::vector<double> sig);
 };
 
 #endif //PASADASUPERPROJECT_STEP_DETECTOR_H

pasada-lib/step_detector.cpp

@@ -8,7 +8,7 @@
 
 StepDetector::StepDetector(double fps, StepListener *listener, bool debug) :
     listener(listener),
-    f_neg(1, 0, 0, std::vector<double> {-1.0}),
+    f_grav(fps),
     f_ssf(fps),
     f_ssd(fps),
     f_sqi(fps),
@@ -30,7 +30,7 @@ GravityFilter::GravityFilter(double fps) :
     gy.prime(-9.81);
 }
 
-double GravityFilter::filter(std::vector<float> values) {
+double GravityFilter::filter(std::vector<double> values) {
     gx.push(values[0]);
     gy.push(values[1]);
     gz.push(values[2]);
@@ -45,10 +45,22 @@ double GravityFilter::filter(std::vector<float> values) {
     return vx + vy + vz;
 }
 
-void StepDetector::filter(std::vector<float> values) {
-    // TODO: later on, we should use a vector projection towards gravity
-    auto s1 = (double) values[1]; // take y-axis value for now
-    auto s2 = f_neg.filter(s1);
+void StepDetector::filter(double ts, std::vector<float> values) {
+    // resample to smooth over Android sensor FPS variations
+    res_x.push(ts, values[0]);
+    res_y.push(ts, values[1]);
+    res_z.push(ts, values[2]);
+    while (res_x.peek()) {
+        double x = res_x.get(), y = res_y.get(), z = res_z.get();
+        std::vector<double> samp { x, y, z };
+        // gravity filtering
+        double a = f_grav.filter(samp);
+        // pass on accel sample
+        filter_a(a);
+    }
+}
+
+void StepDetector::filter_a(double s2) {
     auto s3 = f_ssf.filter(s2);
     auto s4 = f_ssd.filter(s3);
     auto q5 = f_sqi.filter(s2, s3, s4);
@@ -67,15 +79,18 @@ 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(double fps, std::vector<double> sig) {
+double StepDetector::primeFilters(double fps, std::vector<double> sig) {
     const size_t N_INIT = SsfStepDetector::initial_samples(fps);
     // initialize: feed for priming the filters
+    double ts = 0;
     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});
+        filter(ts, std::vector<float> {0.0f, a_i, 0.0f});
+        ts += 1.0 / fps;
     }
     // clear debug buffers
     buf_ssd.clear();
     buf_sqi.clear();
     buf_out.clear();
+    return ts;
 }