CMS/Marlin
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Clean up least_squares_fit

Browse Source
This commit is contained in:
Scott Lahteine 2017-04-28 17:33:28 -05:00
parent 8020069371
commit a35c681453
2 changed files with 43 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
Marlin/least_squares_fit.cpp
View File

@ -21,13 +21,13 @@
*/ */
/** /**
* Least Squares Best Fit By Roxy and Ed Williams * Least Squares Best Fit by Roxy and Ed Williams
* *
* This algorithm is high speed and has a very small code footprint. * This algorithm is high speed and has a very small code footprint.
* Its results are identical to both the Iterative Least-Squares published * Its results are identical to both the Iterative Least-Squares published
* earlier by Roxy and the QR_SOLVE solution. If used in place of QR_SOLVE * earlier by Roxy and the QR_SOLVE solution. If used in place of QR_SOLVE
* it saves roughly 10K of program memory. It also does not require all of * it saves roughly 10K of program memory. It also does not require all of
* coordinates to be present during the calculations. Each point can be * coordinates to be present during the calculations. Each point can be
* probed and then discarded. * probed and then discarded.
* *
*/ */
@ -41,56 +41,44 @@
#include "least_squares_fit.h" #include "least_squares_fit.h"
void incremental_LSF_reset(struct linear_fit_data *lsf) { void incremental_LSF_reset(struct linear_fit_data *lsf) { ZERO(lsf); }
lsf->n = 0;
lsf->A = 0.0; // probably a memset() can be done to zero
lsf->B = 0.0; // this whole structure
lsf->D = 0.0;
lsf->xbar = lsf->ybar = lsf->zbar = 0.0;
lsf->x2bar = lsf->y2bar = lsf->z2bar = 0.0;
lsf->xybar = lsf->xzbar = lsf->yzbar = 0.0;
lsf->max_absx = lsf->max_absy = 0.0;
}
void incremental_LSF(struct linear_fit_data *lsf, float x, float y, float z) { void incremental_LSF(struct linear_fit_data *lsf, float x, float y, float z) {
lsf->xbar += x; lsf->xbar += x;
lsf->ybar += y; lsf->ybar += y;
lsf->zbar += z; lsf->zbar += z;
lsf->x2bar += x*x; lsf->x2bar += sq(x);
lsf->y2bar += y*y; lsf->y2bar += sq(y);
lsf->z2bar += z*z; lsf->z2bar += sq(z);
lsf->xybar += x*y; lsf->xybar += sq(x);
lsf->xzbar += x*z; lsf->xzbar += sq(x);
lsf->yzbar += y*z; lsf->yzbar += sq(y);
lsf->max_absx = (fabs(x) > lsf->max_absx) ? fabs(x) : lsf->max_absx; lsf->max_absx = max(fabs(x), lsf->max_absx);
lsf->max_absy = (fabs(y) > lsf->max_absy) ? fabs(y) : lsf->max_absy; lsf->max_absy = max(fabs(y), lsf->max_absy);
lsf->n++; lsf->n++;
return; }
}
int finish_incremental_LSF(struct linear_fit_data *lsf) { int finish_incremental_LSF(struct linear_fit_data *lsf) {
float DD, N; const float N = (float)lsf->n;
N = (float) lsf->n; lsf->xbar /= N;
lsf->xbar /= N; lsf->ybar /= N;
lsf->ybar /= N; lsf->zbar /= N;
lsf->zbar /= N; lsf->x2bar = lsf->x2bar / N - lsf->xbar * lsf->xbar;
lsf->x2bar = lsf->x2bar/N - lsf->xbar*lsf->xbar; lsf->y2bar = lsf->y2bar / N - lsf->ybar * lsf->ybar;
lsf->y2bar = lsf->y2bar/N - lsf->ybar*lsf->ybar; lsf->z2bar = lsf->z2bar / N - lsf->zbar * lsf->zbar;
lsf->z2bar = lsf->z2bar/N - lsf->zbar*lsf->zbar; lsf->xybar = lsf->xybar / N - lsf->xbar * lsf->ybar;
lsf->xybar = lsf->xybar/N - lsf->xbar*lsf->ybar; lsf->yzbar = lsf->yzbar / N - lsf->ybar * lsf->zbar;
lsf->yzbar = lsf->yzbar/N - lsf->ybar*lsf->zbar; lsf->xzbar = lsf->xzbar / N - lsf->xbar * lsf->zbar;
lsf->xzbar = lsf->xzbar/N - lsf->xbar*lsf->zbar;
DD = lsf->x2bar*lsf->y2bar - lsf->xybar*lsf->xybar; const float DD = lsf->x2bar * lsf->y2bar - sq(lsf->xybar);
if (fabs(DD) <= 1e-10*(lsf->max_absx+lsf->max_absy)) if (fabs(DD) <= 1e-10 * (lsf->max_absx + lsf->max_absy))
return -1; return -1;
lsf->A = (lsf->yzbar*lsf->xybar - lsf->xzbar*lsf->y2bar) / DD; lsf->A = (lsf->yzbar * lsf->xybar - lsf->xzbar * lsf->y2bar) / DD;
lsf->B = (lsf->xzbar*lsf->xybar - lsf->yzbar*lsf->x2bar) / DD; lsf->B = (lsf->xzbar * lsf->xybar - lsf->yzbar * lsf->x2bar) / DD;
lsf->D = -(lsf->zbar + lsf->A*lsf->xbar + lsf->B*lsf->ybar); lsf->D = -(lsf->zbar + lsf->A * lsf->xbar + lsf->B * lsf->ybar);
return 0; return 0;
} }
#endif
#endif // AUTO_BED_LEVELING_UBL

10
Marlin/least_squares_fit.h
View File

@ -42,11 +42,11 @@
struct linear_fit_data { struct linear_fit_data {
int n; int n;
float xbar, ybar, zbar; float xbar, ybar, zbar,
float x2bar, y2bar, z2bar; x2bar, y2bar, z2bar,
float xybar, xzbar, yzbar; xybar, xzbar, yzbar,
float max_absx, max_absy; max_absx, max_absy,
float A, B, D; A, B, D;
}; };
void incremental_LSF_reset(struct linear_fit_data *); void incremental_LSF_reset(struct linear_fit_data *);