From a35c68145365f1ce56aeeb99839e7c00aa2189ea Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 28 Apr 2017 17:33:28 -0500 Subject: [PATCH] Clean up least_squares_fit --- Marlin/least_squares_fit.cpp | 84 ++++++++++++++++-------------------- Marlin/least_squares_fit.h | 14 +++--- 2 files changed, 43 insertions(+), 55 deletions(-) diff --git a/Marlin/least_squares_fit.cpp b/Marlin/least_squares_fit.cpp index ee9d7cc9e9..e6f684e77e 100644 --- a/Marlin/least_squares_fit.cpp +++ b/Marlin/least_squares_fit.cpp @@ -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 diff --git a/Marlin/least_squares_fit.h b/Marlin/least_squares_fit.h index 41a5741cfc..ec863080bd 100644 --- a/Marlin/least_squares_fit.h +++ b/Marlin/least_squares_fit.h @@ -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 *);