diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 35d739a5a..d244b198d 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -261,7 +261,7 @@ #if HAS_ABL #include "vector_3.h" #if ENABLED(AUTO_BED_LEVELING_LINEAR) - #include "qr_solve.h" + #include "least_squares_fit.h" #endif #elif ENABLED(MESH_BED_LEVELING) #include "mesh_bed_leveling.h" @@ -4336,8 +4336,8 @@ void home_all_axes() { gcode_G28(true); } ABL_VAR int indexIntoAB[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y]; ABL_VAR float eqnAMatrix[GRID_MAX_POINTS * 3], // "A" matrix of the linear system of equations - eqnBVector[GRID_MAX_POINTS], // "B" vector of Z points - mean; + eqnBVector[GRID_MAX_POINTS], // "B" vector of Z points + mean; #endif #elif ENABLED(AUTO_BED_LEVELING_3POINT) @@ -4353,6 +4353,11 @@ void home_all_axes() { gcode_G28(true); } #endif // AUTO_BED_LEVELING_3POINT + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + struct linear_fit_data lsf_results; + incremental_LSF_reset(&lsf_results); + #endif + /** * On the initial G29 fetch command parameters. */ @@ -4549,11 +4554,7 @@ void home_all_axes() { gcode_G28(true); } abl_should_enable = false; } - #elif ENABLED(AUTO_BED_LEVELING_LINEAR) - - mean = 0.0; - - #endif // AUTO_BED_LEVELING_LINEAR + #endif // AUTO_BED_LEVELING_BILINEAR #if ENABLED(AUTO_BED_LEVELING_3POINT) @@ -4616,11 +4617,11 @@ void home_all_axes() { gcode_G28(true); } #if ENABLED(AUTO_BED_LEVELING_LINEAR) - mean += measured_z; - eqnBVector[abl_probe_index] = measured_z; - eqnAMatrix[abl_probe_index + 0 * abl2] = xProbe; - eqnAMatrix[abl_probe_index + 1 * abl2] = yProbe; - eqnAMatrix[abl_probe_index + 2 * abl2] = 1; +// mean += measured_z; // I believe this is unused code? +// eqnBVector[abl_probe_index] = measured_z; // I believe this is unused code? +// eqnAMatrix[abl_probe_index + 0 * abl2] = xProbe; // I believe this is unused code? +// eqnAMatrix[abl_probe_index + 1 * abl2] = yProbe; // I believe this is unused code? +// eqnAMatrix[abl_probe_index + 2 * abl2] = 1; // I believe this is unused code? #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) @@ -4794,6 +4795,11 @@ void home_all_axes() { gcode_G28(true); } eqnAMatrix[abl_probe_index + 1 * abl2] = yProbe; eqnAMatrix[abl_probe_index + 2 * abl2] = 1; + incremental_LSF(&lsf_results, xProbe, yProbe, measured_z); + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + indexIntoAB[xCount][yCount] = abl_probe_index; + #endif #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) z_values[xCount][yCount] = measured_z + zoffset; @@ -4894,7 +4900,11 @@ void home_all_axes() { gcode_G28(true); } * so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z */ float plane_equation_coefficients[3]; - qr_solve(plane_equation_coefficients, abl2, 3, eqnAMatrix, eqnBVector); + + finish_incremental_LSF(&lsf_results); + plane_equation_coefficients[0] = -lsf_results.A; // We should be able to eliminate the '-' on these three lines and down below + plane_equation_coefficients[1] = -lsf_results.B; // but that is not yet tested. + plane_equation_coefficients[2] = -lsf_results.D; mean /= abl2; @@ -4916,7 +4926,7 @@ void home_all_axes() { gcode_G28(true); } // Create the matrix but don't correct the position yet if (!dryrun) { planner.bed_level_matrix = matrix_3x3::create_look_at( - vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1) + vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1) // We can eleminate the '-' here and up above ); } diff --git a/Marlin/least_squares_fit.cpp b/Marlin/least_squares_fit.cpp index 42adc8fe6..f8c7a0b52 100644 --- a/Marlin/least_squares_fit.cpp +++ b/Marlin/least_squares_fit.cpp @@ -34,7 +34,7 @@ #include "MarlinConfig.h" -#if ENABLED(AUTO_BED_LEVELING_UBL) // Currently only used by UBL, but is applicable to Grid Based (Linear) Bed Leveling +#if ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(AUTO_BED_LEVELING_LINEAR) #include "macros.h" #include @@ -68,4 +68,4 @@ int finish_incremental_LSF(struct linear_fit_data *lsf) { return 0; } -#endif // AUTO_BED_LEVELING_UBL +#endif // AUTO_BED_LEVELING_UBL || ENABLED(AUTO_BED_LEVELING_LINEAR) diff --git a/Marlin/least_squares_fit.h b/Marlin/least_squares_fit.h index bdb427159..00d7a2419 100644 --- a/Marlin/least_squares_fit.h +++ b/Marlin/least_squares_fit.h @@ -34,7 +34,7 @@ #include "MarlinConfig.h" -#if ENABLED(AUTO_BED_LEVELING_UBL) // Currently only used by UBL, but is applicable to Grid Based (Linear) Bed Leveling +#if ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(AUTO_BED_LEVELING_LINEAR) #include "Marlin.h" #include "macros.h" diff --git a/Marlin/qr_solve.cpp b/Marlin/qr_solve.cpp deleted file mode 100644 index 7706c6f8c..000000000 --- a/Marlin/qr_solve.cpp +++ /dev/null @@ -1,1591 +0,0 @@ -/** - * Marlin 3D Printer Firmware - * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] - * - * Based on Sprinter and grbl. - * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - * - */ - -#include "qr_solve.h" - -#if ENABLED(AUTO_BED_LEVELING_LINEAR) - -#include -#include - -//# include "r8lib.h" - -int i4_min(int i1, int i2) - -/******************************************************************************/ -/** - Purpose: - - I4_MIN returns the smaller of two I4's. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 29 August 2006 - - Author: - - John Burkardt - - Parameters: - - Input, int I1, I2, two integers to be compared. - - Output, int I4_MIN, the smaller of I1 and I2. -*/ -{ - return (i1 < i2) ? i1 : i2; -} - -float r8_epsilon(void) - -/******************************************************************************/ -/** - Purpose: - - R8_EPSILON returns the R8 round off unit. - - Discussion: - - R8_EPSILON is a number R which is a power of 2 with the property that, - to the precision of the computer's arithmetic, - 1 < 1 + R - but - 1 = ( 1 + R / 2 ) - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 01 September 2012 - - Author: - - John Burkardt - - Parameters: - - Output, float R8_EPSILON, the R8 round-off unit. -*/ -{ - const float value = 2.220446049250313E-016; - return value; -} - -float r8_max(float x, float y) - -/******************************************************************************/ -/** - Purpose: - - R8_MAX returns the maximum of two R8's. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 07 May 2006 - - Author: - - John Burkardt - - Parameters: - - Input, float X, Y, the quantities to compare. - - Output, float R8_MAX, the maximum of X and Y. -*/ -{ - return (y < x) ? x : y; -} - -float r8_abs(float x) - -/******************************************************************************/ -/** - Purpose: - - R8_ABS returns the absolute value of an R8. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 07 May 2006 - - Author: - - John Burkardt - - Parameters: - - Input, float X, the quantity whose absolute value is desired. - - Output, float R8_ABS, the absolute value of X. -*/ -{ - return (x < 0.0) ? -x : x; -} - -float r8_sign(float x) - -/******************************************************************************/ -/** - Purpose: - - R8_SIGN returns the sign of an R8. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 08 May 2006 - - Author: - - John Burkardt - - Parameters: - - Input, float X, the number whose sign is desired. - - Output, float R8_SIGN, the sign of X. -*/ -{ - return (x < 0.0) ? -1.0 : 1.0; -} - -float r8mat_amax(int m, int n, float a[]) - -/******************************************************************************/ -/** - Purpose: - - R8MAT_AMAX returns the maximum absolute value entry of an R8MAT. - - Discussion: - - An R8MAT is a doubly dimensioned array of R8 values, stored as a vector - in column-major order. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 07 September 2012 - - Author: - - John Burkardt - - Parameters: - - Input, int M, the number of rows in A. - - Input, int N, the number of columns in A. - - Input, float A[M*N], the M by N matrix. - - Output, float R8MAT_AMAX, the maximum absolute value entry of A. -*/ -{ - float value = r8_abs(a[0 + 0 * m]); - for (int j = 0; j < n; j++) { - for (int i = 0; i < m; i++) { - NOLESS(value, r8_abs(a[i + j * m])); - } - } - return value; -} - -void r8mat_copy(float a2[], int m, int n, float a1[]) - -/******************************************************************************/ -/** - Purpose: - - R8MAT_COPY_NEW copies one R8MAT to a "new" R8MAT. - - Discussion: - - An R8MAT is a doubly dimensioned array of R8 values, stored as a vector - in column-major order. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 26 July 2008 - - Author: - - John Burkardt - - Parameters: - - Input, int M, N, the number of rows and columns. - - Input, float A1[M*N], the matrix to be copied. - - Output, float R8MAT_COPY_NEW[M*N], the copy of A1. -*/ -{ - for (int j = 0; j < n; j++) { - for (int i = 0; i < m; i++) - a2[i + j * m] = a1[i + j * m]; - } -} - -/******************************************************************************/ - -void daxpy(int n, float da, float dx[], int incx, float dy[], int incy) - -/******************************************************************************/ -/** - Purpose: - - DAXPY computes constant times a vector plus a vector. - - Discussion: - - This routine uses unrolled loops for increments equal to one. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 30 March 2007 - - Author: - - C version by John Burkardt - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, - LINPACK User's Guide, - SIAM, 1979. - - Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh, - Basic Linear Algebra Subprograms for Fortran Usage, - Algorithm 539, - ACM Transactions on Mathematical Software, - Volume 5, Number 3, September 1979, pages 308-323. - - Parameters: - - Input, int N, the number of elements in DX and DY. - - Input, float DA, the multiplier of DX. - - Input, float DX[*], the first vector. - - Input, int INCX, the increment between successive entries of DX. - - Input/output, float DY[*], the second vector. - On output, DY[*] has been replaced by DY[*] + DA * DX[*]. - - Input, int INCY, the increment between successive entries of DY. -*/ -{ - if (n <= 0 || da == 0.0) return; - - int i, ix, iy, m; - /** - Code for unequal increments or equal increments - not equal to 1. - */ - if (incx != 1 || incy != 1) { - if (0 <= incx) - ix = 0; - else - ix = (- n + 1) * incx; - if (0 <= incy) - iy = 0; - else - iy = (- n + 1) * incy; - for (i = 0; i < n; i++) { - dy[iy] = dy[iy] + da * dx[ix]; - ix = ix + incx; - iy = iy + incy; - } - } - /** - Code for both increments equal to 1. - */ - else { - m = n % 4; - for (i = 0; i < m; i++) - dy[i] = dy[i] + da * dx[i]; - for (i = m; i < n; i = i + 4) { - dy[i ] = dy[i ] + da * dx[i ]; - dy[i + 1] = dy[i + 1] + da * dx[i + 1]; - dy[i + 2] = dy[i + 2] + da * dx[i + 2]; - dy[i + 3] = dy[i + 3] + da * dx[i + 3]; - } - } -} -/******************************************************************************/ - -float ddot(int n, float dx[], int incx, float dy[], int incy) - -/******************************************************************************/ -/** - Purpose: - - DDOT forms the dot product of two vectors. - - Discussion: - - This routine uses unrolled loops for increments equal to one. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 30 March 2007 - - Author: - - C version by John Burkardt - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, - LINPACK User's Guide, - SIAM, 1979. - - Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh, - Basic Linear Algebra Subprograms for Fortran Usage, - Algorithm 539, - ACM Transactions on Mathematical Software, - Volume 5, Number 3, September 1979, pages 308-323. - - Parameters: - - Input, int N, the number of entries in the vectors. - - Input, float DX[*], the first vector. - - Input, int INCX, the increment between successive entries in DX. - - Input, float DY[*], the second vector. - - Input, int INCY, the increment between successive entries in DY. - - Output, float DDOT, the sum of the product of the corresponding - entries of DX and DY. -*/ -{ - - if (n <= 0) return 0.0; - - int i, m; - float dtemp = 0.0; - - /** - Code for unequal increments or equal increments - not equal to 1. - */ - if (incx != 1 || incy != 1) { - int ix = (incx >= 0) ? 0 : (-n + 1) * incx, - iy = (incy >= 0) ? 0 : (-n + 1) * incy; - for (i = 0; i < n; i++) { - dtemp += dx[ix] * dy[iy]; - ix = ix + incx; - iy = iy + incy; - } - } - /** - Code for both increments equal to 1. - */ - else { - m = n % 5; - for (i = 0; i < m; i++) - dtemp += dx[i] * dy[i]; - for (i = m; i < n; i = i + 5) { - dtemp += dx[i] * dy[i] - + dx[i + 1] * dy[i + 1] - + dx[i + 2] * dy[i + 2] - + dx[i + 3] * dy[i + 3] - + dx[i + 4] * dy[i + 4]; - } - } - return dtemp; -} -/******************************************************************************/ - -float dnrm2(int n, float x[], int incx) - -/******************************************************************************/ -/** - Purpose: - - DNRM2 returns the euclidean norm of a vector. - - Discussion: - - DNRM2 ( X ) = sqrt ( X' * X ) - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 30 March 2007 - - Author: - - C version by John Burkardt - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, - LINPACK User's Guide, - SIAM, 1979. - - Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh, - Basic Linear Algebra Subprograms for Fortran Usage, - Algorithm 539, - ACM Transactions on Mathematical Software, - Volume 5, Number 3, September 1979, pages 308-323. - - Parameters: - - Input, int N, the number of entries in the vector. - - Input, float X[*], the vector whose norm is to be computed. - - Input, int INCX, the increment between successive entries of X. - - Output, float DNRM2, the Euclidean norm of X. -*/ -{ - float norm; - if (n < 1 || incx < 1) - norm = 0.0; - else if (n == 1) - norm = r8_abs(x[0]); - else { - float scale = 0.0, ssq = 1.0; - int ix = 0; - for (int i = 0; i < n; i++) { - if (x[ix] != 0.0) { - float absxi = r8_abs(x[ix]); - if (scale < absxi) { - ssq = 1.0 + ssq * (scale / absxi) * (scale / absxi); - scale = absxi; - } - else - ssq = ssq + (absxi / scale) * (absxi / scale); - } - ix += incx; - } - norm = scale * SQRT(ssq); - } - return norm; -} -/******************************************************************************/ - -void dqrank(float a[], int lda, int m, int n, float tol, int* kr, - int jpvt[], float qraux[]) - -/******************************************************************************/ -/** - Purpose: - - DQRANK computes the QR factorization of a rectangular matrix. - - Discussion: - - This routine is used in conjunction with DQRLSS to solve - overdetermined, underdetermined and singular linear systems - in a least squares sense. - - DQRANK uses the LINPACK subroutine DQRDC to compute the QR - factorization, with column pivoting, of an M by N matrix A. - The numerical rank is determined using the tolerance TOL. - - Note that on output, ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate - of the condition number of the matrix of independent columns, - and of R. This estimate will be <= 1/TOL. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 21 April 2012 - - Author: - - C version by John Burkardt. - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, - LINPACK User's Guide, - SIAM, 1979, - ISBN13: 978-0-898711-72-1, - LC: QA214.L56. - - Parameters: - - Input/output, float A[LDA*N]. On input, the matrix whose - decomposition is to be computed. On output, the information from DQRDC. - The triangular matrix R of the QR factorization is contained in the - upper triangle and information needed to recover the orthogonal - matrix Q is stored below the diagonal in A and in the vector QRAUX. - - Input, int LDA, the leading dimension of A, which must - be at least M. - - Input, int M, the number of rows of A. - - Input, int N, the number of columns of A. - - Input, float TOL, a relative tolerance used to determine the - numerical rank. The problem should be scaled so that all the elements - of A have roughly the same absolute accuracy, EPS. Then a reasonable - value for TOL is roughly EPS divided by the magnitude of the largest - element. - - Output, int *KR, the numerical rank. - - Output, int JPVT[N], the pivot information from DQRDC. - Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly - independent to within the tolerance TOL and the remaining columns - are linearly dependent. - - Output, float QRAUX[N], will contain extra information defining - the QR factorization. -*/ -{ - float work[n]; - - for (int i = 0; i < n; i++) - jpvt[i] = 0; - - int job = 1; - - dqrdc(a, lda, m, n, qraux, jpvt, work, job); - - *kr = 0; - int k = i4_min(m, n); - for (int j = 0; j < k; j++) { - if (r8_abs(a[j + j * lda]) <= tol * r8_abs(a[0 + 0 * lda])) - return; - *kr = j + 1; - } -} -/******************************************************************************/ - -void dqrdc(float a[], int lda, int n, int p, float qraux[], int jpvt[], - float work[], int job) - -/******************************************************************************/ -/** - Purpose: - - DQRDC computes the QR factorization of a real rectangular matrix. - - Discussion: - - DQRDC uses Householder transformations. - - Column pivoting based on the 2-norms of the reduced columns may be - performed at the user's option. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 07 June 2005 - - Author: - - C version by John Burkardt. - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart, - LINPACK User's Guide, - SIAM, (Society for Industrial and Applied Mathematics), - 3600 University City Science Center, - Philadelphia, PA, 19104-2688. - ISBN 0-89871-172-X - - Parameters: - - Input/output, float A(LDA,P). On input, the N by P matrix - whose decomposition is to be computed. On output, A contains in - its upper triangle the upper triangular matrix R of the QR - factorization. Below its diagonal A contains information from - which the orthogonal part of the decomposition can be recovered. - Note that if pivoting has been requested, the decomposition is not that - of the original matrix A but that of A with its columns permuted - as described by JPVT. - - Input, int LDA, the leading dimension of the array A. LDA must - be at least N. - - Input, int N, the number of rows of the matrix A. - - Input, int P, the number of columns of the matrix A. - - Output, float QRAUX[P], contains further information required - to recover the orthogonal part of the decomposition. - - Input/output, integer JPVT[P]. On input, JPVT contains integers that - control the selection of the pivot columns. The K-th column A(*,K) of A - is placed in one of three classes according to the value of JPVT(K). - > 0, then A(K) is an initial column. - = 0, then A(K) is a free column. - < 0, then A(K) is a final column. - Before the decomposition is computed, initial columns are moved to - the beginning of the array A and final columns to the end. Both - initial and final columns are frozen in place during the computation - and only free columns are moved. At the K-th stage of the - reduction, if A(*,K) is occupied by a free column it is interchanged - with the free column of largest reduced norm. JPVT is not referenced - if JOB == 0. On output, JPVT(K) contains the index of the column of the - original matrix that has been interchanged into the K-th column, if - pivoting was requested. - - Workspace, float WORK[P]. WORK is not referenced if JOB == 0. - - Input, int JOB, initiates column pivoting. - 0, no pivoting is done. - nonzero, pivoting is done. -*/ -{ - int jp; - int j; - int lup; - int maxj; - float maxnrm, nrmxl, t, tt; - - int pl = 1, pu = 0; - /** - If pivoting is requested, rearrange the columns. - */ - if (job != 0) { - for (j = 1; j <= p; j++) { - int swapj = (0 < jpvt[j - 1]); - jpvt[j - 1] = (jpvt[j - 1] < 0) ? -j : j; - if (swapj) { - if (j != pl) - dswap(n, a + 0 + (pl - 1)*lda, 1, a + 0 + (j - 1), 1); - jpvt[j - 1] = jpvt[pl - 1]; - jpvt[pl - 1] = j; - pl++; - } - } - pu = p; - for (j = p; 1 <= j; j--) { - if (jpvt[j - 1] < 0) { - jpvt[j - 1] = -jpvt[j - 1]; - if (j != pu) { - dswap(n, a + 0 + (pu - 1)*lda, 1, a + 0 + (j - 1)*lda, 1); - jp = jpvt[pu - 1]; - jpvt[pu - 1] = jpvt[j - 1]; - jpvt[j - 1] = jp; - } - pu = pu - 1; - } - } - } - /** - Compute the norms of the free columns. - */ - for (j = pl; j <= pu; j++) - qraux[j - 1] = dnrm2(n, a + 0 + (j - 1) * lda, 1); - for (j = pl; j <= pu; j++) - work[j - 1] = qraux[j - 1]; - /** - Perform the Householder reduction of A. - */ - lup = i4_min(n, p); - for (int l = 1; l <= lup; l++) { - /** - Bring the column of largest norm into the pivot position. - */ - if (pl <= l && l < pu) { - maxnrm = 0.0; - maxj = l; - for (j = l; j <= pu; j++) { - if (maxnrm < qraux[j - 1]) { - maxnrm = qraux[j - 1]; - maxj = j; - } - } - if (maxj != l) { - dswap(n, a + 0 + (l - 1)*lda, 1, a + 0 + (maxj - 1)*lda, 1); - qraux[maxj - 1] = qraux[l - 1]; - work[maxj - 1] = work[l - 1]; - jp = jpvt[maxj - 1]; - jpvt[maxj - 1] = jpvt[l - 1]; - jpvt[l - 1] = jp; - } - } - /** - Compute the Householder transformation for column L. - */ - qraux[l - 1] = 0.0; - if (l != n) { - nrmxl = dnrm2(n - l + 1, a + l - 1 + (l - 1) * lda, 1); - if (nrmxl != 0.0) { - if (a[l - 1 + (l - 1)*lda] != 0.0) - nrmxl = nrmxl * r8_sign(a[l - 1 + (l - 1) * lda]); - dscal(n - l + 1, 1.0 / nrmxl, a + l - 1 + (l - 1)*lda, 1); - a[l - 1 + (l - 1)*lda] = 1.0 + a[l - 1 + (l - 1) * lda]; - /** - Apply the transformation to the remaining columns, updating the norms. - */ - for (j = l + 1; j <= p; j++) { - t = -ddot(n - l + 1, a + l - 1 + (l - 1) * lda, 1, a + l - 1 + (j - 1) * lda, 1) - / a[l - 1 + (l - 1) * lda]; - daxpy(n - l + 1, t, a + l - 1 + (l - 1)*lda, 1, a + l - 1 + (j - 1)*lda, 1); - if (pl <= j && j <= pu) { - if (qraux[j - 1] != 0.0) { - tt = 1.0 - POW(r8_abs(a[l - 1 + (j - 1) * lda]) / qraux[j - 1], 2); - tt = r8_max(tt, 0.0); - t = tt; - tt = 1.0 + 0.05 * tt * POW(qraux[j - 1] / work[j - 1], 2); - if (tt != 1.0) - qraux[j - 1] = qraux[j - 1] * SQRT(t); - else { - qraux[j - 1] = dnrm2(n - l, a + l + (j - 1) * lda, 1); - work[j - 1] = qraux[j - 1]; - } - } - } - } - /** - Save the transformation. - */ - qraux[l - 1] = a[l - 1 + (l - 1) * lda]; - a[l - 1 + (l - 1)*lda] = -nrmxl; - } - } - } -} -/******************************************************************************/ - -int dqrls(float a[], int lda, int m, int n, float tol, int* kr, float b[], - float x[], float rsd[], int jpvt[], float qraux[], int itask) - -/******************************************************************************/ -/** - Purpose: - - DQRLS factors and solves a linear system in the least squares sense. - - Discussion: - - The linear system may be overdetermined, underdetermined or singular. - The solution is obtained using a QR factorization of the - coefficient matrix. - - DQRLS can be efficiently used to solve several least squares - problems with the same matrix A. The first system is solved - with ITASK = 1. The subsequent systems are solved with - ITASK = 2, to avoid the recomputation of the matrix factors. - The parameters KR, JPVT, and QRAUX must not be modified - between calls to DQRLS. - - DQRLS is used to solve in a least squares sense - overdetermined, underdetermined and singular linear systems. - The system is A*X approximates B where A is M by N. - B is a given M-vector, and X is the N-vector to be computed. - A solution X is found which minimimzes the sum of squares (2-norm) - of the residual, A*X - B. - - The numerical rank of A is determined using the tolerance TOL. - - DQRLS uses the LINPACK subroutine DQRDC to compute the QR - factorization, with column pivoting, of an M by N matrix A. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 10 September 2012 - - Author: - - C version by John Burkardt. - - Reference: - - David Kahaner, Cleve Moler, Steven Nash, - Numerical Methods and Software, - Prentice Hall, 1989, - ISBN: 0-13-627258-4, - LC: TA345.K34. - - Parameters: - - Input/output, float A[LDA*N], an M by N matrix. - On input, the matrix whose decomposition is to be computed. - In a least squares data fitting problem, A(I,J) is the - value of the J-th basis (model) function at the I-th data point. - On output, A contains the output from DQRDC. The triangular matrix R - of the QR factorization is contained in the upper triangle and - information needed to recover the orthogonal matrix Q is stored - below the diagonal in A and in the vector QRAUX. - - Input, int LDA, the leading dimension of A. - - Input, int M, the number of rows of A. - - Input, int N, the number of columns of A. - - Input, float TOL, a relative tolerance used to determine the - numerical rank. The problem should be scaled so that all the elements - of A have roughly the same absolute accuracy EPS. Then a reasonable - value for TOL is roughly EPS divided by the magnitude of the largest - element. - - Output, int *KR, the numerical rank. - - Input, float B[M], the right hand side of the linear system. - - Output, float X[N], a least squares solution to the linear - system. - - Output, float RSD[M], the residual, B - A*X. RSD may - overwrite B. - - Workspace, int JPVT[N], required if ITASK = 1. - Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly - independent to within the tolerance TOL and the remaining columns - are linearly dependent. ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate - of the condition number of the matrix of independent columns, - and of R. This estimate will be <= 1/TOL. - - Workspace, float QRAUX[N], required if ITASK = 1. - - Input, int ITASK. - 1, DQRLS factors the matrix A and solves the least squares problem. - 2, DQRLS assumes that the matrix A was factored with an earlier - call to DQRLS, and only solves the least squares problem. - - Output, int DQRLS, error code. - 0: no error - -1: LDA < M (fatal error) - -2: N < 1 (fatal error) - -3: ITASK < 1 (fatal error) -*/ -{ - int ind; - if (lda < m) { - /*fprintf ( stderr, "\n" ); - fprintf ( stderr, "DQRLS - Fatal error!\n" ); - fprintf ( stderr, " LDA < M.\n" );*/ - ind = -1; - return ind; - } - - if (n <= 0) { - /*fprintf ( stderr, "\n" ); - fprintf ( stderr, "DQRLS - Fatal error!\n" ); - fprintf ( stderr, " N <= 0.\n" );*/ - ind = -2; - return ind; - } - - if (itask < 1) { - /*fprintf ( stderr, "\n" ); - fprintf ( stderr, "DQRLS - Fatal error!\n" ); - fprintf ( stderr, " ITASK < 1.\n" );*/ - ind = -3; - return ind; - } - - ind = 0; - /** - Factor the matrix. - */ - if (itask == 1) - dqrank(a, lda, m, n, tol, kr, jpvt, qraux); - /** - Solve the least-squares problem. - */ - dqrlss(a, lda, m, n, *kr, b, x, rsd, jpvt, qraux); - return ind; -} -/******************************************************************************/ - -void dqrlss(float a[], int lda, int m, int n, int kr, float b[], float x[], - float rsd[], int jpvt[], float qraux[]) - -/******************************************************************************/ -/** - Purpose: - - DQRLSS solves a linear system in a least squares sense. - - Discussion: - - DQRLSS must be preceded by a call to DQRANK. - - The system is to be solved is - A * X = B - where - A is an M by N matrix with rank KR, as determined by DQRANK, - B is a given M-vector, - X is the N-vector to be computed. - - A solution X, with at most KR nonzero components, is found which - minimizes the 2-norm of the residual (A*X-B). - - Once the matrix A has been formed, DQRANK should be - called once to decompose it. Then, for each right hand - side B, DQRLSS should be called once to obtain the - solution and residual. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 10 September 2012 - - Author: - - C version by John Burkardt - - Parameters: - - Input, float A[LDA*N], the QR factorization information - from DQRANK. The triangular matrix R of the QR factorization is - contained in the upper triangle and information needed to recover - the orthogonal matrix Q is stored below the diagonal in A and in - the vector QRAUX. - - Input, int LDA, the leading dimension of A, which must - be at least M. - - Input, int M, the number of rows of A. - - Input, int N, the number of columns of A. - - Input, int KR, the rank of the matrix, as estimated by DQRANK. - - Input, float B[M], the right hand side of the linear system. - - Output, float X[N], a least squares solution to the - linear system. - - Output, float RSD[M], the residual, B - A*X. RSD may - overwrite B. - - Input, int JPVT[N], the pivot information from DQRANK. - Columns JPVT[0], ..., JPVT[KR-1] of the original matrix are linearly - independent to within the tolerance TOL and the remaining columns - are linearly dependent. - - Input, float QRAUX[N], auxiliary information from DQRANK - defining the QR factorization. -*/ -{ - int i; - int info; - int j; - int job; - int k; - float t; - - if (kr != 0) { - job = 110; - info = dqrsl(a, lda, m, kr, qraux, b, rsd, rsd, x, rsd, rsd, job); UNUSED(info); - } - - for (i = 0; i < n; i++) - jpvt[i] = - jpvt[i]; - - for (i = kr; i < n; i++) - x[i] = 0.0; - - for (j = 1; j <= n; j++) { - if (jpvt[j - 1] <= 0) { - k = - jpvt[j - 1]; - jpvt[j - 1] = k; - - while (k != j) { - t = x[j - 1]; - x[j - 1] = x[k - 1]; - x[k - 1] = t; - jpvt[k - 1] = -jpvt[k - 1]; - k = jpvt[k - 1]; - } - } - } -} -/******************************************************************************/ - -int dqrsl(float a[], int lda, int n, int k, float qraux[], float y[], - float qy[], float qty[], float b[], float rsd[], float ab[], int job) - -/******************************************************************************/ -/** - Purpose: - - DQRSL computes transformations, projections, and least squares solutions. - - Discussion: - - DQRSL requires the output of DQRDC. - - For K <= min(N,P), let AK be the matrix - - AK = ( A(JPVT[0]), A(JPVT(2)), ..., A(JPVT(K)) ) - - formed from columns JPVT[0], ..., JPVT(K) of the original - N by P matrix A that was input to DQRDC. If no pivoting was - done, AK consists of the first K columns of A in their - original order. DQRDC produces a factored orthogonal matrix Q - and an upper triangular matrix R such that - - AK = Q * (R) - (0) - - This information is contained in coded form in the arrays - A and QRAUX. - - The parameters QY, QTY, B, RSD, and AB are not referenced - if their computation is not requested and in this case - can be replaced by dummy variables in the calling program. - To save storage, the user may in some cases use the same - array for different parameters in the calling sequence. A - frequently occurring example is when one wishes to compute - any of B, RSD, or AB and does not need Y or QTY. In this - case one may identify Y, QTY, and one of B, RSD, or AB, while - providing separate arrays for anything else that is to be - computed. - - Thus the calling sequence - - dqrsl ( a, lda, n, k, qraux, y, dum, y, b, y, dum, 110, info ) - - will result in the computation of B and RSD, with RSD - overwriting Y. More generally, each item in the following - list contains groups of permissible identifications for - a single calling sequence. - - 1. (Y,QTY,B) (RSD) (AB) (QY) - - 2. (Y,QTY,RSD) (B) (AB) (QY) - - 3. (Y,QTY,AB) (B) (RSD) (QY) - - 4. (Y,QY) (QTY,B) (RSD) (AB) - - 5. (Y,QY) (QTY,RSD) (B) (AB) - - 6. (Y,QY) (QTY,AB) (B) (RSD) - - In any group the value returned in the array allocated to - the group corresponds to the last member of the group. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 07 June 2005 - - Author: - - C version by John Burkardt. - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart, - LINPACK User's Guide, - SIAM, (Society for Industrial and Applied Mathematics), - 3600 University City Science Center, - Philadelphia, PA, 19104-2688. - ISBN 0-89871-172-X - - Parameters: - - Input, float A[LDA*P], contains the output of DQRDC. - - Input, int LDA, the leading dimension of the array A. - - Input, int N, the number of rows of the matrix AK. It must - have the same value as N in DQRDC. - - Input, int K, the number of columns of the matrix AK. K - must not be greater than min(N,P), where P is the same as in the - calling sequence to DQRDC. - - Input, float QRAUX[P], the auxiliary output from DQRDC. - - Input, float Y[N], a vector to be manipulated by DQRSL. - - Output, float QY[N], contains Q * Y, if requested. - - Output, float QTY[N], contains Q' * Y, if requested. - - Output, float B[K], the solution of the least squares problem - minimize norm2 ( Y - AK * B), - if its computation has been requested. Note that if pivoting was - requested in DQRDC, the J-th component of B will be associated with - column JPVT(J) of the original matrix A that was input into DQRDC. - - Output, float RSD[N], the least squares residual Y - AK * B, - if its computation has been requested. RSD is also the orthogonal - projection of Y onto the orthogonal complement of the column space - of AK. - - Output, float AB[N], the least squares approximation Ak * B, - if its computation has been requested. AB is also the orthogonal - projection of Y onto the column space of A. - - Input, integer JOB, specifies what is to be computed. JOB has - the decimal expansion ABCDE, with the following meaning: - - if A != 0, compute QY. - if B != 0, compute QTY. - if C != 0, compute QTY and B. - if D != 0, compute QTY and RSD. - if E != 0, compute QTY and AB. - - Note that a request to compute B, RSD, or AB automatically triggers - the computation of QTY, for which an array must be provided in the - calling sequence. - - Output, int DQRSL, is zero unless the computation of B has - been requested and R is exactly singular. In this case, INFO is the - index of the first zero diagonal element of R, and B is left unaltered. -*/ -{ - int cab; - int cb; - int cqty; - int cqy; - int cr; - int i; - int info; - int j; - int jj; - int ju; - float t; - float temp; - /** - Set INFO flag. - */ - info = 0; - - /** - Determine what is to be computed. - */ - cqy = ( job / 10000 != 0); - cqty = ((job % 10000) != 0); - cb = ((job % 1000) / 100 != 0); - cr = ((job % 100) / 10 != 0); - cab = ((job % 10) != 0); - ju = i4_min(k, n - 1); - - /** - Special action when N = 1. - */ - if (ju == 0) { - if (cqy) - qy[0] = y[0]; - if (cqty) - qty[0] = y[0]; - if (cab) - ab[0] = y[0]; - if (cb) { - if (a[0 + 0 * lda] == 0.0) - info = 1; - else - b[0] = y[0] / a[0 + 0 * lda]; - } - if (cr) - rsd[0] = 0.0; - return info; - } - /** - Set up to compute QY or QTY. - */ - if (cqy) { - for (i = 1; i <= n; i++) - qy[i - 1] = y[i - 1]; - } - if (cqty) { - for (i = 1; i <= n; i++) - qty[i - 1] = y[i - 1]; - } - /** - Compute QY. - */ - if (cqy) { - for (jj = 1; jj <= ju; jj++) { - j = ju - jj + 1; - if (qraux[j - 1] != 0.0) { - temp = a[j - 1 + (j - 1) * lda]; - a[j - 1 + (j - 1)*lda] = qraux[j - 1]; - t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, qy + j - 1, 1) / a[j - 1 + (j - 1) * lda]; - daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, qy + j - 1, 1); - a[j - 1 + (j - 1)*lda] = temp; - } - } - } - /** - Compute Q'*Y. - */ - if (cqty) { - for (j = 1; j <= ju; j++) { - if (qraux[j - 1] != 0.0) { - temp = a[j - 1 + (j - 1) * lda]; - a[j - 1 + (j - 1)*lda] = qraux[j - 1]; - t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, qty + j - 1, 1) / a[j - 1 + (j - 1) * lda]; - daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, qty + j - 1, 1); - a[j - 1 + (j - 1)*lda] = temp; - } - } - } - /** - Set up to compute B, RSD, or AB. - */ - if (cb) { - for (i = 1; i <= k; i++) - b[i - 1] = qty[i - 1]; - } - if (cab) { - for (i = 1; i <= k; i++) - ab[i - 1] = qty[i - 1]; - } - if (cr && k < n) { - for (i = k + 1; i <= n; i++) - rsd[i - 1] = qty[i - 1]; - } - if (cab && k + 1 <= n) { - for (i = k + 1; i <= n; i++) - ab[i - 1] = 0.0; - } - if (cr) { - for (i = 1; i <= k; i++) - rsd[i - 1] = 0.0; - } - /** - Compute B. - */ - if (cb) { - for (jj = 1; jj <= k; jj++) { - j = k - jj + 1; - if (a[j - 1 + (j - 1)*lda] == 0.0) { - info = j; - break; - } - b[j - 1] = b[j - 1] / a[j - 1 + (j - 1) * lda]; - if (j != 1) { - t = -b[j - 1]; - daxpy(j - 1, t, a + 0 + (j - 1)*lda, 1, b, 1); - } - } - } - /** - Compute RSD or AB as required. - */ - if (cr || cab) { - for (jj = 1; jj <= ju; jj++) { - j = ju - jj + 1; - if (qraux[j - 1] != 0.0) { - temp = a[j - 1 + (j - 1) * lda]; - a[j - 1 + (j - 1)*lda] = qraux[j - 1]; - if (cr) { - t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, rsd + j - 1, 1) - / a[j - 1 + (j - 1) * lda]; - daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, rsd + j - 1, 1); - } - if (cab) { - t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, ab + j - 1, 1) - / a[j - 1 + (j - 1) * lda]; - daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, ab + j - 1, 1); - } - a[j - 1 + (j - 1)*lda] = temp; - } - } - } - return info; -} -/******************************************************************************/ - -/******************************************************************************/ - -void dscal(int n, float sa, float x[], int incx) - -/******************************************************************************/ -/** - Purpose: - - DSCAL scales a vector by a constant. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 30 March 2007 - - Author: - - C version by John Burkardt - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, - LINPACK User's Guide, - SIAM, 1979. - - Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh, - Basic Linear Algebra Subprograms for Fortran Usage, - Algorithm 539, - ACM Transactions on Mathematical Software, - Volume 5, Number 3, September 1979, pages 308-323. - - Parameters: - - Input, int N, the number of entries in the vector. - - Input, float SA, the multiplier. - - Input/output, float X[*], the vector to be scaled. - - Input, int INCX, the increment between successive entries of X. -*/ -{ - int i; - int ix; - int m; - - if (n <= 0) return; - - if (incx == 1) { - m = n % 5; - for (i = 0; i < m; i++) - x[i] = sa * x[i]; - for (i = m; i < n; i = i + 5) { - x[i] = sa * x[i]; - x[i + 1] = sa * x[i + 1]; - x[i + 2] = sa * x[i + 2]; - x[i + 3] = sa * x[i + 3]; - x[i + 4] = sa * x[i + 4]; - } - } - else { - if (0 <= incx) - ix = 0; - else - ix = (- n + 1) * incx; - for (i = 0; i < n; i++) { - x[ix] = sa * x[ix]; - ix = ix + incx; - } - } -} -/******************************************************************************/ - - -void dswap(int n, float x[], int incx, float y[], int incy) - -/******************************************************************************/ -/** - Purpose: - - DSWAP interchanges two vectors. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 30 March 2007 - - Author: - - C version by John Burkardt - - Reference: - - Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, - LINPACK User's Guide, - SIAM, 1979. - - Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh, - Basic Linear Algebra Subprograms for Fortran Usage, - Algorithm 539, - ACM Transactions on Mathematical Software, - Volume 5, Number 3, September 1979, pages 308-323. - - Parameters: - - Input, int N, the number of entries in the vectors. - - Input/output, float X[*], one of the vectors to swap. - - Input, int INCX, the increment between successive entries of X. - - Input/output, float Y[*], one of the vectors to swap. - - Input, int INCY, the increment between successive elements of Y. -*/ -{ - if (n <= 0) return; - - int i, ix, iy, m; - float temp; - - if (incx == 1 && incy == 1) { - m = n % 3; - for (i = 0; i < m; i++) { - temp = x[i]; - x[i] = y[i]; - y[i] = temp; - } - for (i = m; i < n; i = i + 3) { - temp = x[i]; - x[i] = y[i]; - y[i] = temp; - temp = x[i + 1]; - x[i + 1] = y[i + 1]; - y[i + 1] = temp; - temp = x[i + 2]; - x[i + 2] = y[i + 2]; - y[i + 2] = temp; - } - } - else { - ix = (incx >= 0) ? 0 : (-n + 1) * incx; - iy = (incy >= 0) ? 0 : (-n + 1) * incy; - for (i = 0; i < n; i++) { - temp = x[ix]; - x[ix] = y[iy]; - y[iy] = temp; - ix = ix + incx; - iy = iy + incy; - } - } -} -/******************************************************************************/ - -/******************************************************************************/ - -void qr_solve(float x[], int m, int n, float a[], float b[]) - -/******************************************************************************/ -/** - Purpose: - - QR_SOLVE solves a linear system in the least squares sense. - - Discussion: - - If the matrix A has full column rank, then the solution X should be the - unique vector that minimizes the Euclidean norm of the residual. - - If the matrix A does not have full column rank, then the solution is - not unique; the vector X will minimize the residual norm, but so will - various other vectors. - - Licensing: - - This code is distributed under the GNU LGPL license. - - Modified: - - 11 September 2012 - - Author: - - John Burkardt - - Reference: - - David Kahaner, Cleve Moler, Steven Nash, - Numerical Methods and Software, - Prentice Hall, 1989, - ISBN: 0-13-627258-4, - LC: TA345.K34. - - Parameters: - - Input, int M, the number of rows of A. - - Input, int N, the number of columns of A. - - Input, float A[M*N], the matrix. - - Input, float B[M], the right hand side. - - Output, float QR_SOLVE[N], the least squares solution. -*/ -{ - float a_qr[n * m], qraux[n], r[m], tol; - int ind, itask, jpvt[n], kr, lda; - - r8mat_copy(a_qr, m, n, a); - lda = m; - tol = r8_epsilon() / r8mat_amax(m, n, a_qr); - itask = 1; - - ind = dqrls(a_qr, lda, m, n, tol, &kr, b, x, r, jpvt, qraux, itask); UNUSED(ind); -} -/******************************************************************************/ - -#endif diff --git a/Marlin/qr_solve.h b/Marlin/qr_solve.h deleted file mode 100644 index c409220d3..000000000 --- a/Marlin/qr_solve.h +++ /dev/null @@ -1,44 +0,0 @@ -/** - * Marlin 3D Printer Firmware - * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] - * - * Based on Sprinter and grbl. - * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - * - */ - -#include "MarlinConfig.h" - -#if ENABLED(AUTO_BED_LEVELING_LINEAR) - -void daxpy(int n, float da, float dx[], int incx, float dy[], int incy); -float ddot(int n, float dx[], int incx, float dy[], int incy); -float dnrm2(int n, float x[], int incx); -void dqrank(float a[], int lda, int m, int n, float tol, int* kr, - int jpvt[], float qraux[]); -void dqrdc(float a[], int lda, int n, int p, float qraux[], int jpvt[], - float work[], int job); -int dqrls(float a[], int lda, int m, int n, float tol, int* kr, float b[], - float x[], float rsd[], int jpvt[], float qraux[], int itask); -void dqrlss(float a[], int lda, int m, int n, int kr, float b[], float x[], - float rsd[], int jpvt[], float qraux[]); -int dqrsl(float a[], int lda, int n, int k, float qraux[], float y[], - float qy[], float qty[], float b[], float rsd[], float ab[], int job); -void dscal(int n, float sa, float x[], int incx); -void dswap(int n, float x[], int incx, float y[], int incy); -void qr_solve(float x[], int m, int n, float a[], float b[]); - -#endif