My Marlin configs for Fabrikator Mini and CTC i3 Pro B
Вы не можете выбрать более 25 тем Темы должны начинаться с буквы или цифры, могут содержать дефисы(-) и должны содержать не более 35 символов.

least_squares_fit.cpp 3.7KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118
  1. /**
  2. * Marlin 3D Printer Firmware
  3. * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
  4. *
  5. * Based on Sprinter and grbl.
  6. * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
  7. *
  8. * This program is free software: you can redistribute it and/or modify
  9. * it under the terms of the GNU General Public License as published by
  10. * the Free Software Foundation, either version 3 of the License, or
  11. * (at your option) any later version.
  12. *
  13. * This program is distributed in the hope that it will be useful,
  14. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  16. * GNU General Public License for more details.
  17. *
  18. * You should have received a copy of the GNU General Public License
  19. * along with this program. If not, see <http://www.gnu.org/licenses/>.
  20. *
  21. */
  22. /**
  23. * Least Squares Best Fit By Roxy and Ed Williams
  24. *
  25. * This algorithm is high speed and has a very small code footprint.
  26. * Its results are identical to both the Iterative Least-Squares published
  27. * earlier by Roxy and the QR_SOLVE solution. If used in place of QR_SOLVE
  28. * it saves roughly 10K of program memory.
  29. *
  30. */
  31. #include "MarlinConfig.h"
  32. #if ENABLED(AUTO_BED_LEVELING_UBL) // Currently only used by UBL, but is applicable to Grid Based (Linear) Bed Leveling
  33. #include "ubl.h"
  34. #include "Marlin.h"
  35. #include "macros.h"
  36. #include <math.h>
  37. double linear_fit_average(double m[], const int);
  38. //double linear_fit_average_squared(double m[], const int);
  39. //double linear_fit_average_mixed_terms(double m1[], double m2[], const int);
  40. double linear_fit_average_product(double matrix1[], double matrix2[], const int n);
  41. void linear_fit_subtract_mean(double matrix[], double bar, const int n);
  42. double linear_fit_max_abs(double m[], const int);
  43. linear_fit linear_fit_results;
  44. linear_fit* lsf_linear_fit(double x[], double y[], double z[], const int n) {
  45. double xbar, ybar, zbar,
  46. x2bar, y2bar,
  47. xybar, xzbar, yzbar,
  48. D;
  49. linear_fit_results.A = 0.0;
  50. linear_fit_results.B = 0.0;
  51. linear_fit_results.D = 0.0;
  52. xbar = linear_fit_average(x, n);
  53. ybar = linear_fit_average(y, n);
  54. zbar = linear_fit_average(z, n);
  55. linear_fit_subtract_mean(x, xbar, n);
  56. linear_fit_subtract_mean(y, ybar, n);
  57. linear_fit_subtract_mean(z, zbar, n);
  58. x2bar = linear_fit_average_product(x, x, n);
  59. y2bar = linear_fit_average_product(y, y, n);
  60. xybar = linear_fit_average_product(x, y, n);
  61. xzbar = linear_fit_average_product(x, z, n);
  62. yzbar = linear_fit_average_product(y, z, n);
  63. D = x2bar * y2bar - xybar * xybar;
  64. for (int i = 0; i < n; i++) {
  65. if (fabs(D) <= 1e-15 * (linear_fit_max_abs(x, n) + linear_fit_max_abs(y, n))) {
  66. printf("error: x,y points are collinear at index:%d\n", i);
  67. return NULL;
  68. }
  69. }
  70. linear_fit_results.A = -(xzbar * y2bar - yzbar * xybar) / D;
  71. linear_fit_results.B = -(yzbar * x2bar - xzbar * xybar) / D;
  72. // linear_fit_results.D = -(zbar - linear_fit_results->A * xbar - linear_fit_results->B * ybar);
  73. linear_fit_results.D = -(zbar + linear_fit_results.A * xbar + linear_fit_results.B * ybar);
  74. return &linear_fit_results;
  75. }
  76. double linear_fit_average(double *matrix, const int n) {
  77. double sum = 0.0;
  78. for (int i = 0; i < n; i++)
  79. sum += matrix[i];
  80. return sum / (double)n;
  81. }
  82. double linear_fit_average_product(double *matrix1, double *matrix2, const int n) {
  83. double sum = 0.0;
  84. for (int i = 0; i < n; i++)
  85. sum += matrix1[i] * matrix2[i];
  86. return sum / (double)n;
  87. }
  88. void linear_fit_subtract_mean(double *matrix, double bar, const int n) {
  89. for (int i = 0; i < n; i++)
  90. matrix[i] -= bar;
  91. }
  92. double linear_fit_max_abs(double *matrix, const int n) {
  93. double max_abs = 0.0;
  94. for (int i = 0; i < n; i++)
  95. NOLESS(max_abs, fabs(matrix[i]));
  96. return max_abs;
  97. }
  98. #endif