You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
234 lines
7.1 KiB
234 lines
7.1 KiB
/////////////////////////////////////////////////////////////////////////////////// |
|
// 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)); |
|
// Reads za directly. This used to be written "b = za;", which repointed the |
|
// member at za's allocation - so dealloc() freed za twice and leaked the real b. |
|
|
|
i = m - 1; |
|
zb[i] = za[i] / ca[i * 3]; |
|
|
|
i = m - 2; |
|
sum = ca[i * 3 + 1] * zb[i + 1]; |
|
zb[i] = (za[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] = (za[i - 1] - sum) / ca[(i - 1) * 3]; |
|
} |
|
|
|
if (std::isnan(zb[0])) { |
|
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]; |
|
} |
|
// 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]; |
|
}*/ |
|
}
|
|
|