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