qm-dsp
1.8
|
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