feat: Resampler: Normalizes incoming Android sensor sampling rate

google-tests/CMakeLists.txt

@@ -15,6 +15,7 @@ add_executable(Google_Tests_run
         test3.cpp
         test4.cpp
         test5.cpp
+        test6.cpp
 )
 
 file(COPY test1/data1.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)

google-tests/test6.cpp

@@ -0,0 +1,122 @@
+//
+// Created by david on 19.05.2026.
+//
+
+#include <filesystem>
+#include <numeric>
+#include <random>
+#include <gtest/gtest.h>
+#include "pd_resamp.h"
+#include "pd_signal.h"
+#include "test_helpers.h"
+
+#define M_PI 3.14159265358979323846
+
+void make_test_signal_1(int N, std::vector<double> &ts, std::vector<double> &sig) {
+    double f = 10.0;
+    double fs = 100.0;
+    pd_signal::linspace(ts, 0.0, (N-1) / fs * 1e9, N, false);
+    sig.resize(N);
+    for (int i = 0; i < N; i++) {
+        sig[i] = std::cos(2 * M_PI * f * i / fs);
+    }
+}
+
+void add_noise(std::vector<double>& x, double mu, double sigma) {
+    if (sigma < 0.0) { throw std::invalid_argument("sigma must be non-negative"); }
+    std::mt19937 rng { 42 }; /* std::random_device{}() */
+    std::normal_distribution<double> dist(mu, sigma);
+    for (double& v : x) {
+        v += dist(rng);
+    }
+}
+
+int make_spiky_times(int N_hint, std::vector<double>& ts_in, std::vector<double>& ts_out, std::vector<double> &sig_in, std::vector<double> &sig_out) {
+    double fs = 100.0;
+    // note that resulting indices will be 100 + halfdist, because of sampling rate change at i=100  =>  120, 139, 149
+    std::vector<int> spikes {140, 178, 198};
+    std::vector<double> rel_spikes { 1.8, 5.6, 2.51 };
+    int N = N_hint + static_cast<int>(std::accumulate(rel_spikes.begin(), rel_spikes.end(), 0) + 1.0);
+
+    // at certain indices, add a larger time spike
+    ts_out.resize(ts_in.size());
+    std::ranges::copy(ts_in, ts_out.begin());
+    for (int i = 0; i < spikes.size(); i++) {
+        int i_spike = spikes[i];
+        double dt_spike = (rel_spikes[i] - 1.0) * 1.0 / fs * 1e9;
+        for (int j = i_spike; j < N_hint; j++) {
+            ts_out[j] += dt_spike;
+        }
+    }
+
+    // add gaussian noise to times
+    add_noise(ts_out, 0.0, 0.01 / fs * 1e9);
+    std::ranges::sort(ts_out); // make sure they remain sorted
+
+    // reduce sampling rate in second half
+    for (int i = 100; i < 100 + (N_hint - 100) / 2; i++) {
+        ts_out[i] = ts_out[100 + (i-100)*2];
+    }
+    ts_out.resize(100 + (N_hint - 100) / 2);
+
+    // compute signal at times
+    pd_signal::interp(sig_out, ts_out, ts_in, sig_in);
+
+    return N;
+}
+
+TEST(HelloTest, Resampler_Test1) {
+    std::vector<double> ts_orig, ts_spiky;
+    std::vector<double> a_orig, a_spiky;
+    std::vector<double> sig_res;
+
+    make_test_signal_1(207, ts_orig, a_orig); // N = 200+sum(rel_spikes)-len(rel_spikes)
+    double fs = 1e9 / (ts_orig[1]-ts_orig[0]);
+    //std::cout << "fs=" << fs << std::endl;
+    make_spiky_times(200, ts_orig, ts_spiky, a_orig, a_spiky);
+
+    Resampler res;
+
+    const int INITIAL_SAMPLES = 100; // Resampler.INITIAL_SAMPLES;
+    int i;
+    // push - initial samples are buffered
+    for (i = 0; i < INITIAL_SAMPLES - 1; i++) {
+        res.push(ts_spiky[i], a_spiky[i]);
+        ASSERT_FALSE(res.peek());
+    }
+    res.push(ts_spiky[i], a_spiky[i]);
+    //ASSERT_NEAR(res.get_fs(), fs, 1e-7); // should fail
+    ASSERT_NEAR(res.get_fs(), fs, 1e-2);
+
+    // get - initial samples are pushed out
+    sig_res.resize(ts_orig.size()+1); // despite gaussian time noise, sum(ts) should roughly be same length as we guessed initially
+    for (i = 0; i < INITIAL_SAMPLES; i++) {
+        ASSERT_TRUE(res.peek());
+        sig_res[i] = res.get();
+    }
+
+    // push - additional samples are all pushed out
+    int j = INITIAL_SAMPLES;
+    for (i = INITIAL_SAMPLES; i < ts_spiky.size(); i++) {
+        res.push(ts_spiky[i], a_spiky[i]);
+        // potentially get multiple samples
+        while (res.peek())
+            sig_res[j++] = res.get();
+    }
+
+    std::filesystem::create_directories("test6");
+    npy_save("test6/ts_orig_t1.npy", ts_orig);
+    npy_save("test6/a_orig_t1.npy", a_orig);
+
+    npy_save("test6/ts_spiky_t1.npy", ts_spiky);
+    npy_save("test6/a_spiky_t1.npy", a_spiky);
+
+    npy_save("test6/sig_res_t1.npy", sig_res);
+    std::vector<double> fs_t1{res.get_fs()};
+    npy_save("test6/fs_t1.npy", fs_t1);
+    /*
+     * ts_gen = np.arange(sig_res_t1.shape[0]) / fs * 1e9
+     * plt.plot(ts_spiky_t1, a_spiky_t1)
+     * plt.plot(ts_gen, sig_res_t1)
+     */
+}

pasada-lib/CMakeLists.txt

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

pasada-lib/include/pd_resamp.h

@@ -0,0 +1,51 @@
+//
+// Created by david on 17.05.2026.
+//
+
+#ifndef PASADASUPERPROJECT_PD_RESAMP_H
+#define PASADASUPERPROJECT_PD_RESAMP_H
+
+#include "iir_filter.h"
+
+/** Filter that changes sampling rate between input and output. */
+class ResamplingFilter {
+public:
+    ResamplingFilter() {}
+    virtual ~ResamplingFilter() {}
+    virtual void push(double ts, double val) = 0;
+    virtual bool peek() = 0;
+    virtual double get() = 0;
+};
+
+/** Normalizes incoming Android sensor sampling rate. */
+class Resampler : public ResamplingFilter {
+protected:
+    const size_t INITIAL_SAMPLES = 100;
+    std::vector<double> times;
+    std::vector<double> data;
+    /** circular buffer size */
+    size_t N;
+    /** write index */
+    size_t n;
+    /** read index */
+    size_t m;
+    bool initialized;
+    bool read_valid;
+    /** computed sampling frequency, this will be the output rate */
+    double fs;
+    void compute_fs();
+public:
+    Resampler();
+    /**
+     * Push a value into the buffer.
+     * Caller is responsible for polling via peek() and get() afterward.
+     * @param ts  timestamp in nanoseconds
+     * @param val signal sample
+     */
+    void push(double ts, double val) override;
+    bool peek() override;
+    double get() override;
+    double get_fs() const;
+};
+
+#endif //PASADASUPERPROJECT_PD_RESAMP_H

pasada-lib/include/pd_signal.h

@@ -40,6 +40,10 @@ namespace pd_signal {
     /** two-dimensional mean of a collection of signals */
     void mean(std::vector<double> &out, std::deque<std::vector<double> >& m);
 
+    /** simple mean of 1-d signal */
+    double mean(const std::vector<double>& in);
+    void diff(std::vector<double>& out, const std::vector<double>& in);
+
     /**
      * 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/pd_resamp.cpp

@@ -0,0 +1,103 @@
+//
+// Created by david on 17.05.2026.
+//
+
+#include "pd_resamp.h"
+#include "pd_signal.h"
+
+Resampler::Resampler(): N(INITIAL_SAMPLES+1), n(0), m(0), initialized(false), read_valid(false), fs(0.0) {
+    times.resize(N);
+    data.resize(N);
+    times.assign(N, 0.0);
+    data.assign(N, 0.0);
+}
+
+void Resampler::push(double ts, double val) {
+    // i: previous write position
+    auto i = static_cast<size_t>((static_cast<int>(n) - 1 + static_cast<int>(N)) % static_cast<int>(N));
+    auto im = static_cast<size_t>((static_cast<int>(m) - 1 + static_cast<int>(N)) % static_cast<int>(N));
+    if (ts < times[i]) throw std::invalid_argument("we expect ts to be time-ascending");
+    // j: current write position
+    auto j = n;
+    times[n] = ts;
+    data[n] = val;
+    n = (n+1) % N;
+    // note: we do not currently handle overrun (assume caller keeps contract)
+    if (n == INITIAL_SAMPLES && !initialized) {
+        compute_fs();
+        read_valid = true;
+        return; // returns INITIAL_SAMPLES all at once
+        // ??? need to compute 'dt' etc. for last sample! -> fall through
+    }
+    // once initialized, we skip ahead as much as possible - avoid buffering too much
+    double dt = ts - times[im];
+    double dtr = dt * 1e-9 * fs;
+    if (dtr < 0) { throw std::invalid_argument("dt is negative"); }
+    // case 1: 'ts' is less than next sample
+    if (dtr < 0.99) {
+        // drop samples (cannot be bothered with interpolation):
+        // practice shows that Android initially provides a high sampling rate, which subsequently drops later on,
+        // so we simply skip the implementation here.
+        read_valid = false;
+        return;
+    }
+    // case 2: 'ts' is exactly next sample
+    if (0.99 < dtr && dtr < 1.01) {
+        m = j; // skip directly to actual sample
+        read_valid = true;
+        return;
+    }
+    // case 3: 'ts' skips samples
+    if (dtr >= 1.01) {
+        auto ts_nm1 = times[i];
+        // x = ts[n-1] + np.linspace(1.0, dtr, int(np.round(dtr)), endpoint=True) / fs * 1e9
+        // xp = [ts[n-1], ts[n]]
+        // fp = [a[n-1], a[n]]
+        // y = np.interp(x, xp, fp)
+        std::vector<double> y;
+        std::vector<double> x;
+        std::vector<double> xp { times[i], ts };
+        std::vector<double> fp { data[i], val };
+        pd_signal::linspace(x, 1.0, dtr, static_cast<int>(round(dtr)), false);
+        for (auto& e : x) e = ts_nm1 + e / fs * 1e9;
+        pd_signal::interp(y, x, xp, fp);
+        // write to data[_ : n]
+        int s = static_cast<int>(x.size());
+        auto p0 = static_cast<size_t>((static_cast<int>(n) - s + static_cast<int>(N)) % static_cast<int>(N));
+        for (int p = 0; p < s; p++) {
+            data[(p0 + p) % N] = y[p];
+        }
+        m = p0; // provides round(dtr) samples output, interpolated
+        read_valid = true;
+        return;
+    }
+}
+
+bool Resampler::peek() {
+    if (!initialized) { return false; }
+    if (!read_valid) { return false; }
+    return n != m;
+}
+
+double Resampler::get() {
+    if (!initialized) { throw std::runtime_error("not initialized"); }
+    if (n == m) { throw std::runtime_error("empty buffer"); }
+    double val = data[m];
+    m = (m+1) % N;
+    return val;
+}
+
+double Resampler::get_fs() const {
+    return fs;
+}
+
+void Resampler::compute_fs() {
+    // compute 'fs' according to first INITIAL_SAMPLES
+    // we ignore 'n' as this is only ever called once per Resampler lifetime, and assume 'times' has been filled from 0 on
+    std::vector<double> delta_times;
+    pd_signal::diff(delta_times, times);
+    delta_times.resize(INITIAL_SAMPLES-1); // trim off trailing buffer slot (which is not filled)
+    double mean_dt = pd_signal::mean(delta_times);
+    fs = 1e9 / mean_dt;
+    initialized = true;
+}

pasada-lib/pd_signal.cpp

@@ -6,6 +6,7 @@
 #include <stdexcept>
 #include <algorithm>
 #include <iostream>
+#include <numeric>
 
 namespace pd_signal {
 
@@ -141,6 +142,25 @@ void mean(std::vector<double> &out, std::deque<std::vector<double> >& m) {
     mean_tpl(out, m);
 }
 
+double mean(const std::vector<double> &in) {
+    if (in.empty()) {
+        throw std::invalid_argument("mean: input vector is empty");
+    }
+    double sum = std::accumulate(in.begin(), in.end(), 0.0);
+    return sum / static_cast<double>(in.size());
+}
+
+void diff(std::vector<double>& out, const std::vector<double>& in) {
+    if (in.size() < 2) {
+        out.clear();
+        return;
+    }
+    out.resize(in.size() - 1);
+    for (std::size_t i = 1; i < in.size(); ++i) {
+        out[i - 1] = in[i] - in[i - 1];
+    }
+}
+
 // Convolution of two polynomials in ascending power order.
 void polymul(std::vector<cplx>& out,
              const std::vector<cplx>& p, const std::vector<cplx>& q) {