13
0
livetrax/libs/qm-dsp/maths/Polyfit.h

408 lines
10 KiB
C
Raw Normal View History

/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*- vi:set ts=8 sts=4 sw=4: */
//---------------------------------------------------------------------------
#ifndef PolyfitHPP
#define PolyfitHPP
//---------------------------------------------------------------------------
// Least-squares curve fitting class for arbitrary data types
/*
{ ******************************************
**** Scientific Subroutine Library ****
**** for C++ Builder ****
******************************************
The following programs were written by Allen Miller and appear in the
book entitled "Pascal Programs For Scientists And Engineers" which is
published by Sybex, 1981.
They were originally typed and submitted to MTPUG in Oct. 1982
Juergen Loewner
Hoher Heckenweg 3
D-4400 Muenster
They have had minor corrections and adaptations for Turbo Pascal by
Jeff Weiss
1572 Peacock Ave.
Sunnyvale, CA 94087.
2000 Oct 28 Updated for Delphi 4, open array parameters.
This allows the routine to be generalised so that it is no longer
hard-coded to make a specific order of best fit or work with a
specific number of points.
2001 Jan 07 Update Web site address
Copyright <EFBFBD> David J Taylor, Edinburgh and others listed above
Web site: www.satsignal.net
E-mail: davidtaylor@writeme.com
}*/
///////////////////////////////////////////////////////////////////////////////
// Modified by CLandone for VC6 Aug 2004
///////////////////////////////////////////////////////////////////////////////
#include <iostream>
using std::vector;
class TPolyFit
{
typedef vector<vector<double> > Matrix;
public:
static double PolyFit2 (const vector<double> &x, // does the work
const vector<double> &y,
vector<double> &coef);
private:
TPolyFit &operator = (const TPolyFit &); // disable assignment
TPolyFit(); // and instantiation
TPolyFit(const TPolyFit&); // and copying
static void Square (const Matrix &x, // Matrix multiplication routine
const vector<double> &y,
Matrix &a, // A = transpose X times X
vector<double> &g, // G = Y times X
const int nrow, const int ncol);
// Forms square coefficient matrix
static bool GaussJordan (Matrix &b, // square matrix of coefficients
const vector<double> &y, // constant vector
vector<double> &coef); // solution vector
// returns false if matrix singular
static bool GaussJordan2(Matrix &b,
const vector<double> &y,
Matrix &w,
vector<vector<int> > &index);
};
// some utility functions
namespace NSUtility
{
inline void swap(double &a, double &b) {double t = a; a = b; b = t;}
void zeroise(vector<double> &array, int n);
void zeroise(vector<int> &array, int n);
void zeroise(vector<vector<double> > &matrix, int m, int n);
void zeroise(vector<vector<int> > &matrix, int m, int n);
inline double sqr(const double &x) {return x * x;}
};
//---------------------------------------------------------------------------
// Implementation
//---------------------------------------------------------------------------
using namespace NSUtility;
//------------------------------------------------------------------------------------------
// main PolyFit routine
double TPolyFit::PolyFit2 (const vector<double> &x,
const vector<double> &y,
vector<double> &coefs)
// nterms = coefs.size()
// npoints = x.size()
{
int i, j;
double xi, yi, yc, srs, sum_y, sum_y2;
Matrix xmatr; // Data matrix
Matrix a;
vector<double> g; // Constant vector
const int npoints(x.size());
const int nterms(coefs.size());
double correl_coef;
zeroise(g, nterms);
zeroise(a, nterms, nterms);
zeroise(xmatr, npoints, nterms);
if (nterms < 1) {
std::cerr << "ERROR: PolyFit called with less than one term" << std::endl;
return 0;
}
if(npoints < 2) {
std::cerr << "ERROR: PolyFit called with less than two points" << std::endl;
return 0;
}
if(npoints != y.size()) {
std::cerr << "ERROR: PolyFit called with x and y of unequal size" << std::endl;
return 0;
}
for(i = 0; i < npoints; ++i)
{
// { setup x matrix }
xi = x[i];
xmatr[i][0] = 1.0; // { first column }
for(j = 1; j < nterms; ++j)
xmatr[i][j] = xmatr [i][j - 1] * xi;
}
Square (xmatr, y, a, g, npoints, nterms);
if(!GaussJordan (a, g, coefs))
return -1;
sum_y = 0.0;
sum_y2 = 0.0;
srs = 0.0;
for(i = 0; i < npoints; ++i)
{
yi = y[i];
yc = 0.0;
for(j = 0; j < nterms; ++j)
yc += coefs [j] * xmatr [i][j];
srs += sqr (yc - yi);
sum_y += yi;
sum_y2 += yi * yi;
}
// If all Y values are the same, avoid dividing by zero
correl_coef = sum_y2 - sqr (sum_y) / npoints;
// Either return 0 or the correct value of correlation coefficient
if (correl_coef != 0)
correl_coef = srs / correl_coef;
if (correl_coef >= 1)
correl_coef = 0.0;
else
correl_coef = sqrt (1.0 - correl_coef);
return correl_coef;
}
//------------------------------------------------------------------------
// Matrix multiplication routine
// A = transpose X times X
// G = Y times X
// Form square coefficient matrix
void TPolyFit::Square (const Matrix &x,
const vector<double> &y,
Matrix &a,
vector<double> &g,
const int nrow,
const int ncol)
{
int i, k, l;
for(k = 0; k < ncol; ++k)
{
for(l = 0; l < k + 1; ++l)
{
a [k][l] = 0.0;
for(i = 0; i < nrow; ++i)
{
a[k][l] += x[i][l] * x [i][k];
if(k != l)
a[l][k] = a[k][l];
}
}
g[k] = 0.0;
for(i = 0; i < nrow; ++i)
g[k] += y[i] * x[i][k];
}
}
//---------------------------------------------------------------------------------
bool TPolyFit::GaussJordan (Matrix &b,
const vector<double> &y,
vector<double> &coef)
//b square matrix of coefficients
//y constant vector
//coef solution vector
//ncol order of matrix got from b.size()
{
/*
{ Gauss Jordan matrix inversion and solution }
{ B (n, n) coefficient matrix becomes inverse }
{ Y (n) original constant vector }
{ W (n, m) constant vector(s) become solution vector }
{ DETERM is the determinant }
{ ERROR = 1 if singular }
{ INDEX (n, 3) }
{ NV is number of constant vectors }
*/
int ncol(b.size());
int irow, icol;
vector<vector<int> >index;
Matrix w;
zeroise(w, ncol, ncol);
zeroise(index, ncol, 3);
if(!GaussJordan2(b, y, w, index))
return false;
// Interchange columns
int m;
for (int i = 0; i < ncol; ++i)
{
m = ncol - i - 1;
if(index [m][0] != index [m][1])
{
irow = index [m][0];
icol = index [m][1];
for(int k = 0; k < ncol; ++k)
swap (b[k][irow], b[k][icol]);
}
}
for(int k = 0; k < ncol; ++k)
{
if(index [k][2] != 0)
{
std::cerr << "ERROR: Error in PolyFit::GaussJordan: matrix is singular" << std::endl;
return false;
}
}
for( int i = 0; i < ncol; ++i)
coef[i] = w[i][0];
return true;
} // end; { procedure GaussJordan }
//----------------------------------------------------------------------------------------------
bool TPolyFit::GaussJordan2(Matrix &b,
const vector<double> &y,
Matrix &w,
vector<vector<int> > &index)
{
//GaussJordan2; // first half of GaussJordan
// actual start of gaussj
double big, t;
double pivot;
double determ;
int irow, icol;
int ncol(b.size());
int nv = 1; // single constant vector
for(int i = 0; i < ncol; ++i)
{
w[i][0] = y[i]; // copy constant vector
index[i][2] = -1;
}
determ = 1.0;
for(int i = 0; i < ncol; ++i)
{
// Search for largest element
big = 0.0;
for(int j = 0; j < ncol; ++j)
{
if(index[j][2] != 0)
{
for(int k = 0; k < ncol; ++k)
{
if(index[k][2] > 0) {
std::cerr << "ERROR: Error in PolyFit::GaussJordan2: matrix is singular" << std::endl;
return false;
}
if(index[k][2] < 0 && fabs(b[j][k]) > big)
{
irow = j;
icol = k;
big = fabs(b[j][k]);
}
} // { k-loop }
}
} // { j-loop }
index [icol][2] = index [icol][2] + 1;
index [i][0] = irow;
index [i][1] = icol;
// Interchange rows to put pivot on diagonal
// GJ3
if(irow != icol)
{
determ = -determ;
for(int m = 0; m < ncol; ++m)
swap (b [irow][m], b[icol][m]);
if (nv > 0)
for (int m = 0; m < nv; ++m)
swap (w[irow][m], w[icol][m]);
} // end GJ3
// divide pivot row by pivot column
pivot = b[icol][icol];
determ *= pivot;
b[icol][icol] = 1.0;
for(int m = 0; m < ncol; ++m)
b[icol][m] /= pivot;
if(nv > 0)
for(int m = 0; m < nv; ++m)
w[icol][m] /= pivot;
// Reduce nonpivot rows
for(int n = 0; n < ncol; ++n)
{
if(n != icol)
{
t = b[n][icol];
b[n][icol] = 0.0;
for(int m = 0; m < ncol; ++m)
b[n][m] -= b[icol][m] * t;
if(nv > 0)
for(int m = 0; m < nv; ++m)
w[n][m] -= w[icol][m] * t;
}
}
} // { i-loop }
return true;
}
//----------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------
// Utility functions
//--------------------------------------------------------------------------
// fills a vector with zeros.
void NSUtility::zeroise(vector<double> &array, int n)
{
array.clear();
for(int j = 0; j < n; ++j)
array.push_back(0);
}
//--------------------------------------------------------------------------
// fills a vector with zeros.
void NSUtility::zeroise(vector<int> &array, int n)
{
array.clear();
for(int j = 0; j < n; ++j)
array.push_back(0);
}
//--------------------------------------------------------------------------
// fills a (m by n) matrix with zeros.
void NSUtility::zeroise(vector<vector<double> > &matrix, int m, int n)
{
vector<double> zero;
zeroise(zero, n);
matrix.clear();
for(int j = 0; j < m; ++j)
matrix.push_back(zero);
}
//--------------------------------------------------------------------------
// fills a (m by n) matrix with zeros.
void NSUtility::zeroise(vector<vector<int> > &matrix, int m, int n)
{
vector<int> zero;
zeroise(zero, n);
matrix.clear();
for(int j = 0; j < m; ++j)
matrix.push_back(zero);
}
//--------------------------------------------------------------------------
#endif