Mercurial > hg > qm-dsp
comparison maths/Polyfit.h @ 477:fa407c1d9923
Untabify + indent
author | Chris Cannam <cannam@all-day-breakfast.com> |
---|---|
date | Thu, 30 May 2019 18:36:48 +0100 |
parents | 7b7691b49197 |
children | 7132d952b9a6 |
comparison
equal
deleted
inserted
replaced
476:2de6184b2ce0 | 477:fa407c1d9923 |
---|---|
48 { | 48 { |
49 typedef vector<vector<double> > Matrix; | 49 typedef vector<vector<double> > Matrix; |
50 public: | 50 public: |
51 | 51 |
52 static double PolyFit2 (const vector<double> &x, // does the work | 52 static double PolyFit2 (const vector<double> &x, // does the work |
53 const vector<double> &y, | 53 const vector<double> &y, |
54 vector<double> &coef); | 54 vector<double> &coef); |
55 | 55 |
56 | 56 |
57 private: | 57 private: |
58 TPolyFit &operator = (const TPolyFit &); // disable assignment | 58 TPolyFit &operator = (const TPolyFit &); // disable assignment |
59 TPolyFit(); // and instantiation | 59 TPolyFit(); // and instantiation |
60 TPolyFit(const TPolyFit&); // and copying | 60 TPolyFit(const TPolyFit&); // and copying |
61 | 61 |
62 | 62 |
63 static void Square (const Matrix &x, // Matrix multiplication routine | 63 static void Square (const Matrix &x, // Matrix multiplication routine |
64 const vector<double> &y, | 64 const vector<double> &y, |
65 Matrix &a, // A = transpose X times X | 65 Matrix &a, // A = transpose X times X |
66 vector<double> &g, // G = Y times X | 66 vector<double> &g, // G = Y times X |
67 const int nrow, const int ncol); | 67 const int nrow, const int ncol); |
68 // Forms square coefficient matrix | 68 // Forms square coefficient matrix |
69 | 69 |
70 static bool GaussJordan (Matrix &b, // square matrix of coefficients | 70 static bool GaussJordan (Matrix &b, // square matrix of coefficients |
71 const vector<double> &y, // constant vector | 71 const vector<double> &y, // constant vector |
72 vector<double> &coef); // solution vector | 72 vector<double> &coef); // solution vector |
73 // returns false if matrix singular | 73 // returns false if matrix singular |
74 | 74 |
75 static bool GaussJordan2(Matrix &b, | 75 static bool GaussJordan2(Matrix &b, |
76 const vector<double> &y, | 76 const vector<double> &y, |
77 Matrix &w, | 77 Matrix &w, |
78 vector<vector<int> > &index); | 78 vector<vector<int> > &index); |
79 }; | 79 }; |
80 | 80 |
81 // some utility functions | 81 // some utility functions |
82 | 82 |
83 struct NSUtility | 83 struct NSUtility |
112 | 112 |
113 | 113 |
114 // main PolyFit routine | 114 // main PolyFit routine |
115 | 115 |
116 double TPolyFit::PolyFit2 (const vector<double> &x, | 116 double TPolyFit::PolyFit2 (const vector<double> &x, |
117 const vector<double> &y, | 117 const vector<double> &y, |
118 vector<double> &coefs) | 118 vector<double> &coefs) |
119 // nterms = coefs.size() | 119 // nterms = coefs.size() |
120 // npoints = x.size() | 120 // npoints = x.size() |
121 { | 121 { |
122 int i, j; | 122 int i, j; |
123 double xi, yi, yc, srs, sum_y, sum_y2; | 123 double xi, yi, yc, srs, sum_y, sum_y2; |
142 std::cerr << "ERROR: PolyFit called with x and y of unequal size" << std::endl; | 142 std::cerr << "ERROR: PolyFit called with x and y of unequal size" << std::endl; |
143 return 0; | 143 return 0; |
144 } | 144 } |
145 for(i = 0; i < npoints; ++i) | 145 for(i = 0; i < npoints; ++i) |
146 { | 146 { |
147 // { setup x matrix } | 147 // { setup x matrix } |
148 xi = x[i]; | 148 xi = x[i]; |
149 xmatr[i][0] = 1.0; // { first column } | 149 xmatr[i][0] = 1.0; // { first column } |
150 for(j = 1; j < nterms; ++j) | 150 for(j = 1; j < nterms; ++j) |
151 xmatr[i][j] = xmatr [i][j - 1] * xi; | 151 xmatr[i][j] = xmatr [i][j - 1] * xi; |
152 } | 152 } |
153 Square (xmatr, y, a, g, npoints, nterms); | 153 Square (xmatr, y, a, g, npoints, nterms); |
154 if(!GaussJordan (a, g, coefs)) | 154 if(!GaussJordan (a, g, coefs)) |
155 return -1; | 155 return -1; |
156 sum_y = 0.0; | 156 sum_y = 0.0; |
157 sum_y2 = 0.0; | 157 sum_y2 = 0.0; |
158 srs = 0.0; | 158 srs = 0.0; |
159 for(i = 0; i < npoints; ++i) | 159 for(i = 0; i < npoints; ++i) |
160 { | 160 { |
161 yi = y[i]; | 161 yi = y[i]; |
162 yc = 0.0; | 162 yc = 0.0; |
163 for(j = 0; j < nterms; ++j) | 163 for(j = 0; j < nterms; ++j) |
164 yc += coefs [j] * xmatr [i][j]; | 164 yc += coefs [j] * xmatr [i][j]; |
165 srs += NSUtility::sqr (yc - yi); | 165 srs += NSUtility::sqr (yc - yi); |
166 sum_y += yi; | 166 sum_y += yi; |
167 sum_y2 += yi * yi; | 167 sum_y2 += yi * yi; |
168 } | 168 } |
169 | 169 |
170 // If all Y values are the same, avoid dividing by zero | 170 // If all Y values are the same, avoid dividing by zero |
171 correl_coef = sum_y2 - NSUtility::sqr (sum_y) / npoints; | 171 correl_coef = sum_y2 - NSUtility::sqr (sum_y) / npoints; |
172 // Either return 0 or the correct value of correlation coefficient | 172 // Either return 0 or the correct value of correlation coefficient |
173 if (correl_coef != 0) | 173 if (correl_coef != 0) |
174 correl_coef = srs / correl_coef; | 174 correl_coef = srs / correl_coef; |
175 if (correl_coef >= 1) | 175 if (correl_coef >= 1) |
176 correl_coef = 0.0; | 176 correl_coef = 0.0; |
177 else | 177 else |
178 correl_coef = sqrt (1.0 - correl_coef); | 178 correl_coef = sqrt (1.0 - correl_coef); |
179 return correl_coef; | 179 return correl_coef; |
180 } | 180 } |
181 | 181 |
182 | 182 |
183 //------------------------------------------------------------------------ | 183 //------------------------------------------------------------------------ |
187 // G = Y times X | 187 // G = Y times X |
188 | 188 |
189 // Form square coefficient matrix | 189 // Form square coefficient matrix |
190 | 190 |
191 void TPolyFit::Square (const Matrix &x, | 191 void TPolyFit::Square (const Matrix &x, |
192 const vector<double> &y, | 192 const vector<double> &y, |
193 Matrix &a, | 193 Matrix &a, |
194 vector<double> &g, | 194 vector<double> &g, |
195 const int nrow, | 195 const int nrow, |
196 const int ncol) | 196 const int ncol) |
197 { | 197 { |
198 int i, k, l; | 198 int i, k, l; |
199 for(k = 0; k < ncol; ++k) | 199 for(k = 0; k < ncol; ++k) |
200 { | 200 { |
201 for(l = 0; l < k + 1; ++l) | 201 for(l = 0; l < k + 1; ++l) |
202 { | 202 { |
203 a [k][l] = 0.0; | 203 a [k][l] = 0.0; |
204 for(i = 0; i < nrow; ++i) | 204 for(i = 0; i < nrow; ++i) |
205 { | 205 { |
206 a[k][l] += x[i][l] * x [i][k]; | 206 a[k][l] += x[i][l] * x [i][k]; |
207 if(k != l) | 207 if(k != l) |
208 a[l][k] = a[k][l]; | 208 a[l][k] = a[k][l]; |
209 } | 209 } |
210 } | 210 } |
211 g[k] = 0.0; | 211 g[k] = 0.0; |
212 for(i = 0; i < nrow; ++i) | 212 for(i = 0; i < nrow; ++i) |
213 g[k] += y[i] * x[i][k]; | 213 g[k] += y[i] * x[i][k]; |
214 } | 214 } |
215 } | 215 } |
216 //--------------------------------------------------------------------------------- | 216 //--------------------------------------------------------------------------------- |
217 | 217 |
218 | 218 |
219 bool TPolyFit::GaussJordan (Matrix &b, | 219 bool TPolyFit::GaussJordan (Matrix &b, |
220 const vector<double> &y, | 220 const vector<double> &y, |
221 vector<double> &coef) | 221 vector<double> &coef) |
222 //b square matrix of coefficients | 222 //b square matrix of coefficients |
223 //y constant vector | 223 //y constant vector |
224 //coef solution vector | 224 //coef solution vector |
225 //ncol order of matrix got from b.size() | 225 //ncol order of matrix got from b.size() |
226 | 226 |
245 | 245 |
246 NSUtility::zeroise(w, ncol, ncol); | 246 NSUtility::zeroise(w, ncol, ncol); |
247 NSUtility::zeroise(index, ncol, 3); | 247 NSUtility::zeroise(index, ncol, 3); |
248 | 248 |
249 if(!GaussJordan2(b, y, w, index)) | 249 if(!GaussJordan2(b, y, w, index)) |
250 return false; | 250 return false; |
251 | 251 |
252 // Interchange columns | 252 // Interchange columns |
253 int m; | 253 int m; |
254 for (int i = 0; i < ncol; ++i) | 254 for (int i = 0; i < ncol; ++i) |
255 { | 255 { |
256 m = ncol - i - 1; | 256 m = ncol - i - 1; |
257 if(index [m][0] != index [m][1]) | 257 if(index [m][0] != index [m][1]) |
258 { | 258 { |
259 irow = index [m][0]; | 259 irow = index [m][0]; |
260 icol = index [m][1]; | 260 icol = index [m][1]; |
261 for(int k = 0; k < ncol; ++k) | 261 for(int k = 0; k < ncol; ++k) |
262 swap (b[k][irow], b[k][icol]); | 262 swap (b[k][irow], b[k][icol]); |
263 } | 263 } |
264 } | 264 } |
265 | 265 |
266 for(int k = 0; k < ncol; ++k) | 266 for(int k = 0; k < ncol; ++k) |
267 { | 267 { |
268 if(index [k][2] != 0) | 268 if(index [k][2] != 0) |
269 { | 269 { |
270 std::cerr << "ERROR: Error in PolyFit::GaussJordan: matrix is singular" << std::endl; | 270 std::cerr << "ERROR: Error in PolyFit::GaussJordan: matrix is singular" << std::endl; |
271 return false; | 271 return false; |
272 } | 272 } |
273 } | 273 } |
274 | 274 |
275 for( int i = 0; i < ncol; ++i) | 275 for( int i = 0; i < ncol; ++i) |
276 coef[i] = w[i][0]; | 276 coef[i] = w[i][0]; |
277 | 277 |
278 | 278 |
279 return true; | 279 return true; |
280 } // end; { procedure GaussJordan } | 280 } // end; { procedure GaussJordan } |
281 //---------------------------------------------------------------------------------------------- | 281 //---------------------------------------------------------------------------------------------- |
282 | 282 |
283 | 283 |
284 bool TPolyFit::GaussJordan2(Matrix &b, | 284 bool TPolyFit::GaussJordan2(Matrix &b, |
285 const vector<double> &y, | 285 const vector<double> &y, |
286 Matrix &w, | 286 Matrix &w, |
287 vector<vector<int> > &index) | 287 vector<vector<int> > &index) |
288 { | 288 { |
289 //GaussJordan2; // first half of GaussJordan | 289 //GaussJordan2; // first half of GaussJordan |
290 // actual start of gaussj | 290 // actual start of gaussj |
291 | 291 |
292 double big, t; | 292 double big, t; |
295 int irow = 0, icol = 0; | 295 int irow = 0, icol = 0; |
296 int ncol(b.size()); | 296 int ncol(b.size()); |
297 int nv = 1; // single constant vector | 297 int nv = 1; // single constant vector |
298 for(int i = 0; i < ncol; ++i) | 298 for(int i = 0; i < ncol; ++i) |
299 { | 299 { |
300 w[i][0] = y[i]; // copy constant vector | 300 w[i][0] = y[i]; // copy constant vector |
301 index[i][2] = -1; | 301 index[i][2] = -1; |
302 } | 302 } |
303 determ = 1.0; | 303 determ = 1.0; |
304 for(int i = 0; i < ncol; ++i) | 304 for(int i = 0; i < ncol; ++i) |
305 { | 305 { |
306 // Search for largest element | 306 // Search for largest element |
307 big = 0.0; | 307 big = 0.0; |
308 for(int j = 0; j < ncol; ++j) | 308 for(int j = 0; j < ncol; ++j) |
309 { | 309 { |
310 if(index[j][2] != 0) | 310 if(index[j][2] != 0) |
311 { | 311 { |
312 for(int k = 0; k < ncol; ++k) | 312 for(int k = 0; k < ncol; ++k) |
313 { | 313 { |
314 if(index[k][2] > 0) { | 314 if(index[k][2] > 0) { |
315 std::cerr << "ERROR: Error in PolyFit::GaussJordan2: matrix is singular" << std::endl; | 315 std::cerr << "ERROR: Error in PolyFit::GaussJordan2: matrix is singular" << std::endl; |
316 return false; | 316 return false; |
317 } | 317 } |
318 | 318 |
319 if(index[k][2] < 0 && fabs(b[j][k]) > big) | 319 if(index[k][2] < 0 && fabs(b[j][k]) > big) |
320 { | 320 { |
321 irow = j; | 321 irow = j; |
322 icol = k; | 322 icol = k; |
323 big = fabs(b[j][k]); | 323 big = fabs(b[j][k]); |
324 } | 324 } |
325 } // { k-loop } | 325 } // { k-loop } |
326 } | 326 } |
327 } // { j-loop } | 327 } // { j-loop } |
328 index [icol][2] = index [icol][2] + 1; | 328 index [icol][2] = index [icol][2] + 1; |
329 index [i][0] = irow; | 329 index [i][0] = irow; |
330 index [i][1] = icol; | 330 index [i][1] = icol; |
331 | 331 |
332 // Interchange rows to put pivot on diagonal | 332 // Interchange rows to put pivot on diagonal |
333 // GJ3 | 333 // GJ3 |
334 if(irow != icol) | 334 if(irow != icol) |
335 { | 335 { |
336 determ = -determ; | 336 determ = -determ; |
337 for(int m = 0; m < ncol; ++m) | 337 for(int m = 0; m < ncol; ++m) |
338 swap (b [irow][m], b[icol][m]); | 338 swap (b [irow][m], b[icol][m]); |
339 if (nv > 0) | 339 if (nv > 0) |
340 for (int m = 0; m < nv; ++m) | 340 for (int m = 0; m < nv; ++m) |
341 swap (w[irow][m], w[icol][m]); | 341 swap (w[irow][m], w[icol][m]); |
342 } // end GJ3 | 342 } // end GJ3 |
343 | 343 |
344 // divide pivot row by pivot column | 344 // divide pivot row by pivot column |
345 pivot = b[icol][icol]; | 345 pivot = b[icol][icol]; |
346 determ *= pivot; | 346 determ *= pivot; |
347 b[icol][icol] = 1.0; | 347 b[icol][icol] = 1.0; |
348 | 348 |
349 for(int m = 0; m < ncol; ++m) | 349 for(int m = 0; m < ncol; ++m) |
350 b[icol][m] /= pivot; | 350 b[icol][m] /= pivot; |
351 if(nv > 0) | 351 if(nv > 0) |
352 for(int m = 0; m < nv; ++m) | 352 for(int m = 0; m < nv; ++m) |
353 w[icol][m] /= pivot; | 353 w[icol][m] /= pivot; |
354 | 354 |
355 // Reduce nonpivot rows | 355 // Reduce nonpivot rows |
356 for(int n = 0; n < ncol; ++n) | 356 for(int n = 0; n < ncol; ++n) |
357 { | 357 { |
358 if(n != icol) | 358 if(n != icol) |
359 { | 359 { |
360 t = b[n][icol]; | 360 t = b[n][icol]; |
361 b[n][icol] = 0.0; | 361 b[n][icol] = 0.0; |
362 for(int m = 0; m < ncol; ++m) | 362 for(int m = 0; m < ncol; ++m) |
363 b[n][m] -= b[icol][m] * t; | 363 b[n][m] -= b[icol][m] * t; |
364 if(nv > 0) | 364 if(nv > 0) |
365 for(int m = 0; m < nv; ++m) | 365 for(int m = 0; m < nv; ++m) |
366 w[n][m] -= w[icol][m] * t; | 366 w[n][m] -= w[icol][m] * t; |
367 } | 367 } |
368 } | 368 } |
369 } // { i-loop } | 369 } // { i-loop } |
370 return true; | 370 return true; |
371 } | 371 } |
372 | 372 |
373 #endif | 373 #endif |