diff --git a/google-tests/CMakeLists.txt b/google-tests/CMakeLists.txt index 2119c13..3364f43 100644 --- a/google-tests/CMakeLists.txt +++ b/google-tests/CMakeLists.txt @@ -8,7 +8,10 @@ include_directories(${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR} libnpy/inclu # 'Google_Tests_run' is the target name # 'test1.cpp test2.cpp' are source files with tests -add_executable(Google_Tests_run test1.cpp) +add_executable(Google_Tests_run + test1.cpp + test2.cpp +) file(COPY test1/data1.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1) file(COPY test1/iir_t1_a.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1) @@ -16,6 +19,9 @@ file(COPY test1/iir_t1_b.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1) file(COPY test1/iir_t1_x.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1) file(COPY test1/iir_t1_y.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1) +file(COPY test2/ssf_t2_acc.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test2) +file(COPY test2/ssf_t2_y_ref.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test2) + target_link_libraries(Google_Tests_run pasada) #target_include_directories(Google_Tests_run PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/pasada-lib/include") diff --git a/google-tests/test2.cpp b/google-tests/test2.cpp new file mode 100644 index 0000000..7ff00af --- /dev/null +++ b/google-tests/test2.cpp @@ -0,0 +1,78 @@ +// +// Created by david on 02.03.2026. +// +#include +#include "npy.hpp" +#include +#include "iir_filter.h" +#include + +TEST(HelloTest, Zong_SSF_Stage1) { + npy::npy_data acc = npy::read_npy("test2/ssf_t2_acc.npy"); + npy::npy_data y_ref = npy::read_npy("test2/ssf_t2_y_ref.npy"); + + // + // fetch y axis + // + // TODO: later on, we should use a vector projection towards gravity + std::vector signal; + const size_t rows_real = acc.shape[0]; +#if DEBUG_IIR == 1 + const size_t rows = 5; +#else + const size_t rows = acc.shape[0]; +#endif + int stride = 3; + int offset = 1; // [x,y,z] per row - fetch y + signal.resize(rows); + if (acc.fortran_order) { + stride = 1; + offset = (int) rows_real; + } + /* + std::cout << "is_fortran=" << acc.fortran_order << std::endl; + for (size_t i = 0; i < 10; i++) { + std::cout << "acc.data[" << i << "]=" << acc.data[i] << std::endl; + } + */ + for (int i = 0; i < rows; i++) { + signal[i] = acc.data[i * stride + offset]; + } + ASSERT_NEAR(1.7, signal[0], 1e-5); + ASSERT_NEAR(3.6, signal[1], 1e-5); + ASSERT_NEAR(4.3, signal[2], 1e-5); + + // # de-trending using a high-pass has helped the SSF be less noisy! + // # but it adds delay... + // # <- we try reducing that to 100 ms delay. + + // Butterworth filter: order=5, fc=0.5, fs=60, btype='highpass' + std::vector b {0.91875845, -4.59379227, 9.18758454, -9.18758454, 4.59379227, -0.91875845}; + std::vector a {1. , -4.83056552, 9.33652742, -9.02545247, 4.36360803, -0.8441171}; + IirFilter filter(b, a); + + // + // apply high-pass filter + // + std::vector y; + y.resize(rows); + for (int i = 0; i < rows; i++) { + y[i] = filter.filter(signal[i]); + } + // see: http://localhost:8888/notebooks/2026-02-25%20Accelero1/2026-03-01%20SSF3.ipynb + + // check results + for (int i = 0; i < rows; i++) { + //double rel_error = std::abs((y[i] - y_ref.data[i]) / y_ref.data[i]); + //ASSERT_NEAR(0, rel_error, 1e-2 + ((double) i) * 1e-9); // hack: put in the index into error msg + double abs_error = (y[i] - y_ref.data[i]); + //ASSERT_NEAR(0, abs_error, 1e-2 + ((double) i) * 1e-9); // hack: put in the index into error msg + ASSERT_NEAR(0, abs_error, 0.1 + ((double) i) * 1e-9); // hack: put in the index into error msg + } + + npy::npy_data_ptr d; + d.data_ptr = y.data(); + d.shape = {(unsigned long) rows}; + const std::string path{"test2/ssf_t2_y_out.npy"}; + npy::write_npy(path, d); +} diff --git a/google-tests/test2/ssf_t2_acc.npy b/google-tests/test2/ssf_t2_acc.npy new file mode 100644 index 0000000..e018df0 Binary files /dev/null and b/google-tests/test2/ssf_t2_acc.npy differ diff --git a/google-tests/test2/ssf_t2_y_ref.npy b/google-tests/test2/ssf_t2_y_ref.npy new file mode 100644 index 0000000..8eb0bf2 Binary files /dev/null and b/google-tests/test2/ssf_t2_y_ref.npy differ diff --git a/pasada-lib/iir_filter.cpp b/pasada-lib/iir_filter.cpp index 6c308b5..fd050b2 100644 --- a/pasada-lib/iir_filter.cpp +++ b/pasada-lib/iir_filter.cpp @@ -3,6 +3,15 @@ // #include "iir_filter.h" +#include + +#define DEBUG_IIR 0 + +#if (DEBUG_IIR == 1) +#define DEBUG_PRINT(expr) do { expr; } while (0) +#else +#define DEBUG_PRINT(expr) while(0) { expr; } +#endif Buf::Buf(size_t N): size(N), n(0) { data.resize(N); @@ -25,7 +34,7 @@ double Filt::peek() { for (size_t i = offset; i < this->size; 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; - //std::cout << " t[" << i << "] * v[" << n << "]" << std::endl; + DEBUG_PRINT(std::cout << " t[" << i << "] * v[" << n << "]" << std::endl); sum += this->data[n] * this->taps[i]; } return sum; @@ -38,13 +47,13 @@ IirFilter::IirFilter(std::vector b, std::vector a) : x(b.size(), if (b.size() != a.size()) throw std::invalid_argument("b.size() != a.size()"); } double IirFilter::filter(double val) { - //std::cout << "x.filter(" << val << ")" << std::endl; + DEBUG_PRINT(std::cout << "x.filter(" << val << ")" << std::endl); double xv = x.filter(val); - //std::cout << "xv=" << xv << std::endl; - //std::cout << "y.peek()" << std::endl; + DEBUG_PRINT(std::cout << "xv=" << xv << std::endl); + DEBUG_PRINT(std::cout << "y.peek()" << std::endl); double yv = y.peek(); - //std::cout << "yv=" << yv << std::endl; - //std::cout << "---" << std::endl; + DEBUG_PRINT(std::cout << "yv=" << yv << std::endl); + DEBUG_PRINT(std::cout << "---" << std::endl); double yo = xv - yv; y.push(yo); return yo;