123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219 |
- /* This function computes the solution to the least squares system
- phi = [ A x = b , lambda D x = 0 ]^2
-
- where A is an M by N matrix, D is an N by N diagonal matrix, lambda
- is a scalar parameter and b is a vector of length M.
- The function requires the factorization of A into A = Q R P^T,
- where Q is an orthogonal matrix, R is an upper triangular matrix
- with diagonal elements of non-increasing magnitude and P is a
- permuation matrix. The system above is then equivalent to
- [ R z = Q^T b, P^T (lambda D) P z = 0 ]
- where x = P z. If this system does not have full rank then a least
- squares solution is obtained. On output the function also provides
- an upper triangular matrix S such that
- P^T (A^T A + lambda^2 D^T D) P = S^T S
- Parameters,
-
- r: On input, contains the full upper triangle of R. On output the
- strict lower triangle contains the transpose of the strict upper
- triangle of S, and the diagonal of S is stored in sdiag. The full
- upper triangle of R is not modified.
- p: the encoded form of the permutation matrix P. column j of P is
- column p[j] of the identity matrix.
- lambda, diag: contains the scalar lambda and the diagonal elements
- of the matrix D
- qtb: contains the product Q^T b
- x: on output contains the least squares solution of the system
- wa: is a workspace of length N
- */
- static int
- qrsolv (gsl_matrix * r, const gsl_permutation * p, const double lambda,
- const gsl_vector * diag, const gsl_vector * qtb,
- gsl_vector * x, gsl_vector * sdiag, gsl_vector * wa)
- {
- size_t n = r->size2;
- size_t i, j, k, nsing;
- /* Copy r and qtb to preserve input and initialise s. In particular,
- save the diagonal elements of r in x */
- for (j = 0; j < n; j++)
- {
- double rjj = gsl_matrix_get (r, j, j);
- double qtbj = gsl_vector_get (qtb, j);
- for (i = j + 1; i < n; i++)
- {
- double rji = gsl_matrix_get (r, j, i);
- gsl_matrix_set (r, i, j, rji);
- }
- gsl_vector_set (x, j, rjj);
- gsl_vector_set (wa, j, qtbj);
- }
- /* Eliminate the diagonal matrix d using a Givens rotation */
- for (j = 0; j < n; j++)
- {
- double qtbpj;
- size_t pj = gsl_permutation_get (p, j);
- double diagpj = lambda * gsl_vector_get (diag, pj);
- if (diagpj == 0)
- {
- continue;
- }
- gsl_vector_set (sdiag, j, diagpj);
- for (k = j + 1; k < n; k++)
- {
- gsl_vector_set (sdiag, k, 0.0);
- }
- /* The transformations to eliminate the row of d modify only a
- single element of qtb beyond the first n, which is initially
- zero */
- qtbpj = 0;
- for (k = j; k < n; k++)
- {
- /* Determine a Givens rotation which eliminates the
- appropriate element in the current row of d */
- double sine, cosine;
- double wak = gsl_vector_get (wa, k);
- double rkk = gsl_matrix_get (r, k, k);
- double sdiagk = gsl_vector_get (sdiag, k);
- if (sdiagk == 0)
- {
- continue;
- }
- if (fabs (rkk) < fabs (sdiagk))
- {
- double cotangent = rkk / sdiagk;
- sine = 0.5 / sqrt (0.25 + 0.25 * cotangent * cotangent);
- cosine = sine * cotangent;
- }
- else
- {
- double tangent = sdiagk / rkk;
- cosine = 0.5 / sqrt (0.25 + 0.25 * tangent * tangent);
- sine = cosine * tangent;
- }
- /* Compute the modified diagonal element of r and the
- modified element of [qtb,0] */
- {
- double new_rkk = cosine * rkk + sine * sdiagk;
- double new_wak = cosine * wak + sine * qtbpj;
-
- qtbpj = -sine * wak + cosine * qtbpj;
- gsl_matrix_set(r, k, k, new_rkk);
- gsl_vector_set(wa, k, new_wak);
- }
- /* Accumulate the transformation in the row of s */
- for (i = k + 1; i < n; i++)
- {
- double rik = gsl_matrix_get (r, i, k);
- double sdiagi = gsl_vector_get (sdiag, i);
-
- double new_rik = cosine * rik + sine * sdiagi;
- double new_sdiagi = -sine * rik + cosine * sdiagi;
-
- gsl_matrix_set(r, i, k, new_rik);
- gsl_vector_set(sdiag, i, new_sdiagi);
- }
- }
- /* Store the corresponding diagonal element of s and restore the
- corresponding diagonal element of r */
- {
- double rjj = gsl_matrix_get (r, j, j);
- double xj = gsl_vector_get(x, j);
-
- gsl_vector_set (sdiag, j, rjj);
- gsl_matrix_set (r, j, j, xj);
- }
- }
- /* Solve the triangular system for z. If the system is singular then
- obtain a least squares solution */
- nsing = n;
- for (j = 0; j < n; j++)
- {
- double sdiagj = gsl_vector_get (sdiag, j);
- if (sdiagj == 0)
- {
- nsing = j;
- break;
- }
- }
- for (j = nsing; j < n; j++)
- {
- gsl_vector_set (wa, j, 0.0);
- }
- for (k = 0; k < nsing; k++)
- {
- double sum = 0;
- j = (nsing - 1) - k;
- for (i = j + 1; i < nsing; i++)
- {
- sum += gsl_matrix_get(r, i, j) * gsl_vector_get(wa, i);
- }
- {
- double waj = gsl_vector_get (wa, j);
- double sdiagj = gsl_vector_get (sdiag, j);
- gsl_vector_set (wa, j, (waj - sum) / sdiagj);
- }
- }
- /* Permute the components of z back to the components of x */
- for (j = 0; j < n; j++)
- {
- size_t pj = gsl_permutation_get (p, j);
- double waj = gsl_vector_get (wa, j);
- gsl_vector_set (x, pj, waj);
- }
- return GSL_SUCCESS;
- }
|