comparison dsp/maths/Polyfit.h @ 225:49844bc8a895

* Queen Mary C++ DSP library

+/* -*- 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 © David J Taylor, Edinburgh and others listed above
+Web site:  www.satsignal.net
+E-mail:    [e-mail]
+}*/
+
+ ///////////////////////////////////////////////////////////////////////////////
+ // Modified by CLandone for VC6 Aug 2004
+ ///////////////////////////////////////////////////////////////////////////////
+
+
+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)
+	throw "PolyFit called with less than one term";
+    if(npoints < 2)
+	throw "PolyFit called with less than two points";
+    if(npoints != y.size())
+	throw "PolyFit called with x and y of unequal size";
+    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)
+	{
+	    throw "Error in GaussJordan: matrix is singular";
+	}
+    }
+
+    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)
+			throw "Error in GaussJordan2: matrix is singular";
+
+		    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
+