htswanalysis/MACS/lib/gsl/gsl-1.11/multifit/qrsolv.c

   1 /* This function computes the solution to the least squares system
   2
   3    phi = [ A x =  b , lambda D x = 0 ]^2
   4
   5    where A is an M by N matrix, D is an N by N diagonal matrix, lambda
   6    is a scalar parameter and b is a vector of length M.
   7
   8    The function requires the factorization of A into A = Q R P^T,
   9    where Q is an orthogonal matrix, R is an upper triangular matrix
  10    with diagonal elements of non-increasing magnitude and P is a
  11    permuation matrix. The system above is then equivalent to
  12
  13    [ R z = Q^T b, P^T (lambda D) P z = 0 ]
  14
  15    where x = P z. If this system does not have full rank then a least
  16    squares solution is obtained.  On output the function also provides
  17    an upper triangular matrix S such that
  18
  19    P^T (A^T A + lambda^2 D^T D) P = S^T S
  20
  21    Parameters,
  22
  23    r: On input, contains the full upper triangle of R. On output the
  24    strict lower triangle contains the transpose of the strict upper
  25    triangle of S, and the diagonal of S is stored in sdiag.  The full
  26    upper triangle of R is not modified.
  27
  28    p: the encoded form of the permutation matrix P. column j of P is
  29    column p[j] of the identity matrix.
  30
  31    lambda, diag: contains the scalar lambda and the diagonal elements
  32    of the matrix D
  33
  34    qtb: contains the product Q^T b
  35
  36    x: on output contains the least squares solution of the system
  37
  38    wa: is a workspace of length N
  39
  40    */
  41
  42 static int
  43 qrsolv (gsl_matrix * r, const gsl_permutation * p, const double lambda,
  44         const gsl_vector * diag, const gsl_vector * qtb,
  45         gsl_vector * x, gsl_vector * sdiag, gsl_vector * wa)
  46 {
  47   size_t n = r->size2;
  48
  49   size_t i, j, k, nsing;
  50
  51   /* Copy r and qtb to preserve input and initialise s. In particular,
  52      save the diagonal elements of r in x */
  53
  54   for (j = 0; j < n; j++)
  55     {
  56       double rjj = gsl_matrix_get (r, j, j);
  57       double qtbj = gsl_vector_get (qtb, j);
  58
  59       for (i = j + 1; i < n; i++)
  60         {
  61           double rji = gsl_matrix_get (r, j, i);
  62           gsl_matrix_set (r, i, j, rji);
  63         }
  64
  65       gsl_vector_set (x, j, rjj);
  66       gsl_vector_set (wa, j, qtbj);
  67     }
  68
  69   /* Eliminate the diagonal matrix d using a Givens rotation */
  70
  71   for (j = 0; j < n; j++)
  72     {
  73       double qtbpj;
  74
  75       size_t pj = gsl_permutation_get (p, j);
  76
  77       double diagpj = lambda * gsl_vector_get (diag, pj);
  78
  79       if (diagpj == 0)
  80         {
  81           continue;
  82         }
  83
  84       gsl_vector_set (sdiag, j, diagpj);
  85
  86       for (k = j + 1; k < n; k++)
  87         {
  88           gsl_vector_set (sdiag, k, 0.0);
  89         }
  90
  91       /* The transformations to eliminate the row of d modify only a
  92          single element of qtb beyond the first n, which is initially
  93          zero */
  94
  95       qtbpj = 0;
  96
  97       for (k = j; k < n; k++)
  98         {
  99           /* Determine a Givens rotation which eliminates the
 100              appropriate element in the current row of d */
 101
 102           double sine, cosine;
 103
 104           double wak = gsl_vector_get (wa, k);
 105           double rkk = gsl_matrix_get (r, k, k);
 106           double sdiagk = gsl_vector_get (sdiag, k);
 107
 108           if (sdiagk == 0)
 109             {
 110               continue;
 111             }
 112
 113           if (fabs (rkk) < fabs (sdiagk))
 114             {
 115               double cotangent = rkk / sdiagk;
 116               sine = 0.5 / sqrt (0.25 + 0.25 * cotangent * cotangent);
 117               cosine = sine * cotangent;
 118             }
 119           else
 120             {
 121               double tangent = sdiagk / rkk;
 122               cosine = 0.5 / sqrt (0.25 + 0.25 * tangent * tangent);
 123               sine = cosine * tangent;
 124             }
 125
 126           /* Compute the modified diagonal element of r and the
 127              modified element of [qtb,0] */
 128
 129           {
 130             double new_rkk = cosine * rkk + sine * sdiagk;
 131             double new_wak = cosine * wak + sine * qtbpj;
 132
 133             qtbpj = -sine * wak + cosine * qtbpj;
 134
 135             gsl_matrix_set(r, k, k, new_rkk);
 136             gsl_vector_set(wa, k, new_wak);
 137           }
 138
 139           /* Accumulate the transformation in the row of s */
 140
 141           for (i = k + 1; i < n; i++)
 142             {
 143               double rik = gsl_matrix_get (r, i, k);
 144               double sdiagi = gsl_vector_get (sdiag, i);
 145
 146               double new_rik = cosine * rik + sine * sdiagi;
 147               double new_sdiagi = -sine * rik + cosine * sdiagi;
 148
 149               gsl_matrix_set(r, i, k, new_rik);
 150               gsl_vector_set(sdiag, i, new_sdiagi);
 151             }
 152         }
 153
 154       /* Store the corresponding diagonal element of s and restore the
 155          corresponding diagonal element of r */
 156
 157       {
 158         double rjj = gsl_matrix_get (r, j, j);
 159         double xj = gsl_vector_get(x, j);
 160
 161         gsl_vector_set (sdiag, j, rjj);
 162         gsl_matrix_set (r, j, j, xj);
 163       }
 164
 165     }
 166
 167   /* Solve the triangular system for z. If the system is singular then
 168      obtain a least squares solution */
 169
 170   nsing = n;
 171
 172   for (j = 0; j < n; j++)
 173     {
 174       double sdiagj = gsl_vector_get (sdiag, j);
 175
 176       if (sdiagj == 0)
 177         {
 178           nsing = j;
 179           break;
 180         }
 181     }
 182
 183   for (j = nsing; j < n; j++)
 184     {
 185       gsl_vector_set (wa, j, 0.0);
 186     }
 187
 188   for (k = 0; k < nsing; k++)
 189     {
 190       double sum = 0;
 191
 192       j = (nsing - 1) - k;
 193
 194       for (i = j + 1; i < nsing; i++)
 195         {
 196           sum += gsl_matrix_get(r, i, j) * gsl_vector_get(wa, i);
 197         }
 198
 199       {
 200         double waj = gsl_vector_get (wa, j);
 201         double sdiagj = gsl_vector_get (sdiag, j);
 202
 203         gsl_vector_set (wa, j, (waj - sum) / sdiagj);
 204       }
 205     }
 206
 207   /* Permute the components of z back to the components of x */
 208
 209   for (j = 0; j < n; j++)
 210     {
 211       size_t pj = gsl_permutation_get (p, j);
 212       double waj = gsl_vector_get (wa, j);
 213
 214       gsl_vector_set (x, pj, waj);
 215     }
 216
 217   return GSL_SUCCESS;
 218 }