qm-dsp  1.8
Polyfit.h
Go to the documentation of this file.
00001 /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
00002 //---------------------------------------------------------------------------
00003 
00004 #ifndef PolyfitHPP
00005 #define PolyfitHPP
00006 //---------------------------------------------------------------------------
00007 // Least-squares curve fitting class for arbitrary data types
00008 /*
00009 
00010 { ******************************************
00011   ****   Scientific Subroutine Library  ****
00012   ****         for C++ Builder          ****
00013   ******************************************
00014 
00015   The following programs were written by Allen Miller and appear in the
00016   book entitled "Pascal Programs For Scientists And Engineers" which is
00017   published by Sybex, 1981.
00018   They were originally typed and submitted to MTPUG in Oct. 1982
00019     Juergen Loewner
00020     Hoher Heckenweg 3
00021     D-4400 Muenster
00022   They have had minor corrections and adaptations for Turbo Pascal by
00023     Jeff Weiss
00024     1572 Peacock Ave.
00025     Sunnyvale, CA 94087.
00026 
00027 
00028 2000 Oct 28  Updated for Delphi 4, open array parameters.
00029              This allows the routine to be generalised so that it is no longer
00030              hard-coded to make a specific order of best fit or work with a
00031              specific number of points.
00032 2001 Jan 07  Update Web site address
00033 
00034 Copyright © David J Taylor, Edinburgh and others listed above
00035 Web site:  www.satsignal.net
00036 E-mail:    davidtaylor@writeme.com
00037 }*/
00038 
00040  // Modified by CLandone for VC6 Aug 2004
00042 
00043 #include <iostream>
00044 
00045 using std::vector;
00046 
00047 class TPolyFit
00048 {
00049     typedef vector<vector<double> > Matrix;
00050 public:
00051 
00052     static double PolyFit2 (const vector<double> &x,  // does the work
00053                             const vector<double> &y,
00054                             vector<double> &coef);
00055 
00056                    
00057 private:
00058     TPolyFit &operator = (const TPolyFit &);   // disable assignment
00059     TPolyFit();                                // and instantiation
00060     TPolyFit(const TPolyFit&);                 // and copying
00061 
00062   
00063     static void Square (const Matrix &x,              // Matrix multiplication routine
00064                         const vector<double> &y,
00065                         Matrix &a,                    // A = transpose X times X
00066                         vector<double> &g,         // G = Y times X
00067                         const int nrow, const int ncol);
00068     // Forms square coefficient matrix
00069 
00070     static bool GaussJordan (Matrix &b,                  // square matrix of coefficients
00071                              const vector<double> &y, // constant vector
00072                              vector<double> &coef);   // solution vector
00073     // returns false if matrix singular
00074 
00075     static bool GaussJordan2(Matrix &b,
00076                              const vector<double> &y,
00077                              Matrix &w,
00078                              vector<vector<int> > &index);
00079 };
00080 
00081 // some utility functions
00082 
00083 namespace NSUtility
00084 {
00085     inline void swap(double &a, double &b) {double t = a; a = b; b = t;}
00086     void zeroise(vector<double> &array, int n);
00087     void zeroise(vector<int> &array, int n);
00088     void zeroise(vector<vector<double> > &matrix, int m, int n);
00089     void zeroise(vector<vector<int> > &matrix, int m, int n);
00090     inline double sqr(const double &x) {return x * x;}
00091 };
00092 
00093 //---------------------------------------------------------------------------
00094 // Implementation
00095 //---------------------------------------------------------------------------
00096 using namespace NSUtility;
00097 //------------------------------------------------------------------------------------------
00098 
00099 
00100 // main PolyFit routine
00101 
00102 double TPolyFit::PolyFit2 (const vector<double> &x,
00103                            const vector<double> &y,
00104                            vector<double> &coefs)
00105 // nterms = coefs.size()
00106 // npoints = x.size()
00107 {
00108     int i, j;
00109     double xi, yi, yc, srs, sum_y, sum_y2;
00110     Matrix xmatr;        // Data matrix
00111     Matrix a;
00112     vector<double> g;      // Constant vector
00113     const int npoints(x.size());
00114     const int nterms(coefs.size());
00115     double correl_coef;
00116     zeroise(g, nterms);
00117     zeroise(a, nterms, nterms);
00118     zeroise(xmatr, npoints, nterms);
00119     if (nterms < 1) {
00120         std::cerr << "ERROR: PolyFit called with less than one term" << std::endl;
00121         return 0;
00122     }
00123     if(npoints < 2) {
00124         std::cerr << "ERROR: PolyFit called with less than two points" << std::endl;
00125         return 0;
00126     }
00127     if(npoints != (int)y.size()) {
00128         std::cerr << "ERROR: PolyFit called with x and y of unequal size" << std::endl;
00129         return 0;
00130     }
00131     for(i = 0; i < npoints; ++i)
00132     {
00133         //      { setup x matrix }
00134         xi = x[i];
00135         xmatr[i][0] = 1.0;         //     { first column }
00136         for(j = 1; j < nterms; ++j)
00137             xmatr[i][j] = xmatr [i][j - 1] * xi;
00138     }
00139     Square (xmatr, y, a, g, npoints, nterms);
00140     if(!GaussJordan (a, g, coefs))
00141         return -1;
00142     sum_y = 0.0;
00143     sum_y2 = 0.0;
00144     srs = 0.0;
00145     for(i = 0; i < npoints; ++i)
00146     {
00147         yi = y[i];
00148         yc = 0.0;
00149         for(j = 0; j < nterms; ++j)
00150             yc += coefs [j] * xmatr [i][j];
00151         srs += sqr (yc - yi);
00152         sum_y += yi;
00153         sum_y2 += yi * yi;
00154     }
00155 
00156     // If all Y values are the same, avoid dividing by zero
00157     correl_coef = sum_y2 - sqr (sum_y) / npoints;
00158     // Either return 0 or the correct value of correlation coefficient
00159     if (correl_coef != 0)
00160         correl_coef = srs / correl_coef;
00161     if (correl_coef >= 1)
00162         correl_coef = 0.0;
00163     else
00164         correl_coef = sqrt (1.0 - correl_coef);
00165     return correl_coef;
00166 }
00167 
00168 
00169 //------------------------------------------------------------------------
00170 
00171 // Matrix multiplication routine
00172 // A = transpose X times X
00173 // G = Y times X
00174 
00175 // Form square coefficient matrix
00176 
00177 void TPolyFit::Square (const Matrix &x,
00178                        const vector<double> &y,
00179                        Matrix &a,
00180                        vector<double> &g,
00181                        const int nrow,
00182                        const int ncol)
00183 {
00184     int i, k, l;
00185     for(k = 0; k < ncol; ++k)
00186     {
00187         for(l = 0; l < k + 1; ++l)
00188         {
00189             a [k][l] = 0.0;
00190             for(i = 0; i < nrow; ++i)
00191             {
00192                 a[k][l] += x[i][l] * x [i][k];
00193                 if(k != l)
00194                     a[l][k] = a[k][l];
00195             }
00196         }
00197         g[k] = 0.0;
00198         for(i = 0; i < nrow; ++i)
00199             g[k] += y[i] * x[i][k];
00200     }
00201 }
00202 //---------------------------------------------------------------------------------
00203 
00204 
00205 bool TPolyFit::GaussJordan (Matrix &b,
00206                             const vector<double> &y,
00207                             vector<double> &coef)
00208 //b square matrix of coefficients
00209 //y constant vector
00210 //coef solution vector
00211 //ncol order of matrix got from b.size()
00212 
00213 
00214 {
00215 /*
00216   { Gauss Jordan matrix inversion and solution }
00217 
00218   { B (n, n) coefficient matrix becomes inverse }
00219   { Y (n) original constant vector }
00220   { W (n, m) constant vector(s) become solution vector }
00221   { DETERM is the determinant }
00222   { ERROR = 1 if singular }
00223   { INDEX (n, 3) }
00224   { NV is number of constant vectors }
00225 */
00226 
00227     int ncol(b.size());
00228     int irow, icol;
00229     vector<vector<int> >index;
00230     Matrix w;
00231 
00232     zeroise(w, ncol, ncol);
00233     zeroise(index, ncol, 3);
00234 
00235     if(!GaussJordan2(b, y, w, index))
00236         return false;
00237 
00238     // Interchange columns
00239     int m;
00240     for (int i = 0; i <  ncol; ++i)
00241     {
00242         m = ncol - i - 1;
00243         if(index [m][0] != index [m][1])
00244         {
00245             irow = index [m][0];
00246             icol = index [m][1];
00247             for(int k = 0; k < ncol; ++k)
00248                 swap (b[k][irow], b[k][icol]);
00249         }
00250     }
00251 
00252     for(int k = 0; k < ncol; ++k)
00253     {
00254         if(index [k][2] != 0)
00255         {
00256             std::cerr << "ERROR: Error in PolyFit::GaussJordan: matrix is singular" << std::endl;
00257             return false;
00258         }
00259     }
00260 
00261     for( int i = 0; i < ncol; ++i)
00262         coef[i] = w[i][0];
00263  
00264  
00265     return true;
00266 }   // end;     { procedure GaussJordan }
00267 //----------------------------------------------------------------------------------------------
00268 
00269 
00270 bool TPolyFit::GaussJordan2(Matrix &b,
00271                             const vector<double> &y,
00272                             Matrix &w,
00273                             vector<vector<int> > &index)
00274 {
00275     //GaussJordan2;         // first half of GaussJordan
00276     // actual start of gaussj
00277  
00278     double big, t;
00279     double pivot;
00280     double determ;
00281     int irow, icol;
00282     int ncol(b.size());
00283     int nv = 1;                  // single constant vector
00284     for(int i = 0; i < ncol; ++i)
00285     {
00286         w[i][0] = y[i];      // copy constant vector
00287         index[i][2] = -1;
00288     }
00289     determ = 1.0;
00290     for(int i = 0; i < ncol; ++i)
00291     {
00292         // Search for largest element
00293         big = 0.0;
00294         for(int j = 0; j < ncol; ++j)
00295         {
00296             if(index[j][2] != 0)
00297             {
00298                 for(int k = 0; k < ncol; ++k)
00299                 {
00300                     if(index[k][2] > 0) {
00301                         std::cerr << "ERROR: Error in PolyFit::GaussJordan2: matrix is singular" << std::endl;
00302                         return false;
00303                     }
00304 
00305                     if(index[k][2] < 0 && fabs(b[j][k]) > big)
00306                     {
00307                         irow = j;
00308                         icol = k;
00309                         big = fabs(b[j][k]);
00310                     }
00311                 } //   { k-loop }
00312             }
00313         }  // { j-loop }
00314         index [icol][2] = index [icol][2] + 1;
00315         index [i][0] = irow;
00316         index [i][1] = icol;
00317 
00318         // Interchange rows to put pivot on diagonal
00319         // GJ3
00320         if(irow != icol)
00321         {
00322             determ = -determ;
00323             for(int m = 0; m < ncol; ++m)
00324                 swap (b [irow][m], b[icol][m]);
00325             if (nv > 0)
00326                 for (int m = 0; m < nv; ++m)
00327                     swap (w[irow][m], w[icol][m]);
00328         } // end GJ3
00329 
00330         // divide pivot row by pivot column
00331         pivot = b[icol][icol];
00332         determ *= pivot;
00333         b[icol][icol] = 1.0;
00334 
00335         for(int m = 0; m < ncol; ++m)
00336             b[icol][m] /= pivot;
00337         if(nv > 0)
00338             for(int m = 0; m < nv; ++m)
00339                 w[icol][m] /= pivot;
00340 
00341         // Reduce nonpivot rows
00342         for(int n = 0; n < ncol; ++n)
00343         {
00344             if(n != icol)
00345             {
00346                 t = b[n][icol];
00347                 b[n][icol] = 0.0;
00348                 for(int m = 0; m < ncol; ++m)
00349                     b[n][m] -= b[icol][m] * t;
00350                 if(nv > 0)
00351                     for(int m = 0; m < nv; ++m)
00352                         w[n][m] -= w[icol][m] * t;
00353             }
00354         }
00355     } // { i-loop }
00356     return true;
00357 }
00358 //----------------------------------------------------------------------------------------------
00359 
00360 //------------------------------------------------------------------------------------
00361 
00362 // Utility functions
00363 //--------------------------------------------------------------------------
00364 
00365 // fills a vector with zeros.
00366 void NSUtility::zeroise(vector<double> &array, int n)
00367 {
00368     array.clear();
00369     for(int j = 0; j < n; ++j)
00370         array.push_back(0);
00371 }
00372 //--------------------------------------------------------------------------
00373 
00374 // fills a vector with zeros.
00375 void NSUtility::zeroise(vector<int> &array, int n)
00376 {
00377     array.clear();
00378     for(int j = 0; j < n; ++j)
00379         array.push_back(0);
00380 }
00381 //--------------------------------------------------------------------------
00382 
00383 // fills a (m by n) matrix with zeros.
00384 void NSUtility::zeroise(vector<vector<double> > &matrix, int m, int n)
00385 {
00386     vector<double> zero;
00387     zeroise(zero, n);
00388     matrix.clear();
00389     for(int j = 0; j < m; ++j)
00390         matrix.push_back(zero);
00391 }
00392 //--------------------------------------------------------------------------
00393 
00394 // fills a (m by n) matrix with zeros.
00395 void NSUtility::zeroise(vector<vector<int> > &matrix, int m, int n)
00396 {
00397     vector<int> zero;
00398     zeroise(zero, n);
00399     matrix.clear();
00400     for(int j = 0; j < m; ++j)
00401         matrix.push_back(zero);
00402 }
00403 //--------------------------------------------------------------------------
00404 
00405 
00406 #endif
00407