3 changed files with 290 additions and 0 deletions
@ -0,0 +1,236 @@
@@ -0,0 +1,236 @@
|
||||
///////////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2025 Jon Beniston, M7RCE <jon@beniston.com> //
|
||||
// //
|
||||
// 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 <http://www.gnu.org/licenses/>. //
|
||||
///////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#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];
|
||||
} |
||||
|
||||
} |
||||
@ -0,0 +1,52 @@
@@ -0,0 +1,52 @@
|
||||
///////////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2025 Jon Beniston, M7RCE <jon@beniston.com> //
|
||||
// //
|
||||
// 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 <http://www.gnu.org/licenses/>. //
|
||||
///////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifndef INCLUDE_WHITTAKEREILERS_H |
||||
#define INCLUDE_WHITTAKEREILERS_H |
||||
|
||||
#include <QtCore> |
||||
|
||||
#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 */ |
||||
Loading…
Reference in new issue