feat: add GravityFilter and Resampler to StepDetector

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