Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
qr_solve.c
Go to the documentation of this file.
1 /*
2  * This is part of the qr_solve library from John Burkardt.
3  * http://people.sc.fsu.edu/~jburkardt/c_src/qr_solve/qr_solve.html
4  *
5  * It is slightly modified to make it compile on simple microprocessors,
6  * and to remove all dynamic memory.
7  *
8  * This code is distributed under the GNU LGPL license.
9  */
10 
11 #include "std.h"
12 # include <stdlib.h>
13 # include <math.h>
14 
15 # include "qr_solve.h"
16 # include "r8lib_min.h"
17 
18 #define DEBUG_FPRINTF(...)
19 #define DEBUG_EXIT(...)
20 
21 /******************************************************************************/
22 
23 void daxpy ( int n, float da, float dx[], int incx, float dy[], int incy )
24 
25 /******************************************************************************/
26 /*
27  Purpose:
28 
29  DAXPY computes constant times a vector plus a vector.
30 
31  Discussion:
32 
33  This routine uses unrolled loops for increments equal to one.
34 
35  Licensing:
36 
37  This code is distributed under the GNU LGPL license.
38 
39  Modified:
40 
41  30 March 2007
42 
43  Author:
44 
45  C version by John Burkardt
46 
47  Reference:
48 
49  Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
50  LINPACK User's Guide,
51  SIAM, 1979.
52 
53  Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
54  Basic Linear Algebra Subprograms for Fortran Usage,
55  Algorithm 539,
56  ACM Transactions on Mathematical Software,
57  Volume 5, Number 3, September 1979, pages 308-323.
58 
59  Parameters:
60 
61  Input, int N, the number of elements in DX and DY.
62 
63  Input, float DA, the multiplier of DX.
64 
65  Input, float DX[*], the first vector.
66 
67  Input, int INCX, the increment between successive entries of DX.
68 
69  Input/output, float DY[*], the second vector.
70  On output, DY[*] has been replaced by DY[*] + DA * DX[*].
71 
72  Input, int INCY, the increment between successive entries of DY.
73 */
74 {
75  int i;
76  int ix;
77  int iy;
78  int m;
79 
80  if ( n <= 0 )
81  {
82  return;
83  }
84 
85  if ( da == 0.0 )
86  {
87  return;
88  }
89 /*
90  Code for unequal increments or equal increments
91  not equal to 1.
92 */
93  if ( incx != 1 || incy != 1 )
94  {
95  if ( 0 <= incx )
96  {
97  ix = 0;
98  }
99  else
100  {
101  ix = ( - n + 1 ) * incx;
102  }
103 
104  if ( 0 <= incy )
105  {
106  iy = 0;
107  }
108  else
109  {
110  iy = ( - n + 1 ) * incy;
111  }
112 
113  for ( i = 0; i < n; i++ )
114  {
115  dy[iy] = dy[iy] + da * dx[ix];
116  ix = ix + incx;
117  iy = iy + incy;
118  }
119  }
120 /*
121  Code for both increments equal to 1.
122 */
123  else
124  {
125  m = n % 4;
126 
127  for ( i = 0; i < m; i++ )
128  {
129  dy[i] = dy[i] + da * dx[i];
130  }
131 
132  for ( i = m; i < n; i = i + 4 )
133  {
134  dy[i ] = dy[i ] + da * dx[i ];
135  dy[i+1] = dy[i+1] + da * dx[i+1];
136  dy[i+2] = dy[i+2] + da * dx[i+2];
137  dy[i+3] = dy[i+3] + da * dx[i+3];
138  }
139  }
140  return;
141 }
142 /******************************************************************************/
143 
144 float ddot ( int n, float dx[], int incx, float dy[], int incy )
145 
146 /******************************************************************************/
147 /*
148  Purpose:
149 
150  DDOT forms the dot product of two vectors.
151 
152  Discussion:
153 
154  This routine uses unrolled loops for increments equal to one.
155 
156  Licensing:
157 
158  This code is distributed under the GNU LGPL license.
159 
160  Modified:
161 
162  30 March 2007
163 
164  Author:
165 
166  C version by John Burkardt
167 
168  Reference:
169 
170  Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
171  LINPACK User's Guide,
172  SIAM, 1979.
173 
174  Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
175  Basic Linear Algebra Subprograms for Fortran Usage,
176  Algorithm 539,
177  ACM Transactions on Mathematical Software,
178  Volume 5, Number 3, September 1979, pages 308-323.
179 
180  Parameters:
181 
182  Input, int N, the number of entries in the vectors.
183 
184  Input, float DX[*], the first vector.
185 
186  Input, int INCX, the increment between successive entries in DX.
187 
188  Input, float DY[*], the second vector.
189 
190  Input, int INCY, the increment between successive entries in DY.
191 
192  Output, float DDOT, the sum of the product of the corresponding
193  entries of DX and DY.
194 */
195 {
196  float dtemp;
197  int i;
198  int ix;
199  int iy;
200  int m;
201 
202  dtemp = 0.0;
203 
204  if ( n <= 0 )
205  {
206  return dtemp;
207  }
208 /*
209  Code for unequal increments or equal increments
210  not equal to 1.
211 */
212  if ( incx != 1 || incy != 1 )
213  {
214  if ( 0 <= incx )
215  {
216  ix = 0;
217  }
218  else
219  {
220  ix = ( - n + 1 ) * incx;
221  }
222 
223  if ( 0 <= incy )
224  {
225  iy = 0;
226  }
227  else
228  {
229  iy = ( - n + 1 ) * incy;
230  }
231 
232  for ( i = 0; i < n; i++ )
233  {
234  dtemp = dtemp + dx[ix] * dy[iy];
235  ix = ix + incx;
236  iy = iy + incy;
237  }
238  }
239 /*
240  Code for both increments equal to 1.
241 */
242  else
243  {
244  m = n % 5;
245 
246  for ( i = 0; i < m; i++ )
247  {
248  dtemp = dtemp + dx[i] * dy[i];
249  }
250 
251  for ( i = m; i < n; i = i + 5 )
252  {
253  dtemp = dtemp + dx[i ] * dy[i ]
254  + dx[i+1] * dy[i+1]
255  + dx[i+2] * dy[i+2]
256  + dx[i+3] * dy[i+3]
257  + dx[i+4] * dy[i+4];
258  }
259  }
260  return dtemp;
261 }
262 /******************************************************************************/
263 
264 float dnrm2 ( int n, float x[], int incx )
265 
266 /******************************************************************************/
267 /*
268  Purpose:
269 
270  DNRM2 returns the euclidean norm of a vector.
271 
272  Discussion:
273 
274  DNRM2 ( X ) = sqrt ( X' * X )
275 
276  Licensing:
277 
278  This code is distributed under the GNU LGPL license.
279 
280  Modified:
281 
282  30 March 2007
283 
284  Author:
285 
286  C version by John Burkardt
287 
288  Reference:
289 
290  Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
291  LINPACK User's Guide,
292  SIAM, 1979.
293 
294  Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
295  Basic Linear Algebra Subprograms for Fortran Usage,
296  Algorithm 539,
297  ACM Transactions on Mathematical Software,
298  Volume 5, Number 3, September 1979, pages 308-323.
299 
300  Parameters:
301 
302  Input, int N, the number of entries in the vector.
303 
304  Input, float X[*], the vector whose norm is to be computed.
305 
306  Input, int INCX, the increment between successive entries of X.
307 
308  Output, float DNRM2, the Euclidean norm of X.
309 */
310 {
311  float absxi;
312  int i;
313  int ix;
314  float norm;
315  float scale;
316  float ssq;
317 
318  if ( n < 1 || incx < 1 )
319  {
320  norm = 0.0;
321  }
322  else if ( n == 1 )
323  {
324  norm = fabs ( x[0] );
325  }
326  else
327  {
328  scale = 0.0;
329  ssq = 1.0;
330  ix = 0;
331 
332  for ( i = 0; i < n; i++ )
333  {
334  if ( x[ix] != 0.0 )
335  {
336  absxi = fabs ( x[ix] );
337  if ( scale < absxi )
338  {
339  ssq = 1.0 + ssq * ( scale / absxi ) * ( scale / absxi );
340  scale = absxi;
341  }
342  else
343  {
344  ssq = ssq + ( absxi / scale ) * ( absxi / scale );
345  }
346  }
347  ix = ix + incx;
348  }
349 
350  norm = scale * sqrt ( ssq );
351  }
352 
353  return norm;
354 }
355 /******************************************************************************/
356 
357 void dqrank ( float a[], int lda, int m, int n, float tol, int *kr,
358  int jpvt[], float qraux[] )
359 
360 /******************************************************************************/
361 /*
362  Purpose:
363 
364  DQRANK computes the QR factorization of a rectangular matrix.
365 
366  Discussion:
367 
368  This routine is used in conjunction with DQRLSS to solve
369  overdetermined, underdetermined and singular linear systems
370  in a least squares sense.
371 
372  DQRANK uses the LINPACK subroutine DQRDC to compute the QR
373  factorization, with column pivoting, of an M by N matrix A.
374  The numerical rank is determined using the tolerance TOL.
375 
376  Note that on output, ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate
377  of the condition number of the matrix of independent columns,
378  and of R. This estimate will be <= 1/TOL.
379 
380  Licensing:
381 
382  This code is distributed under the GNU LGPL license.
383 
384  Modified:
385 
386  21 April 2012
387 
388  Author:
389 
390  C version by John Burkardt.
391 
392  Reference:
393 
394  Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
395  LINPACK User's Guide,
396  SIAM, 1979,
397  ISBN13: 978-0-898711-72-1,
398  LC: QA214.L56.
399 
400  Parameters:
401 
402  Input/output, float A[LDA*N]. On input, the matrix whose
403  decomposition is to be computed. On output, the information from DQRDC.
404  The triangular matrix R of the QR factorization is contained in the
405  upper triangle and information needed to recover the orthogonal
406  matrix Q is stored below the diagonal in A and in the vector QRAUX.
407 
408  Input, int LDA, the leading dimension of A, which must
409  be at least M.
410 
411  Input, int M, the number of rows of A.
412 
413  Input, int N, the number of columns of A.
414 
415  Input, float TOL, a relative tolerance used to determine the
416  numerical rank. The problem should be scaled so that all the elements
417  of A have roughly the same absolute accuracy, EPS. Then a reasonable
418  value for TOL is roughly EPS divided by the magnitude of the largest
419  element.
420 
421  Output, int *KR, the numerical rank.
422 
423  Output, int JPVT[N], the pivot information from DQRDC.
424  Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly
425  independent to within the tolerance TOL and the remaining columns
426  are linearly dependent.
427 
428  Output, float QRAUX[N], will contain extra information defining
429  the QR factorization.
430 */
431 {
432  int i;
433  int j;
434  int job;
435  int k;
436  /*float *work;*/
437 
438  for ( i = 0; i < n; i++ )
439  {
440  jpvt[i] = 0;
441  }
442 
443  float work[n];
444  /*work = ( float * ) malloc ( n * sizeof ( float ) );*/
445  job = 1;
446 
447  dqrdc ( a, lda, m, n, qraux, jpvt, work, job );
448 
449  *kr = 0;
450  k = i4_min ( m, n );
451 
452  for ( j = 0; j < k; j++ )
453  {
454  if ( fabs ( a[j+j*lda] ) <= tol * fabs ( a[0+0*lda] ) )
455  {
456  return;
457  }
458  *kr = j + 1;
459  }
460 
461  return;
462 }
463 /******************************************************************************/
464 
465 void dqrdc ( float a[], int lda, int n, int p, float qraux[], int jpvt[],
466  float work[], int job )
467 
468 /******************************************************************************/
469 /*
470  Purpose:
471 
472  DQRDC computes the QR factorization of a real rectangular matrix.
473 
474  Discussion:
475 
476  DQRDC uses Householder transformations.
477 
478  Column pivoting based on the 2-norms of the reduced columns may be
479  performed at the user's option.
480 
481  Licensing:
482 
483  This code is distributed under the GNU LGPL license.
484 
485  Modified:
486 
487  07 June 2005
488 
489  Author:
490 
491  C version by John Burkardt.
492 
493  Reference:
494 
495  Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
496  LINPACK User's Guide,
497  SIAM, (Society for Industrial and Applied Mathematics),
498  3600 University City Science Center,
499  Philadelphia, PA, 19104-2688.
500  ISBN 0-89871-172-X
501 
502  Parameters:
503 
504  Input/output, float A(LDA,P). On input, the N by P matrix
505  whose decomposition is to be computed. On output, A contains in
506  its upper triangle the upper triangular matrix R of the QR
507  factorization. Below its diagonal A contains information from
508  which the orthogonal part of the decomposition can be recovered.
509  Note that if pivoting has been requested, the decomposition is not that
510  of the original matrix A but that of A with its columns permuted
511  as described by JPVT.
512 
513  Input, int LDA, the leading dimension of the array A. LDA must
514  be at least N.
515 
516  Input, int N, the number of rows of the matrix A.
517 
518  Input, int P, the number of columns of the matrix A.
519 
520  Output, float QRAUX[P], contains further information required
521  to recover the orthogonal part of the decomposition.
522 
523  Input/output, integer JPVT[P]. On input, JPVT contains integers that
524  control the selection of the pivot columns. The K-th column A(*,K) of A
525  is placed in one of three classes according to the value of JPVT(K).
526  > 0, then A(K) is an initial column.
527  = 0, then A(K) is a free column.
528  < 0, then A(K) is a final column.
529  Before the decomposition is computed, initial columns are moved to
530  the beginning of the array A and final columns to the end. Both
531  initial and final columns are frozen in place during the computation
532  and only free columns are moved. At the K-th stage of the
533  reduction, if A(*,K) is occupied by a free column it is interchanged
534  with the free column of largest reduced norm. JPVT is not referenced
535  if JOB == 0. On output, JPVT(K) contains the index of the column of the
536  original matrix that has been interchanged into the K-th column, if
537  pivoting was requested.
538 
539  Workspace, float WORK[P]. WORK is not referenced if JOB == 0.
540 
541  Input, int JOB, initiates column pivoting.
542  0, no pivoting is done.
543  nonzero, pivoting is done.
544 */
545 {
546  int j;
547  int jp;
548  int l;
549  int lup;
550  int maxj;
551  float maxnrm;
552  float nrmxl;
553  int pl;
554  int pu;
555  int swapj;
556  float t;
557  float tt;
558 
559  pl = 1;
560  pu = 0;
561 /*
562  If pivoting is requested, rearrange the columns.
563 */
564  if ( job != 0 )
565  {
566  for ( j = 1; j <= p; j++ )
567  {
568  swapj = ( 0 < jpvt[j-1] );
569 
570  if ( jpvt[j-1] < 0 )
571  {
572  jpvt[j-1] = -j;
573  }
574  else
575  {
576  jpvt[j-1] = j;
577  }
578 
579  if ( swapj )
580  {
581  if ( j != pl )
582  {
583  dswap ( n, a+0+(pl-1)*lda, 1, a+0+(j-1), 1 );
584  }
585  jpvt[j-1] = jpvt[pl-1];
586  jpvt[pl-1] = j;
587  pl = pl + 1;
588  }
589  }
590  pu = p;
591 
592  for ( j = p; 1 <= j; j-- )
593  {
594  if ( jpvt[j-1] < 0 )
595  {
596  jpvt[j-1] = -jpvt[j-1];
597 
598  if ( j != pu )
599  {
600  dswap ( n, a+0+(pu-1)*lda, 1, a+0+(j-1)*lda, 1 );
601  jp = jpvt[pu-1];
602  jpvt[pu-1] = jpvt[j-1];
603  jpvt[j-1] = jp;
604  }
605  pu = pu - 1;
606  }
607  }
608  }
609 /*
610  Compute the norms of the free columns.
611 */
612  for ( j = pl; j <= pu; j++ )
613  {
614  qraux[j-1] = dnrm2 ( n, a+0+(j-1)*lda, 1 );
615  }
616 
617  for ( j = pl; j <= pu; j++ )
618  {
619  work[j-1] = qraux[j-1];
620  }
621 /*
622  Perform the Householder reduction of A.
623 */
624  lup = i4_min ( n, p );
625 
626  for ( l = 1; l <= lup; l++ )
627  {
628 /*
629  Bring the column of largest norm into the pivot position.
630 */
631  if ( pl <= l && l < pu )
632  {
633  maxnrm = 0.0;
634  maxj = l;
635  for ( j = l; j <= pu; j++ )
636  {
637  if ( maxnrm < qraux[j-1] )
638  {
639  maxnrm = qraux[j-1];
640  maxj = j;
641  }
642  }
643 
644  if ( maxj != l )
645  {
646  dswap ( n, a+0+(l-1)*lda, 1, a+0+(maxj-1)*lda, 1 );
647  qraux[maxj-1] = qraux[l-1];
648  work[maxj-1] = work[l-1];
649  jp = jpvt[maxj-1];
650  jpvt[maxj-1] = jpvt[l-1];
651  jpvt[l-1] = jp;
652  }
653  }
654 /*
655  Compute the Householder transformation for column L.
656 */
657  qraux[l-1] = 0.0;
658 
659  if ( l != n )
660  {
661  nrmxl = dnrm2 ( n-l+1, a+l-1+(l-1)*lda, 1 );
662 
663  if ( nrmxl != 0.0 )
664  {
665  if ( a[l-1+(l-1)*lda] != 0.0 )
666  {
667  nrmxl = nrmxl * r8_sign ( a[l-1+(l-1)*lda] );
668  }
669 
670  dscal ( n-l+1, 1.0 / nrmxl, a+l-1+(l-1)*lda, 1 );
671  a[l-1+(l-1)*lda] = 1.0 + a[l-1+(l-1)*lda];
672 /*
673  Apply the transformation to the remaining columns, updating the norms.
674 */
675  for ( j = l + 1; j <= p; j++ )
676  {
677  t = -ddot ( n-l+1, a+l-1+(l-1)*lda, 1, a+l-1+(j-1)*lda, 1 )
678  / a[l-1+(l-1)*lda];
679  daxpy ( n-l+1, t, a+l-1+(l-1)*lda, 1, a+l-1+(j-1)*lda, 1 );
680 
681  if ( pl <= j && j <= pu )
682  {
683  if ( qraux[j-1] != 0.0 )
684  {
685  tt = 1.0 - pow ( fabs ( a[l-1+(j-1)*lda] ) / qraux[j-1], 2 );
686  tt = r8_max ( tt, 0.0 );
687  t = tt;
688  tt = 1.0 + 0.05 * tt * pow ( qraux[j-1] / work[j-1], 2 );
689 
690  if ( tt != 1.0 )
691  {
692  qraux[j-1] = qraux[j-1] * sqrt ( t );
693  }
694  else
695  {
696  qraux[j-1] = dnrm2 ( n-l, a+l+(j-1)*lda, 1 );
697  work[j-1] = qraux[j-1];
698  }
699  }
700  }
701  }
702 /*
703  Save the transformation.
704 */
705  qraux[l-1] = a[l-1+(l-1)*lda];
706  a[l-1+(l-1)*lda] = -nrmxl;
707  }
708  }
709  }
710  return;
711 }
712 /******************************************************************************/
713 
714 int dqrls ( float a[], int lda, int m, int n, float tol, int *kr, float b[],
715  float x[], float rsd[], int jpvt[], float qraux[], int itask )
716 
717 /******************************************************************************/
718 /*
719  Purpose:
720 
721  DQRLS factors and solves a linear system in the least squares sense.
722 
723  Discussion:
724 
725  The linear system may be overdetermined, underdetermined or singular.
726  The solution is obtained using a QR factorization of the
727  coefficient matrix.
728 
729  DQRLS can be efficiently used to solve several least squares
730  problems with the same matrix A. The first system is solved
731  with ITASK = 1. The subsequent systems are solved with
732  ITASK = 2, to avoid the recomputation of the matrix factors.
733  The parameters KR, JPVT, and QRAUX must not be modified
734  between calls to DQRLS.
735 
736  DQRLS is used to solve in a least squares sense
737  overdetermined, underdetermined and singular linear systems.
738  The system is A*X approximates B where A is M by N.
739  B is a given M-vector, and X is the N-vector to be computed.
740  A solution X is found which minimimzes the sum of squares (2-norm)
741  of the residual, A*X - B.
742 
743  The numerical rank of A is determined using the tolerance TOL.
744 
745  DQRLS uses the LINPACK subroutine DQRDC to compute the QR
746  factorization, with column pivoting, of an M by N matrix A.
747 
748  Licensing:
749 
750  This code is distributed under the GNU LGPL license.
751 
752  Modified:
753 
754  10 September 2012
755 
756  Author:
757 
758  C version by John Burkardt.
759 
760  Reference:
761 
762  David Kahaner, Cleve Moler, Steven Nash,
763  Numerical Methods and Software,
764  Prentice Hall, 1989,
765  ISBN: 0-13-627258-4,
766  LC: TA345.K34.
767 
768  Parameters:
769 
770  Input/output, float A[LDA*N], an M by N matrix.
771  On input, the matrix whose decomposition is to be computed.
772  In a least squares data fitting problem, A(I,J) is the
773  value of the J-th basis (model) function at the I-th data point.
774  On output, A contains the output from DQRDC. The triangular matrix R
775  of the QR factorization is contained in the upper triangle and
776  information needed to recover the orthogonal matrix Q is stored
777  below the diagonal in A and in the vector QRAUX.
778 
779  Input, int LDA, the leading dimension of A.
780 
781  Input, int M, the number of rows of A.
782 
783  Input, int N, the number of columns of A.
784 
785  Input, float TOL, a relative tolerance used to determine the
786  numerical rank. The problem should be scaled so that all the elements
787  of A have roughly the same absolute accuracy EPS. Then a reasonable
788  value for TOL is roughly EPS divided by the magnitude of the largest
789  element.
790 
791  Output, int *KR, the numerical rank.
792 
793  Input, float B[M], the right hand side of the linear system.
794 
795  Output, float X[N], a least squares solution to the linear
796  system.
797 
798  Output, float RSD[M], the residual, B - A*X. RSD may
799  overwrite B.
800 
801  Workspace, int JPVT[N], required if ITASK = 1.
802  Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly
803  independent to within the tolerance TOL and the remaining columns
804  are linearly dependent. ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate
805  of the condition number of the matrix of independent columns,
806  and of R. This estimate will be <= 1/TOL.
807 
808  Workspace, float QRAUX[N], required if ITASK = 1.
809 
810  Input, int ITASK.
811  1, DQRLS factors the matrix A and solves the least squares problem.
812  2, DQRLS assumes that the matrix A was factored with an earlier
813  call to DQRLS, and only solves the least squares problem.
814 
815  Output, int DQRLS, error code.
816  0: no error
817  -1: LDA < M (fatal error)
818  -2: N < 1 (fatal error)
819  -3: ITASK < 1 (fatal error)
820 */
821 {
822  int ind;
823 
824  if ( lda < m )
825  {
826  DEBUG_FPRINTF ( stderr, "\n" );
827  DEBUG_FPRINTF ( stderr, "DQRLS - Fatal error!\n" );
828  DEBUG_FPRINTF ( stderr, " LDA < M.\n" );
829  ind = -1;
830  return ind;
831  }
832 
833  if ( n <= 0 )
834  {
835  DEBUG_FPRINTF ( stderr, "\n" );
836  DEBUG_FPRINTF ( stderr, "DQRLS - Fatal error!\n" );
837  DEBUG_FPRINTF ( stderr, " N <= 0.\n" );
838  ind = -2;
839  return ind;
840  }
841 
842  if ( itask < 1 )
843  {
844  DEBUG_FPRINTF ( stderr, "\n" );
845  DEBUG_FPRINTF ( stderr, "DQRLS - Fatal error!\n" );
846  DEBUG_FPRINTF ( stderr, " ITASK < 1.\n" );
847  ind = -3;
848  return ind;
849  }
850 
851  ind = 0;
852 /*
853  Factor the matrix.
854 */
855  if ( itask == 1 )
856  {
857  dqrank ( a, lda, m, n, tol, kr, jpvt, qraux );
858  }
859 /*
860  Solve the least-squares problem.
861 */
862  dqrlss ( a, lda, m, n, *kr, b, x, rsd, jpvt, qraux );
863 
864  return ind;
865 }
866 /******************************************************************************/
867 void dqrlss ( float a[], int lda, int m, int n, int kr, float b[], float x[],
868  float rsd[], int jpvt[], float qraux[] )
869 
870 /******************************************************************************/
871 /*
872  Purpose:
873 
874  DQRLSS solves a linear system in a least squares sense.
875 
876  Discussion:
877 
878  DQRLSS must be preceeded by a call to DQRANK.
879 
880  The system is to be solved is
881  A * X = B
882  where
883  A is an M by N matrix with rank KR, as determined by DQRANK,
884  B is a given M-vector,
885  X is the N-vector to be computed.
886 
887  A solution X, with at most KR nonzero components, is found which
888  minimizes the 2-norm of the residual (A*X-B).
889 
890  Once the matrix A has been formed, DQRANK should be
891  called once to decompose it. Then, for each right hand
892  side B, DQRLSS should be called once to obtain the
893  solution and residual.
894 
895  Licensing:
896 
897  This code is distributed under the GNU LGPL license.
898 
899  Modified:
900 
901  10 September 2012
902 
903  Author:
904 
905  C version by John Burkardt
906 
907  Parameters:
908 
909  Input, float A[LDA*N], the QR factorization information
910  from DQRANK. The triangular matrix R of the QR factorization is
911  contained in the upper triangle and information needed to recover
912  the orthogonal matrix Q is stored below the diagonal in A and in
913  the vector QRAUX.
914 
915  Input, int LDA, the leading dimension of A, which must
916  be at least M.
917 
918  Input, int M, the number of rows of A.
919 
920  Input, int N, the number of columns of A.
921 
922  Input, int KR, the rank of the matrix, as estimated by DQRANK.
923 
924  Input, float B[M], the right hand side of the linear system.
925 
926  Output, float X[N], a least squares solution to the
927  linear system.
928 
929  Output, float RSD[M], the residual, B - A*X. RSD may
930  overwite B.
931 
932  Input, int JPVT[N], the pivot information from DQRANK.
933  Columns JPVT[0], ..., JPVT[KR-1] of the original matrix are linearly
934  independent to within the tolerance TOL and the remaining columns
935  are linearly dependent.
936 
937  Input, float QRAUX[N], auxiliary information from DQRANK
938  defining the QR factorization.
939 */
940 {
941  int i;
942  int info UNUSED;
943  int j;
944  int job;
945  int k;
946  float t;
947 
948  if ( kr != 0 )
949  {
950  job = 110;
951  info = dqrsl ( a, lda, m, kr, qraux, b, rsd, rsd, x, rsd, rsd, job );
952  }
953 
954  for ( i = 0; i < n; i++ )
955  {
956  jpvt[i] = - jpvt[i];
957  }
958 
959  for ( i = kr; i < n; i++ )
960  {
961  x[i] = 0.0;
962  }
963 
964  for ( j = 1; j <= n; j++ )
965  {
966  if ( jpvt[j-1] <= 0 )
967  {
968  k = - jpvt[j-1];
969  jpvt[j-1] = k;
970 
971  while ( k != j )
972  {
973  t = x[j-1];
974  x[j-1] = x[k-1];
975  x[k-1] = t;
976  jpvt[k-1] = -jpvt[k-1];
977  k = jpvt[k-1];
978  }
979  }
980  }
981  return;
982 }
983 /******************************************************************************/
984 
985 int dqrsl ( float a[], int lda, int n, int k, float qraux[], float y[],
986  float qy[], float qty[], float b[], float rsd[], float ab[], int job )
987 
988 /******************************************************************************/
989 /*
990  Purpose:
991 
992  DQRSL computes transformations, projections, and least squares solutions.
993 
994  Discussion:
995 
996  DQRSL requires the output of DQRDC.
997 
998  For K <= min(N,P), let AK be the matrix
999 
1000  AK = ( A(JPVT[0]), A(JPVT(2)), ..., A(JPVT(K)) )
1001 
1002  formed from columns JPVT[0], ..., JPVT(K) of the original
1003  N by P matrix A that was input to DQRDC. If no pivoting was
1004  done, AK consists of the first K columns of A in their
1005  original order. DQRDC produces a factored orthogonal matrix Q
1006  and an upper triangular matrix R such that
1007 
1008  AK = Q * (R)
1009  (0)
1010 
1011  This information is contained in coded form in the arrays
1012  A and QRAUX.
1013 
1014  The parameters QY, QTY, B, RSD, and AB are not referenced
1015  if their computation is not requested and in this case
1016  can be replaced by dummy variables in the calling program.
1017  To save storage, the user may in some cases use the same
1018  array for different parameters in the calling sequence. A
1019  frequently occuring example is when one wishes to compute
1020  any of B, RSD, or AB and does not need Y or QTY. In this
1021  case one may identify Y, QTY, and one of B, RSD, or AB, while
1022  providing separate arrays for anything else that is to be
1023  computed.
1024 
1025  Thus the calling sequence
1026 
1027  dqrsl ( a, lda, n, k, qraux, y, dum, y, b, y, dum, 110, info )
1028 
1029  will result in the computation of B and RSD, with RSD
1030  overwriting Y. More generally, each item in the following
1031  list contains groups of permissible identifications for
1032  a single calling sequence.
1033 
1034  1. (Y,QTY,B) (RSD) (AB) (QY)
1035 
1036  2. (Y,QTY,RSD) (B) (AB) (QY)
1037 
1038  3. (Y,QTY,AB) (B) (RSD) (QY)
1039 
1040  4. (Y,QY) (QTY,B) (RSD) (AB)
1041 
1042  5. (Y,QY) (QTY,RSD) (B) (AB)
1043 
1044  6. (Y,QY) (QTY,AB) (B) (RSD)
1045 
1046  In any group the value returned in the array allocated to
1047  the group corresponds to the last member of the group.
1048 
1049  Licensing:
1050 
1051  This code is distributed under the GNU LGPL license.
1052 
1053  Modified:
1054 
1055  07 June 2005
1056 
1057  Author:
1058 
1059  C version by John Burkardt.
1060 
1061  Reference:
1062 
1063  Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
1064  LINPACK User's Guide,
1065  SIAM, (Society for Industrial and Applied Mathematics),
1066  3600 University City Science Center,
1067  Philadelphia, PA, 19104-2688.
1068  ISBN 0-89871-172-X
1069 
1070  Parameters:
1071 
1072  Input, float A[LDA*P], contains the output of DQRDC.
1073 
1074  Input, int LDA, the leading dimension of the array A.
1075 
1076  Input, int N, the number of rows of the matrix AK. It must
1077  have the same value as N in DQRDC.
1078 
1079  Input, int K, the number of columns of the matrix AK. K
1080  must not be greater than min(N,P), where P is the same as in the
1081  calling sequence to DQRDC.
1082 
1083  Input, float QRAUX[P], the auxiliary output from DQRDC.
1084 
1085  Input, float Y[N], a vector to be manipulated by DQRSL.
1086 
1087  Output, float QY[N], contains Q * Y, if requested.
1088 
1089  Output, float QTY[N], contains Q' * Y, if requested.
1090 
1091  Output, float B[K], the solution of the least squares problem
1092  minimize norm2 ( Y - AK * B),
1093  if its computation has been requested. Note that if pivoting was
1094  requested in DQRDC, the J-th component of B will be associated with
1095  column JPVT(J) of the original matrix A that was input into DQRDC.
1096 
1097  Output, float RSD[N], the least squares residual Y - AK * B,
1098  if its computation has been requested. RSD is also the orthogonal
1099  projection of Y onto the orthogonal complement of the column space
1100  of AK.
1101 
1102  Output, float AB[N], the least squares approximation Ak * B,
1103  if its computation has been requested. AB is also the orthogonal
1104  projection of Y onto the column space of A.
1105 
1106  Input, integer JOB, specifies what is to be computed. JOB has
1107  the decimal expansion ABCDE, with the following meaning:
1108 
1109  if A != 0, compute QY.
1110  if B != 0, compute QTY.
1111  if C != 0, compute QTY and B.
1112  if D != 0, compute QTY and RSD.
1113  if E != 0, compute QTY and AB.
1114 
1115  Note that a request to compute B, RSD, or AB automatically triggers
1116  the computation of QTY, for which an array must be provided in the
1117  calling sequence.
1118 
1119  Output, int DQRSL, is zero unless the computation of B has
1120  been requested and R is exactly singular. In this case, INFO is the
1121  index of the first zero diagonal element of R, and B is left unaltered.
1122 */
1123 {
1124  int cab;
1125  int cb;
1126  int cqty;
1127  int cqy;
1128  int cr;
1129  int i;
1130  int info;
1131  int j;
1132  int jj;
1133  int ju;
1134  float t;
1135  float temp;
1136 /*
1137  Set INFO flag.
1138 */
1139  info = 0;
1140 /*
1141  Determine what is to be computed.
1142 */
1143  cqy = ( job / 10000 != 0 );
1144  cqty = ( ( job % 10000 ) != 0 );
1145  cb = ( ( job % 1000 ) / 100 != 0 );
1146  cr = ( ( job % 100 ) / 10 != 0 );
1147  cab = ( ( job % 10 ) != 0 );
1148 
1149  ju = i4_min ( k, n-1 );
1150 /*
1151  Special action when N = 1.
1152 */
1153  if ( ju == 0 )
1154  {
1155  if ( cqy )
1156  {
1157  qy[0] = y[0];
1158  }
1159 
1160  if ( cqty )
1161  {
1162  qty[0] = y[0];
1163  }
1164 
1165  if ( cab )
1166  {
1167  ab[0] = y[0];
1168  }
1169 
1170  if ( cb )
1171  {
1172  if ( a[0+0*lda] == 0.0 )
1173  {
1174  info = 1;
1175  }
1176  else
1177  {
1178  b[0] = y[0] / a[0+0*lda];
1179  }
1180  }
1181 
1182  if ( cr )
1183  {
1184  rsd[0] = 0.0;
1185  }
1186  return info;
1187  }
1188 /*
1189  Set up to compute QY or QTY.
1190 */
1191  if ( cqy )
1192  {
1193  for ( i = 1; i <= n; i++ )
1194  {
1195  qy[i-1] = y[i-1];
1196  }
1197  }
1198 
1199  if ( cqty )
1200  {
1201  for ( i = 1; i <= n; i++ )
1202  {
1203  qty[i-1] = y[i-1];
1204  }
1205  }
1206 /*
1207  Compute QY.
1208 */
1209  if ( cqy )
1210  {
1211  for ( jj = 1; jj <= ju; jj++ )
1212  {
1213  j = ju - jj + 1;
1214 
1215  if ( qraux[j-1] != 0.0 )
1216  {
1217  temp = a[j-1+(j-1)*lda];
1218  a[j-1+(j-1)*lda] = qraux[j-1];
1219  t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, qy+j-1, 1 ) / a[j-1+(j-1)*lda];
1220  daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, qy+j-1, 1 );
1221  a[j-1+(j-1)*lda] = temp;
1222  }
1223  }
1224  }
1225 /*
1226  Compute Q'*Y.
1227 */
1228  if ( cqty )
1229  {
1230  for ( j = 1; j <= ju; j++ )
1231  {
1232  if ( qraux[j-1] != 0.0 )
1233  {
1234  temp = a[j-1+(j-1)*lda];
1235  a[j-1+(j-1)*lda] = qraux[j-1];
1236  t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, qty+j-1, 1 ) / a[j-1+(j-1)*lda];
1237  daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, qty+j-1, 1 );
1238  a[j-1+(j-1)*lda] = temp;
1239  }
1240  }
1241  }
1242 /*
1243  Set up to compute B, RSD, or AB.
1244 */
1245  if ( cb )
1246  {
1247  for ( i = 1; i <= k; i++ )
1248  {
1249  b[i-1] = qty[i-1];
1250  }
1251  }
1252 
1253  if ( cab )
1254  {
1255  for ( i = 1; i <= k; i++ )
1256  {
1257  ab[i-1] = qty[i-1];
1258  }
1259  }
1260 
1261  if ( cr && k < n )
1262  {
1263  for ( i = k+1; i <= n; i++ )
1264  {
1265  rsd[i-1] = qty[i-1];
1266  }
1267  }
1268 
1269  if ( cab && k+1 <= n )
1270  {
1271  for ( i = k+1; i <= n; i++ )
1272  {
1273  ab[i-1] = 0.0;
1274  }
1275  }
1276 
1277  if ( cr )
1278  {
1279  for ( i = 1; i <= k; i++ )
1280  {
1281  rsd[i-1] = 0.0;
1282  }
1283  }
1284 /*
1285  Compute B.
1286 */
1287  if ( cb )
1288  {
1289  for ( jj = 1; jj <= k; jj++ )
1290  {
1291  j = k - jj + 1;
1292 
1293  if ( a[j-1+(j-1)*lda] == 0.0 )
1294  {
1295  info = j;
1296  break;
1297  }
1298 
1299  b[j-1] = b[j-1] / a[j-1+(j-1)*lda];
1300 
1301  if ( j != 1 )
1302  {
1303  t = -b[j-1];
1304  daxpy ( j-1, t, a+0+(j-1)*lda, 1, b, 1 );
1305  }
1306  }
1307  }
1308 /*
1309  Compute RSD or AB as required.
1310 */
1311  if ( cr || cab )
1312  {
1313  for ( jj = 1; jj <= ju; jj++ )
1314  {
1315  j = ju - jj + 1;
1316 
1317  if ( qraux[j-1] != 0.0 )
1318  {
1319  temp = a[j-1+(j-1)*lda];
1320  a[j-1+(j-1)*lda] = qraux[j-1];
1321 
1322  if ( cr )
1323  {
1324  t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, rsd+j-1, 1 )
1325  / a[j-1+(j-1)*lda];
1326  daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, rsd+j-1, 1 );
1327  }
1328 
1329  if ( cab )
1330  {
1331  t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, ab+j-1, 1 )
1332  / a[j-1+(j-1)*lda];
1333  daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, ab+j-1, 1 );
1334  }
1335  a[j-1+(j-1)*lda] = temp;
1336  }
1337  }
1338  }
1339 
1340  return info;
1341 }
1342 /******************************************************************************/
1343 
1344 void dscal ( int n, float sa, float x[], int incx )
1345 
1346 /******************************************************************************/
1347 /*
1348  Purpose:
1349 
1350  DSCAL scales a vector by a constant.
1351 
1352  Licensing:
1353 
1354  This code is distributed under the GNU LGPL license.
1355 
1356  Modified:
1357 
1358  30 March 2007
1359 
1360  Author:
1361 
1362  C version by John Burkardt
1363 
1364  Reference:
1365 
1366  Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
1367  LINPACK User's Guide,
1368  SIAM, 1979.
1369 
1370  Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
1371  Basic Linear Algebra Subprograms for Fortran Usage,
1372  Algorithm 539,
1373  ACM Transactions on Mathematical Software,
1374  Volume 5, Number 3, September 1979, pages 308-323.
1375 
1376  Parameters:
1377 
1378  Input, int N, the number of entries in the vector.
1379 
1380  Input, float SA, the multiplier.
1381 
1382  Input/output, float X[*], the vector to be scaled.
1383 
1384  Input, int INCX, the increment between successive entries of X.
1385 */
1386 {
1387  int i;
1388  int ix;
1389  int m;
1390 
1391  if ( n <= 0 )
1392  {
1393  }
1394  else if ( incx == 1 )
1395  {
1396  m = n % 5;
1397 
1398  for ( i = 0; i < m; i++ )
1399  {
1400  x[i] = sa * x[i];
1401  }
1402 
1403  for ( i = m; i < n; i = i + 5 )
1404  {
1405  x[i] = sa * x[i];
1406  x[i+1] = sa * x[i+1];
1407  x[i+2] = sa * x[i+2];
1408  x[i+3] = sa * x[i+3];
1409  x[i+4] = sa * x[i+4];
1410  }
1411  }
1412  else
1413  {
1414  if ( 0 <= incx )
1415  {
1416  ix = 0;
1417  }
1418  else
1419  {
1420  ix = ( - n + 1 ) * incx;
1421  }
1422 
1423  for ( i = 0; i < n; i++ )
1424  {
1425  x[ix] = sa * x[ix];
1426  ix = ix + incx;
1427  }
1428  }
1429  return;
1430 }
1431 /******************************************************************************/
1432 
1433 void dswap ( int n, float x[], int incx, float y[], int incy )
1434 
1435 /******************************************************************************/
1436 /*
1437  Purpose:
1438 
1439  DSWAP interchanges two vectors.
1440 
1441  Licensing:
1442 
1443  This code is distributed under the GNU LGPL license.
1444 
1445  Modified:
1446 
1447  30 March 2007
1448 
1449  Author:
1450 
1451  C version by John Burkardt
1452 
1453  Reference:
1454 
1455  Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
1456  LINPACK User's Guide,
1457  SIAM, 1979.
1458 
1459  Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
1460  Basic Linear Algebra Subprograms for Fortran Usage,
1461  Algorithm 539,
1462  ACM Transactions on Mathematical Software,
1463  Volume 5, Number 3, September 1979, pages 308-323.
1464 
1465  Parameters:
1466 
1467  Input, int N, the number of entries in the vectors.
1468 
1469  Input/output, float X[*], one of the vectors to swap.
1470 
1471  Input, int INCX, the increment between successive entries of X.
1472 
1473  Input/output, float Y[*], one of the vectors to swap.
1474 
1475  Input, int INCY, the increment between successive elements of Y.
1476 */
1477 {
1478  int i;
1479  int ix;
1480  int iy;
1481  int m;
1482  float temp;
1483 
1484  if ( n <= 0 )
1485  {
1486  }
1487  else if ( incx == 1 && incy == 1 )
1488  {
1489  m = n % 3;
1490 
1491  for ( i = 0; i < m; i++ )
1492  {
1493  temp = x[i];
1494  x[i] = y[i];
1495  y[i] = temp;
1496  }
1497 
1498  for ( i = m; i < n; i = i + 3 )
1499  {
1500  temp = x[i];
1501  x[i] = y[i];
1502  y[i] = temp;
1503 
1504  temp = x[i+1];
1505  x[i+1] = y[i+1];
1506  y[i+1] = temp;
1507 
1508  temp = x[i+2];
1509  x[i+2] = y[i+2];
1510  y[i+2] = temp;
1511  }
1512  }
1513  else
1514  {
1515  if ( 0 <= incx )
1516  {
1517  ix = 0;
1518  }
1519  else
1520  {
1521  ix = ( - n + 1 ) * incx;
1522  }
1523 
1524  if ( 0 <= incy )
1525  {
1526  iy = 0;
1527  }
1528  else
1529  {
1530  iy = ( - n + 1 ) * incy;
1531  }
1532 
1533  for ( i = 0; i < n; i++ )
1534  {
1535  temp = x[ix];
1536  x[ix] = y[iy];
1537  y[iy] = temp;
1538  ix = ix + incx;
1539  iy = iy + incy;
1540  }
1541 
1542  }
1543 
1544  return;
1545 }
1546 /******************************************************************************/
1547 
1548 void qr_solve ( int m, int n, float a[], float b[], float x[] )
1549 
1550 /******************************************************************************/
1551 /*
1552  Purpose:
1553 
1554  QR_SOLVE solves a linear system in the least squares sense.
1555 
1556  Discussion:
1557 
1558  If the matrix A has full column rank, then the solution X should be the
1559  unique vector that minimizes the Euclidean norm of the residual.
1560 
1561  If the matrix A does not have full column rank, then the solution is
1562  not unique; the vector X will minimize the residual norm, but so will
1563  various other vectors.
1564 
1565  Licensing:
1566 
1567  This code is distributed under the GNU LGPL license.
1568 
1569  Modified:
1570 
1571  11 September 2012
1572 
1573  Author:
1574 
1575  John Burkardt
1576 
1577  Reference:
1578 
1579  David Kahaner, Cleve Moler, Steven Nash,
1580  Numerical Methods and Software,
1581  Prentice Hall, 1989,
1582  ISBN: 0-13-627258-4,
1583  LC: TA345.K34.
1584 
1585  Parameters:
1586 
1587  Input, int M, the number of rows of A.
1588 
1589  Input, int N, the number of columns of A.
1590 
1591  Input, float A[M*N], the matrix.
1592 
1593  Input, float B[M], the right hand side.
1594 
1595  Output, float QR_SOLVE[N], the least squares solution.
1596 */
1597 {
1598  int ind UNUSED;
1599  int itask;
1600  int kr;
1601  int lda;
1602  float tol;
1603 
1604  float a_qr[m*n];
1605  r8mat_copy_new ( m, n, a, a_qr );
1606  lda = m;
1607  tol = r8_epsilon ( ) / r8mat_amax ( m, n, a_qr );
1608  int jpvt[n];
1609  float qraux[n];
1610  float r[m];
1611  itask = 1;
1612 
1613  ind = dqrls ( a_qr, lda, m, n, tol, &kr, b, x, r, jpvt, qraux, itask );
1614 }
1615 /******************************************************************************/
1616 
dqrsl
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)
Definition: qr_solve.c:985
b
float b
Definition: wedgebug.c:202
scale
static const float scale[]
Definition: dw1000_arduino.c:200
dqrank
void dqrank(float a[], int lda, int m, int n, float tol, int *kr, int jpvt[], float qraux[])
Definition: qr_solve.c:357
qr_solve
void qr_solve(int m, int n, float a[], float b[], float x[])
Definition: qr_solve.c:1548
UNUSED
uint8_t last_wp UNUSED
Definition: navigation.c:96
ddot
float ddot(int n, float dx[], int incx, float dy[], int incy)
Definition: qr_solve.c:144
dnrm2
float dnrm2(int n, float x[], int incx)
Definition: qr_solve.c:264
DEBUG_FPRINTF
#define DEBUG_FPRINTF(...)
Definition: qr_solve.c:18
r8_epsilon
float r8_epsilon(void)
Definition: r8lib_min.c:68
r8mat_amax
float r8mat_amax(int m, int n, float a[])
Definition: r8lib_min.c:107
std.h
r8lib_min.h
dqrdc
void dqrdc(float a[], int lda, int n, int p, float qraux[], int jpvt[], float work[], int job)
Definition: qr_solve.c:465
dqrlss
void dqrlss(float a[], int lda, int m, int n, int kr, float b[], float x[], float rsd[], int jpvt[], float qraux[])
Definition: qr_solve.c:867
dswap
void dswap(int n, float x[], int incx, float y[], int incy)
Definition: qr_solve.c:1433
daxpy
void daxpy(int n, float da, float dx[], int incx, float dy[], int incy)
Definition: qr_solve.c:23
r8_max
float r8_max(float x, float y)
Definition: r8lib_min.c:204
i4_min
int i4_min(int i1, int i2)
Definition: r8lib_min.c:474
r8mat_copy_new
void r8mat_copy_new(int m, int n, float a1[], float a2[])
Definition: r8lib_min.c:19
r8_sign
float r8_sign(float x)
Definition: r8lib_min.c:163
dqrls
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)
Definition: qr_solve.c:714
qr_solve.h
dscal
void dscal(int n, float sa, float x[], int incx)
Definition: qr_solve.c:1344
p
static float p[2][2]
Definition: ins_alt_float.c:268