bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 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

38
Marlin/least_squares_fit.cpp
View File

@ -21,7 +21,7 @@
*/ */
/** /**
* 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
@ -41,37 +41,26 @@
#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;
@ -82,7 +71,7 @@ int finish_incremental_LSF(struct linear_fit_data *lsf) {
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;
@ -91,6 +80,5 @@ int finish_incremental_LSF(struct linear_fit_data *lsf) {
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 *);