diff options
Diffstat (limited to 'gsl-1.9/multifit/qrsolv.c')
| -rw-r--r-- | gsl-1.9/multifit/qrsolv.c | 218 |
1 files changed, 218 insertions, 0 deletions
diff --git a/gsl-1.9/multifit/qrsolv.c b/gsl-1.9/multifit/qrsolv.c new file mode 100644 index 0000000..06f0399 --- /dev/null +++ b/gsl-1.9/multifit/qrsolv.c @@ -0,0 +1,218 @@ +/* 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; +} |