MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests 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

84
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.
* 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
* 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
* 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
* probed and then discarded.
*
*/
@ -41,56 +41,44 @@
#include "least_squares_fit.h"
void incremental_LSF_reset(struct linear_fit_data *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_reset(struct linear_fit_data *lsf) { ZERO(lsf); }
void incremental_LSF(struct linear_fit_data *lsf, float x, float y, float z) {
lsf->xbar += x;
lsf->ybar += y;
lsf->zbar += z;
lsf->x2bar += x*x;
lsf->y2bar += y*y;
lsf->z2bar += z*z;
lsf->xybar += x*y;
lsf->xzbar += x*z;
lsf->yzbar += y*z;
lsf->max_absx = (fabs(x) > lsf->max_absx) ? fabs(x) : lsf->max_absx;
lsf->max_absy = (fabs(y) > lsf->max_absy) ? fabs(y) : lsf->max_absy;
lsf->n++;
return;
}
lsf->xbar += x;
lsf->ybar += y;
lsf->zbar += z;
lsf->x2bar += sq(x);
lsf->y2bar += sq(y);
lsf->z2bar += sq(z);
lsf->xybar += sq(x);
lsf->xzbar += sq(x);
lsf->yzbar += sq(y);
lsf->max_absx = max(fabs(x), lsf->max_absx);
lsf->max_absy = max(fabs(y), lsf->max_absy);
lsf->n++;
}
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->ybar /= N;
lsf->zbar /= N;
lsf->x2bar = lsf->x2bar/N - lsf->xbar*lsf->xbar;
lsf->y2bar = lsf->y2bar/N - lsf->ybar*lsf->ybar;
lsf->z2bar = lsf->z2bar/N - lsf->zbar*lsf->zbar;
lsf->xybar = lsf->xybar/N - lsf->xbar*lsf->ybar;
lsf->yzbar = lsf->yzbar/N - lsf->ybar*lsf->zbar;
lsf->xzbar = lsf->xzbar/N - lsf->xbar*lsf->zbar;
lsf->xbar /= N;
lsf->ybar /= N;
lsf->zbar /= N;
lsf->x2bar = lsf->x2bar / N - lsf->xbar * lsf->xbar;
lsf->y2bar = lsf->y2bar / N - lsf->ybar * lsf->ybar;
lsf->z2bar = lsf->z2bar / N - lsf->zbar * lsf->zbar;
lsf->xybar = lsf->xybar / N - lsf->xbar * lsf->ybar;
lsf->yzbar = lsf->yzbar / N - lsf->ybar * lsf->zbar;
lsf->xzbar = lsf->xzbar / N - lsf->xbar * lsf->zbar;
DD = lsf->x2bar*lsf->y2bar - lsf->xybar*lsf->xybar;
if (fabs(DD) <= 1e-10*(lsf->max_absx+lsf->max_absy))
return -1;
lsf->A = (lsf->yzbar*lsf->xybar - lsf->xzbar*lsf->y2bar) / DD;
lsf->B = (lsf->xzbar*lsf->xybar - lsf->yzbar*lsf->x2bar) / DD;
lsf->D = -(lsf->zbar + lsf->A*lsf->xbar + lsf->B*lsf->ybar);
return 0;
const float DD = lsf->x2bar * lsf->y2bar - sq(lsf->xybar);
if (fabs(DD) <= 1e-10 * (lsf->max_absx + lsf->max_absy))
return -1;
lsf->A = (lsf->yzbar * lsf->xybar - lsf->xzbar * lsf->y2bar) / DD;
lsf->B = (lsf->xzbar * lsf->xybar - lsf->yzbar * lsf->x2bar) / DD;
lsf->D = -(lsf->zbar + lsf->A * lsf->xbar + lsf->B * lsf->ybar);
return 0;
}
#endif
#endif // AUTO_BED_LEVELING_UBL

14
Marlin/least_squares_fit.h
View File

@ -27,7 +27,7 @@
* 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
* it saves roughly 10K of program memory. And even better... the data
* fed into the algorithm does not need to all be present at the same time.
* fed into the algorithm does not need to all be present at the same time.
* A point can be probed and its values fed into the algorithm and then discarded.
*
*/
@ -42,14 +42,14 @@
struct linear_fit_data {
int n;
float xbar, ybar, zbar;
float x2bar, y2bar, z2bar;
float xybar, xzbar, yzbar;
float max_absx, max_absy;
float A, B, D;
float xbar, ybar, zbar,
x2bar, y2bar, z2bar,
xybar, xzbar, yzbar,
max_absx, max_absy,
A, B, D;
};
void incremental_LSF_reset(struct linear_fit_data *);
void incremental_LSF_reset(struct linear_fit_data *);
void incremental_LSF(struct linear_fit_data *, float x, float y, float z);
int finish_incremental_LSF(struct linear_fit_data *);