From bc235856a883e32f29492da459a8a1f1746317e7 Mon Sep 17 00:00:00 2001 From: srcejon Date: Mon, 9 Jun 2025 11:01:25 +0100 Subject: [PATCH] Add Whittaker Eilers smoothing. --- sdrbase/CMakeLists.txt | 2 + sdrbase/util/whittakereilers.cpp | 236 +++++++++++++++++++++++++++++++ sdrbase/util/whittakereilers.h | 52 +++++++ 3 files changed, 290 insertions(+) create mode 100644 sdrbase/util/whittakereilers.cpp create mode 100644 sdrbase/util/whittakereilers.h diff --git a/sdrbase/CMakeLists.txt b/sdrbase/CMakeLists.txt index 3be8c6929..e07ed93ef 100644 --- a/sdrbase/CMakeLists.txt +++ b/sdrbase/CMakeLists.txt @@ -285,6 +285,7 @@ set(sdrbase_SOURCES util/vlftransmitters.cpp util/waypoints.cpp util/weather.cpp + util/whittakereilers.cpp util/iot/device.cpp util/iot/homeassistant.cpp util/iot/tplink.cpp @@ -548,6 +549,7 @@ set(sdrbase_HEADERS util/vlftransmitters.h util/waypoints.h util/weather.h + util/whittakereilers.h util/iot/device.h util/iot/homeassistant.h util/iot/tplink.h diff --git a/sdrbase/util/whittakereilers.cpp b/sdrbase/util/whittakereilers.cpp new file mode 100644 index 000000000..e8b5c4f8b --- /dev/null +++ b/sdrbase/util/whittakereilers.cpp @@ -0,0 +1,236 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2025 Jon Beniston, M7RCE // +// // +// This program is free software; you can redistribute it and/or modify // +// it under the terms of the GNU General Public License as published by // +// the Free Software Foundation as version 3 of the License, or // +// (at your option) any later version. // +// // +// This program is distributed in the hope that it will be useful, // +// but WITHOUT ANY WARRANTY; without even the implied warranty of // +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // +// GNU General Public License V3 for more details. // +// // +// You should have received a copy of the GNU General Public License // +// along with this program. If not, see . // +/////////////////////////////////////////////////////////////////////////////////// + +#include "whittakereilers.h" + +WhittakerEilers::WhittakerEilers() : + v1a(nullptr), + v2a(nullptr), + da(nullptr), + dtd(nullptr), + ca(nullptr), + za(nullptr), + zb(nullptr), + b(nullptr), + m_length(0) +{ +} + +WhittakerEilers::~WhittakerEilers() +{ + dealloc(); +} + +void WhittakerEilers::alloc(int length) +{ + v1a = new double[length]; + v2a = new double[length]; + da = new double[length * 3]; + dtd = new double[length * 3]; + ca = new double[length * 3]; + za = new double[length]; + zb = new double[length]; + b = new double[length]; + m_length = length; +} + +void WhittakerEilers::dealloc() +{ + delete v1a; + delete v2a; + delete da; + delete dtd; + delete ca; + delete za; + delete zb; + delete b; + m_length = 0; +} + +void WhittakerEilers::filter(const double *xi, double *yi, const double *w, const int length, const double lambda) +{ + int i; + int j; + int k; + const int m = length; + + if (m_length < length) + { + dealloc(); + alloc(length); + } + + for (i = 0; i < m - 1; i++) { + v1a[i] = 1.0 / (xi[i + 1] - xi[i]); + } + v1a[m - 1] = 0.0; + for (i = 0; i < m - 2; i++) { + v2a[i] = 1.0 / (xi[i + 2] - xi[i]); + } + v2a[m - 1] = 0.0; + v2a[m - 2] = 0.0; + + //Wa = w; + // D1 = V1 * diff(I) + for (i = 0; i < m - 1; i++) { + da[i*3] = -v1a[i]; + da[i*3+1] = v1a[i]; + } + + // D2 = V2 * diff(D1) + for (i = 0; i < m - 2; i++) { + da[i*3] = v2a[i] * -da[i*3]; + da[i*3+1] = v2a[i] * (da[(i+1)*3] - da[i*3+1]); + da[i*3+2] = v2a[i] * da[(i+1)*3+1]; + } + for (i = 1; i <= 6; i++) { + da[m * 3 - i] = 0; + } + + dtd[0 * 3] = lambda * da[0 * 3] * da[0 * 3]; + dtd[0 * 3 + 1] = lambda * da[0 * 3] * da[0 * 3 + 1]; + dtd[0 * 3 + 2] = lambda * da[0 * 3] * da[0 * 3 + 2]; + + dtd[1 * 3] = lambda * (da[0 * 3 + 1] * da[0 * 3 + 1] + da[1 * 3] * da[1 * 3]); + dtd[1 * 3 + 1] = lambda * (da[0 * 3 + 1] * da[0 * 3 + 2] + da[1 * 3] * da[1 * 3 + 1]); + dtd[1 * 3 + 2] = lambda * (da[1 * 3] * da[1 * 3 + 2]); + + for (int row = 2; row < m; row++) { + dtd[row * 3] = lambda * (da[(row - 2) * 3 + 2] * da[(row - 2) * 3 + 2] + da[(row - 1) * 3 + 1] * da[(row - 1) * 3 + 1] + da[row * 3] * da[row * 3]); + dtd[row * 3 + 1] = lambda * (da[(row - 1) * 3 + 1] * da[(row - 1) * 3 + 2] + da[(row) * 3] * da[(row) * 3 + 1]); + dtd[row * 3 + 2] = lambda * (da[(row) * 3] * da[(row) * 3 + 2]); + } + + // Add in W + for (i = 0; i < m; i++) { + dtd[i * 3] = w[i] + dtd[i * 3]; + } + + // Cholesky Decomposition + + i = 1; + j = i - 1; + ca[j * 3] = sqrt(dtd[j * 3]); + + i = 2; + j = i - 2; + ca[j * 3 + 1] = 1.0 / ca[j * 3] * dtd[j * 3 + 1]; + + i = 2; + j = i - 1; + k = i - 2; + double sum = ca[k * 3 + 1] * ca[k * 3 + 1]; + ca[j * 3] = sqrt(dtd[j * 3] - sum); + + for (i = 3; i <= m; i++) { + j = i - 3; + ca[j * 3 + 2] = 1.0 / ca[j * 3] * dtd[j * 3 + 2]; + + k = i - 3; + sum = ca[k * 3 + 2] * ca[k * 3 + 1]; + j = i - 2; + ca[j * 3 + 1] = 1.0 / ca[j * 3] * (dtd[j * 3 + 1] - sum); + + k = i - 3; + sum = ca[k * 3 + 2] * ca[k * 3 + 2]; + k = i - 2; + sum = sum + ca[k * 3 + 1] * ca[k * 3 + 1]; + j = i - 1; + ca[j * 3] = sqrt(dtd[j * 3] - sum); + } + ca[m * 3 - 1] = 0; + ca[m * 3 - 2] = 0; + ca[m * 3 - 4] = 0; + + // % Forward substitution(C' \ (w .* y)) + for (i = 0; i < m; i++) { + b[i] = w[i] * yi[i]; + } + + za[0] = b[0] / ca[0]; + + sum = ca[0 * 3 + 1] * za[0]; + za[1] = (b[1] - sum) / ca[1 * 3]; + + for (i = 3; i <= m; i++) { + sum = ca[(i - 3) * 3 + 2] * za[i - 3] + ca[(i - 2) * 3 + 1] * za[i - 2]; + za[i - 1] = (b[i - 1] - sum) / ca[(i - 1) * 3]; + } + + // Backward substituion C \ (C' \ (w .* y)); + b = za; + + i = m - 1; + zb[i] = b[i] / ca[i * 3]; + + i = m - 2; + sum = ca[i * 3 + 1] * zb[i + 1]; + zb[i] = (b[i] - sum) / ca[i * 3]; + + for (i = m - 2; i >= 1; i--) { + sum = ca[(i - 1) * 3 + 2] * zb[i + 1] + ca[(i - 1) * 3 + 1] * zb[i]; + zb[i-1] = (b[i - 1] - sum) / ca[(i - 1) * 3]; + } + + if (isnan(zb[0])) { + qDebug() << "naan"; + qDebug() << "lambda" << lambda; + for (int i = 0; i < m; i++) { + qDebug() << "xi[" << i << "]" << qSetRealNumberPrecision(12) << xi[i]; + } + for (int i = 0; i < m; i++) { + qDebug() << "yi[" << i << "]" << qSetRealNumberPrecision(12) << yi[i]; + } + for (int i = 0; i < m; i++) { + qDebug() << "w[" << i << "]" << qSetRealNumberPrecision(12) << w[i]; + } + for (int i = 0; i < m; i++) { + qDebug() << "v1a[" << i << "]" << qSetRealNumberPrecision(12) << v1a[i]; + } + for (int i = 0; i < m; i++) { + qDebug() << "v2a[" << i << "]" << qSetRealNumberPrecision(12) << v2a[i]; + } + for (int i = 0; i < m * 3; i++) { + qDebug() << "da[" << i << "]" << qSetRealNumberPrecision(12) << da[i]; + } + for (int i = 0; i < m * 3; i++) { + qDebug() << "dtd[" << i << "]" << qSetRealNumberPrecision(12) << dtd[i]; + } + for (int i = 0; i < m * 3; i++) { + qDebug() << "ca[" << i << "]" << qSetRealNumberPrecision(12) << ca[i]; + } + for (int i = 0; i < m; i++) { + qDebug() << "za[" << i << "]" << qSetRealNumberPrecision(12) << za[i]; + } + for (int i = 0; i < m; i++) { + qDebug() << "zb[" << i << "]" << qSetRealNumberPrecision(12) << zb[i]; + } + qDebug() << "bread"; + // Don't put NaNs in output + return; + } + + // Copy result back to input + for (i = 0; i < m; i++) { + yi[i] = zb[i]; + } + + for (i = 0; i < m; i++) { + // qDebug() << "zb[" << i << "]=" << zb[i]; + } + +} \ No newline at end of file diff --git a/sdrbase/util/whittakereilers.h b/sdrbase/util/whittakereilers.h new file mode 100644 index 000000000..93d94c245 --- /dev/null +++ b/sdrbase/util/whittakereilers.h @@ -0,0 +1,52 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2025 Jon Beniston, M7RCE // +// // +// This program is free software; you can redistribute it and/or modify // +// it under the terms of the GNU General Public License as published by // +// the Free Software Foundation as version 3 of the License, or // +// (at your option) any later version. // +// // +// This program is distributed in the hope that it will be useful, // +// but WITHOUT ANY WARRANTY; without even the implied warranty of // +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // +// GNU General Public License V3 for more details. // +// // +// You should have received a copy of the GNU General Public License // +// along with this program. If not, see . // +/////////////////////////////////////////////////////////////////////////////////// + +#ifndef INCLUDE_WHITTAKEREILERS_H +#define INCLUDE_WHITTAKEREILERS_H + +#include + +#include "export.h" + +// Whittaker-Eilers smoother +// https://pubs.acs.org/doi/10.1021/ac034173t +class SDRBASE_API WhittakerEilers +{ + +public: + WhittakerEilers(); + ~WhittakerEilers(); + void filter(const double *xi, double *yi, const double *w, const int length, const double lambda=2000); + +private: + void alloc(int length); + void dealloc(); + + double *v1a; + double *v2a; + double *da; + double *dtd; + double *ca; + double *za; + double *zb; + double *b; + + int m_length; + +}; + +#endif /* INCLUDE_WHITTAKEREILERS_H */