79 lines
2.5 KiB
C++
79 lines
2.5 KiB
C++
//
|
|
// Created by david on 02.03.2026.
|
|
//
|
|
#include <gtest/gtest.h>
|
|
#include "npy.hpp"
|
|
#include <vector>
|
|
#include "iir_filter.h"
|
|
#include <cmath>
|
|
|
|
TEST(HelloTest, Zong_SSF_Stage1) {
|
|
npy::npy_data acc = npy::read_npy<double>("test2/ssf_t2_acc.npy");
|
|
npy::npy_data y_ref = npy::read_npy<double>("test2/ssf_t2_y_ref.npy");
|
|
|
|
//
|
|
// fetch y axis
|
|
//
|
|
// TODO: later on, we should use a vector projection towards gravity
|
|
std::vector<double> 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<double> 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<double> 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);
|
|
}
|