Paparazzi UAS  v5.15_devel-230-gc96ce27 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  *
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
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
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
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
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
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,
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
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
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
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
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,
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
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
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
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;
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];
1612
1613  ind = dqrls ( a_qr, lda, m, n, tol, &kr, b, x, r, jpvt, qraux, itask );
1614 }
1615 /******************************************************************************/
1616
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
void dqrdc(float a[], int lda, int n, int p, float qraux[], int jpvt[], float work[], int job)
Definition: qr_solve.c:465
#define DEBUG_FPRINTF(...)
Definition: qr_solve.c:18
void r8mat_copy_new(int m, int n, float a1[], float a2[])
Definition: r8lib_min.c:19
void dscal(int n, float sa, float x[], int incx)
Definition: qr_solve.c:1344
float ddot(int n, float dx[], int incx, float dy[], int incy)
Definition: qr_solve.c:144
float dnrm2(int n, float x[], int incx)
Definition: qr_solve.c:264
uint8_t last_wp UNUSED
void dqrank(float a[], int lda, int m, int n, float tol, int *kr, int jpvt[], float qraux[])
Definition: qr_solve.c:357
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
float r8_sign(float x)
Definition: r8lib_min.c:163
float r8_epsilon(void)
Definition: r8lib_min.c:68
void daxpy(int n, float da, float dx[], int incx, float dy[], int incy)
Definition: qr_solve.c:23
float r8mat_amax(int m, int n, float a[])
Definition: r8lib_min.c:107
void qr_solve(int m, int n, float a[], float b[], float x[])
Definition: qr_solve.c:1548
static const float scale[]
float r8_max(float x, float y)
Definition: r8lib_min.c:204
static float p[2][2]
void dswap(int n, float x[], int incx, float y[], int incy)
Definition: qr_solve.c:1433
int i4_min(int i1, int i2)
Definition: r8lib_min.c:474
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