add: GravityFilter

pasada-lib/include/pd_signal.h

@@ -44,6 +44,8 @@ namespace pd_signal {
     double mean(const std::vector<double>& in);
     void diff(std::vector<double>& out, const std::vector<double>& in);
 
+    std::vector<double> gauss(size_t N, double mu, double sigma);
+
     /**
      * Convolution of two polynomials given in ASCENDING power order.
      * If <c>p = p_0 + p_1 x + ... + p_{P-1} x^{P-1}</c> and likewise for q,

pasada-lib/include/step_detector.h

@@ -48,4 +48,17 @@ public:
     void primeFilters(double fps, std::vector<double> sig);
 };
 
+/** mean-filter the gravity vector, then take acceleration downwards */
+class GravityFilter {
+    size_t N;
+    std::vector<double> gauss_taps;
+    Filt gx;
+    Filt gy;
+    Filt gz;
+public:
+    // 5 secs buffer, prime y with direction of gravity (for tests & faster init)
+    GravityFilter(double fps);
+    double filter(std::vector<float> values);
+};
+
 #endif //PASADASUPERPROJECT_STEP_DETECTOR_H

pasada-lib/pd_signal.cpp

@@ -161,6 +161,16 @@ void diff(std::vector<double>& out, const std::vector<double>& in) {
     }
 }
 
+std::vector<double> gauss(size_t N, double mu, double sigma) {
+    const double norm = sigma * sqrt(2.0 * kPi);
+    std::vector<double> data(N);
+    for (int i = 0; i < N; i++) {
+        const double x = i;
+        data[i] = std::exp(-0.5 * (x - mu) * (x - mu) / (sigma * sigma)) / norm;
+    }
+    return data;
+}
+
 // Convolution of two polynomials in ascending power order.
 void polymul(std::vector<cplx>& out,
              const std::vector<cplx>& p, const std::vector<cplx>& q) {

pasada-lib/step_detector.cpp

@@ -4,6 +4,8 @@
 
 #include "step_detector.h"
 
+#include "pd_signal.h"
+
 StepDetector::StepDetector(double fps, StepListener *listener, bool debug) :
     listener(listener),
     f_neg(1, 0, 0, std::vector<double> {-1.0}),
@@ -13,6 +15,36 @@ StepDetector::StepDetector(double fps, StepListener *listener, bool debug) :
     debug(debug)
 {}
 
+static int gravity_num_taps(double fps) {
+    return 5.0 * fps;
+}
+
+// 5 secs buffer, prime y with direction of gravity (for tests & faster init)
+GravityFilter::GravityFilter(double fps) :
+    N(gravity_num_taps(fps)),
+    gauss_taps(pd_signal::gauss(N, N/2, N/4)),
+    gx(N, 0, 0, gauss_taps),
+    gy(N, 0, 0, gauss_taps),
+    gz(N, 0, 0, gauss_taps)
+{
+    gy.prime(-9.81);
+}
+
+double GravityFilter::filter(std::vector<float> values) {
+    gx.push(values[0]);
+    gy.push(values[1]);
+    gz.push(values[2]);
+    double x = gx.peek(), y = gy.peek(), z = gz.peek();
+    double g = sqrt(x * x + y * y + z * z);
+    // e = mean(a)
+    double ex = x / g, ey = y / g, ez = z / g;
+    // e \in a
+    double vx = values[0] * ex;
+    double vy = values[1] * ey;
+    double vz = values[2] * ez;
+    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