qm-dsp  1.8
Polyfit.h
Go to the documentation of this file.
1 /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*- vi:set ts=8 sts=4 sw=4: */
2 //---------------------------------------------------------------------------
3 
4 #ifndef PolyfitHPP
5 #define PolyfitHPP
6 //---------------------------------------------------------------------------
7 // Least-squares curve fitting class for arbitrary data types
8 /*
9 
10 { ******************************************
11  **** Scientific Subroutine Library ****
12  **** for C++ Builder ****
13  ******************************************
14 
15  The following programs were written by Allen Miller and appear in the
16  book entitled "Pascal Programs For Scientists And Engineers" which is
17  published by Sybex, 1981.
18  They were originally typed and submitted to MTPUG in Oct. 1982
19  Juergen Loewner
20  Hoher Heckenweg 3
21  D-4400 Muenster
22  They have had minor corrections and adaptations for Turbo Pascal by
23  Jeff Weiss
24  1572 Peacock Ave.
25  Sunnyvale, CA 94087.
26 
27 
28 2000 Oct 28 Updated for Delphi 4, open array parameters.
29  This allows the routine to be generalised so that it is no longer
30  hard-coded to make a specific order of best fit or work with a
31  specific number of points.
32 2001 Jan 07 Update Web site address
33 
34 Copyright © David J Taylor, Edinburgh and others listed above
35 Web site: www.satsignal.net
36 E-mail: davidtaylor@writeme.com
37 }*/
38 
40  // Modified by CLandone for VC6 Aug 2004
42 
43 #include <iostream>
44 
45 using std::vector;
46 
47 class TPolyFit
48 {
49  typedef vector<vector<double> > Matrix;
50 public:
51 
52  static double PolyFit2 (const vector<double> &x, // does the work
53  const vector<double> &y,
54  vector<double> &coef);
55 
56 
57 private:
58  TPolyFit &operator = (const TPolyFit &); // disable assignment
59  TPolyFit(); // and instantiation
60  TPolyFit(const TPolyFit&); // and copying
61 
62 
63  static void Square (const Matrix &x, // Matrix multiplication routine
64  const vector<double> &y,
65  Matrix &a, // A = transpose X times X
66  vector<double> &g, // G = Y times X
67  const int nrow, const int ncol);
68  // Forms square coefficient matrix
69 
70  static bool GaussJordan (Matrix &b, // square matrix of coefficients
71  const vector<double> &y, // constant vector
72  vector<double> &coef); // solution vector
73  // returns false if matrix singular
74 
75  static bool GaussJordan2(Matrix &b,
76  const vector<double> &y,
77  Matrix &w,
78  vector<vector<int> > &index);
79 };
80 
81 // some utility functions
82 
83 namespace NSUtility
84 {
85  inline void swap(double &a, double &b) {double t = a; a = b; b = t;}
86  void zeroise(vector<double> &array, int n);
87  void zeroise(vector<int> &array, int n);
88  void zeroise(vector<vector<double> > &matrix, int m, int n);
89  void zeroise(vector<vector<int> > &matrix, int m, int n);
90  inline double sqr(const double &x) {return x * x;}
91 };
92 
93 //---------------------------------------------------------------------------
94 // Implementation
95 //---------------------------------------------------------------------------
96 using namespace NSUtility;
97 //------------------------------------------------------------------------------------------
98 
99 
100 // main PolyFit routine
101 
102 double TPolyFit::PolyFit2 (const vector<double> &x,
103  const vector<double> &y,
104  vector<double> &coefs)
105 // nterms = coefs.size()
106 // npoints = x.size()
107 {
108  int i, j;
109  double xi, yi, yc, srs, sum_y, sum_y2;
110  Matrix xmatr; // Data matrix
111  Matrix a;
112  vector<double> g; // Constant vector
113  const int npoints(x.size());
114  const int nterms(coefs.size());
115  double correl_coef;
116  zeroise(g, nterms);
117  zeroise(a, nterms, nterms);
118  zeroise(xmatr, npoints, nterms);
119  if (nterms < 1) {
120  std::cerr << "ERROR: PolyFit called with less than one term" << std::endl;
121  return 0;
122  }
123  if(npoints < 2) {
124  std::cerr << "ERROR: PolyFit called with less than two points" << std::endl;
125  return 0;
126  }
127  if(npoints != (int)y.size()) {
128  std::cerr << "ERROR: PolyFit called with x and y of unequal size" << std::endl;
129  return 0;
130  }
131  for(i = 0; i < npoints; ++i)
132  {
133  // { setup x matrix }
134  xi = x[i];
135  xmatr[i][0] = 1.0; // { first column }
136  for(j = 1; j < nterms; ++j)
137  xmatr[i][j] = xmatr [i][j - 1] * xi;
138  }
139  Square (xmatr, y, a, g, npoints, nterms);
140  if(!GaussJordan (a, g, coefs))
141  return -1;
142  sum_y = 0.0;
143  sum_y2 = 0.0;
144  srs = 0.0;
145  for(i = 0; i < npoints; ++i)
146  {
147  yi = y[i];
148  yc = 0.0;
149  for(j = 0; j < nterms; ++j)
150  yc += coefs [j] * xmatr [i][j];
151  srs += sqr (yc - yi);
152  sum_y += yi;
153  sum_y2 += yi * yi;
154  }
155 
156  // If all Y values are the same, avoid dividing by zero
157  correl_coef = sum_y2 - sqr (sum_y) / npoints;
158  // Either return 0 or the correct value of correlation coefficient
159  if (correl_coef != 0)
160  correl_coef = srs / correl_coef;
161  if (correl_coef >= 1)
162  correl_coef = 0.0;
163  else
164  correl_coef = sqrt (1.0 - correl_coef);
165  return correl_coef;
166 }
167 
168 
169 //------------------------------------------------------------------------
170 
171 // Matrix multiplication routine
172 // A = transpose X times X
173 // G = Y times X
174 
175 // Form square coefficient matrix
176 
177 void TPolyFit::Square (const Matrix &x,
178  const vector<double> &y,
179  Matrix &a,
180  vector<double> &g,
181  const int nrow,
182  const int ncol)
183 {
184  int i, k, l;
185  for(k = 0; k < ncol; ++k)
186  {
187  for(l = 0; l < k + 1; ++l)
188  {
189  a [k][l] = 0.0;
190  for(i = 0; i < nrow; ++i)
191  {
192  a[k][l] += x[i][l] * x [i][k];
193  if(k != l)
194  a[l][k] = a[k][l];
195  }
196  }
197  g[k] = 0.0;
198  for(i = 0; i < nrow; ++i)
199  g[k] += y[i] * x[i][k];
200  }
201 }
202 //---------------------------------------------------------------------------------
203 
204 
206  const vector<double> &y,
207  vector<double> &coef)
208 //b square matrix of coefficients
209 //y constant vector
210 //coef solution vector
211 //ncol order of matrix got from b.size()
212 
213 
214 {
215 /*
216  { Gauss Jordan matrix inversion and solution }
217 
218  { B (n, n) coefficient matrix becomes inverse }
219  { Y (n) original constant vector }
220  { W (n, m) constant vector(s) become solution vector }
221  { DETERM is the determinant }
222  { ERROR = 1 if singular }
223  { INDEX (n, 3) }
224  { NV is number of constant vectors }
225 */
226 
227  int ncol(b.size());
228  int irow, icol;
229  vector<vector<int> >index;
230  Matrix w;
231 
232  zeroise(w, ncol, ncol);
233  zeroise(index, ncol, 3);
234 
235  if(!GaussJordan2(b, y, w, index))
236  return false;
237 
238  // Interchange columns
239  int m;
240  for (int i = 0; i < ncol; ++i)
241  {
242  m = ncol - i - 1;
243  if(index [m][0] != index [m][1])
244  {
245  irow = index [m][0];
246  icol = index [m][1];
247  for(int k = 0; k < ncol; ++k)
248  swap (b[k][irow], b[k][icol]);
249  }
250  }
251 
252  for(int k = 0; k < ncol; ++k)
253  {
254  if(index [k][2] != 0)
255  {
256  std::cerr << "ERROR: Error in PolyFit::GaussJordan: matrix is singular" << std::endl;
257  return false;
258  }
259  }
260 
261  for( int i = 0; i < ncol; ++i)
262  coef[i] = w[i][0];
263 
264 
265  return true;
266 } // end; { procedure GaussJordan }
267 //----------------------------------------------------------------------------------------------
268 
269 
271  const vector<double> &y,
272  Matrix &w,
273  vector<vector<int> > &index)
274 {
275  //GaussJordan2; // first half of GaussJordan
276  // actual start of gaussj
277 
278  double big, t;
279  double pivot;
280  double determ;
281  int irow, icol;
282  int ncol(b.size());
283  int nv = 1; // single constant vector
284  for(int i = 0; i < ncol; ++i)
285  {
286  w[i][0] = y[i]; // copy constant vector
287  index[i][2] = -1;
288  }
289  determ = 1.0;
290  for(int i = 0; i < ncol; ++i)
291  {
292  // Search for largest element
293  big = 0.0;
294  for(int j = 0; j < ncol; ++j)
295  {
296  if(index[j][2] != 0)
297  {
298  for(int k = 0; k < ncol; ++k)
299  {
300  if(index[k][2] > 0) {
301  std::cerr << "ERROR: Error in PolyFit::GaussJordan2: matrix is singular" << std::endl;
302  return false;
303  }
304 
305  if(index[k][2] < 0 && fabs(b[j][k]) > big)
306  {
307  irow = j;
308  icol = k;
309  big = fabs(b[j][k]);
310  }
311  } // { k-loop }
312  }
313  } // { j-loop }
314  index [icol][2] = index [icol][2] + 1;
315  index [i][0] = irow;
316  index [i][1] = icol;
317 
318  // Interchange rows to put pivot on diagonal
319  // GJ3
320  if(irow != icol)
321  {
322  determ = -determ;
323  for(int m = 0; m < ncol; ++m)
324  swap (b [irow][m], b[icol][m]);
325  if (nv > 0)
326  for (int m = 0; m < nv; ++m)
327  swap (w[irow][m], w[icol][m]);
328  } // end GJ3
329 
330  // divide pivot row by pivot column
331  pivot = b[icol][icol];
332  determ *= pivot;
333  b[icol][icol] = 1.0;
334 
335  for(int m = 0; m < ncol; ++m)
336  b[icol][m] /= pivot;
337  if(nv > 0)
338  for(int m = 0; m < nv; ++m)
339  w[icol][m] /= pivot;
340 
341  // Reduce nonpivot rows
342  for(int n = 0; n < ncol; ++n)
343  {
344  if(n != icol)
345  {
346  t = b[n][icol];
347  b[n][icol] = 0.0;
348  for(int m = 0; m < ncol; ++m)
349  b[n][m] -= b[icol][m] * t;
350  if(nv > 0)
351  for(int m = 0; m < nv; ++m)
352  w[n][m] -= w[icol][m] * t;
353  }
354  }
355  } // { i-loop }
356  return true;
357 }
358 //----------------------------------------------------------------------------------------------
359 
360 //------------------------------------------------------------------------------------
361 
362 // Utility functions
363 //--------------------------------------------------------------------------
364 
365 // fills a vector with zeros.
366 void NSUtility::zeroise(vector<double> &array, int n)
367 {
368  array.clear();
369  for(int j = 0; j < n; ++j)
370  array.push_back(0);
371 }
372 //--------------------------------------------------------------------------
373 
374 // fills a vector with zeros.
375 void NSUtility::zeroise(vector<int> &array, int n)
376 {
377  array.clear();
378  for(int j = 0; j < n; ++j)
379  array.push_back(0);
380 }
381 //--------------------------------------------------------------------------
382 
383 // fills a (m by n) matrix with zeros.
384 void NSUtility::zeroise(vector<vector<double> > &matrix, int m, int n)
385 {
386  vector<double> zero;
387  zeroise(zero, n);
388  matrix.clear();
389  for(int j = 0; j < m; ++j)
390  matrix.push_back(zero);
391 }
392 //--------------------------------------------------------------------------
393 
394 // fills a (m by n) matrix with zeros.
395 void NSUtility::zeroise(vector<vector<int> > &matrix, int m, int n)
396 {
397  vector<int> zero;
398  zeroise(zero, n);
399  matrix.clear();
400  for(int j = 0; j < m; ++j)
401  matrix.push_back(zero);
402 }
403 //--------------------------------------------------------------------------
404 
405 
406 #endif
407 
static double PolyFit2(const vector< double > &x, const vector< double > &y, vector< double > &coef)
Definition: Polyfit.h:102
double sqr(const double &x)
Definition: Polyfit.h:90
void swap(double &a, double &b)
Definition: Polyfit.h:85
vector< vector< double > > Matrix
Definition: Polyfit.h:49
void zeroise(vector< double > &array, int n)
Definition: Polyfit.h:366
static void Square(const Matrix &x, const vector< double > &y, Matrix &a, vector< double > &g, const int nrow, const int ncol)
Definition: Polyfit.h:177
TPolyFit & operator=(const TPolyFit &)
static bool GaussJordan2(Matrix &b, const vector< double > &y, Matrix &w, vector< vector< int > > &index)
Definition: Polyfit.h:270
static bool GaussJordan(Matrix &b, const vector< double > &y, vector< double > &coef)
Definition: Polyfit.h:205
void zeroise(vector< vector< int > > &matrix, int m, int n)
Definition: Polyfit.h:395