|
Packit |
67cb25 |
/* linalg/cod.c
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* Copyright (C) 2016, 2017 Patrick Alken
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* This program is free software; you can redistribute it and/or modify
|
|
Packit |
67cb25 |
* it under the terms of the GNU General Public License as published by
|
|
Packit |
67cb25 |
* the Free Software Foundation; either version 3 of the License, or (at
|
|
Packit |
67cb25 |
* your option) any later version.
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* This program is distributed in the hope that it will be useful, but
|
|
Packit |
67cb25 |
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
Packit |
67cb25 |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
Packit |
67cb25 |
* General Public License for more details.
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* You should have received a copy of the GNU General Public License
|
|
Packit |
67cb25 |
* along with this program; if not, write to the Free Software
|
|
Packit |
67cb25 |
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
#include <config.h>
|
|
Packit |
67cb25 |
#include <stdlib.h>
|
|
Packit |
67cb25 |
#include <string.h>
|
|
Packit |
67cb25 |
#include <gsl/gsl_math.h>
|
|
Packit |
67cb25 |
#include <gsl/gsl_vector.h>
|
|
Packit |
67cb25 |
#include <gsl/gsl_matrix.h>
|
|
Packit |
67cb25 |
#include <gsl/gsl_permute_vector.h>
|
|
Packit |
67cb25 |
#include <gsl/gsl_blas.h>
|
|
Packit |
67cb25 |
#include <gsl/gsl_linalg.h>
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
* This module contains routines for factoring an M-by-N matrix A as:
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* A P = Q R Z^T
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* known as the Complete Orthogonal Decomposition, where:
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* P is a N-by-N permutation matrix
|
|
Packit |
67cb25 |
* Q is M-by-M orthogonal
|
|
Packit |
67cb25 |
* R has an r-by-r upper triangular block
|
|
Packit |
67cb25 |
* Z is N-by-N orthogonal
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* When A is full rank, Z = I and this becomes the QR decomposition
|
|
Packit |
67cb25 |
* with column pivoting. When A is rank deficient, then
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* R = [ R11 0 ] where R11 is r-by-r and r = rank(A)
|
|
Packit |
67cb25 |
* [ 0 0 ]
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static int cod_RZ(gsl_matrix * A, gsl_vector * tau);
|
|
Packit |
67cb25 |
static double cod_householder_transform(double *alpha, gsl_vector * v);
|
|
Packit |
67cb25 |
static int cod_householder_mh(const double tau, const gsl_vector * v,
|
|
Packit |
67cb25 |
gsl_matrix * A, gsl_vector * work);
|
|
Packit |
67cb25 |
static int cod_householder_hv(const double tau, const gsl_vector * v, gsl_vector * w);
|
|
Packit |
67cb25 |
static int cod_householder_Zvec(const gsl_matrix * QRZT, const gsl_vector * tau_Z, const size_t rank,
|
|
Packit |
67cb25 |
gsl_vector * v);
|
|
Packit |
67cb25 |
static int cod_trireg_solve(const gsl_matrix * R, const double lambda, const gsl_vector * b,
|
|
Packit |
67cb25 |
gsl_matrix * S, gsl_vector * x, gsl_vector * work);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
int
|
|
Packit |
67cb25 |
gsl_linalg_COD_decomp_e(gsl_matrix * A, gsl_vector * tau_Q, gsl_vector * tau_Z,
|
|
Packit |
67cb25 |
gsl_permutation * p, double tol, size_t * rank, gsl_vector * work)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
const size_t M = A->size1;
|
|
Packit |
67cb25 |
const size_t N = A->size2;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (tau_Q->size != GSL_MIN (M, N))
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("size of tau_Q must be MIN(M,N)", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (tau_Z->size != GSL_MIN (M, N))
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("size of tau_Z must be MIN(M,N)", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (p->size != N)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (work->size != N)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("work size must be N", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
int status, signum;
|
|
Packit |
67cb25 |
size_t r;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* decompose: A P = Q R */
|
|
Packit |
67cb25 |
status = gsl_linalg_QRPT_decomp(A, tau_Q, p, &signum, work);
|
|
Packit |
67cb25 |
if (status)
|
|
Packit |
67cb25 |
return status;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* estimate rank of A */
|
|
Packit |
67cb25 |
r = gsl_linalg_QRPT_rank(A, tol);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (r < N)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
* matrix is rank-deficient, so that the R factor is
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* R = [ R11 R12 ] =~ [ R11 R12 ]
|
|
Packit |
67cb25 |
* [ 0 R22 ] [ 0 0 ]
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* compute RZ decomposition of upper trapezoidal matrix
|
|
Packit |
67cb25 |
* [ R11 R12 ] = [ R11~ 0 ] Z
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
gsl_matrix_view R_upper = gsl_matrix_submatrix(A, 0, 0, r, N);
|
|
Packit |
67cb25 |
gsl_vector_view t = gsl_vector_subvector(tau_Z, 0, r);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
cod_RZ(&R_upper.matrix, &t.vector);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
*rank = r;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return GSL_SUCCESS;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
int
|
|
Packit |
67cb25 |
gsl_linalg_COD_decomp(gsl_matrix * A, gsl_vector * tau_Q, gsl_vector * tau_Z,
|
|
Packit |
67cb25 |
gsl_permutation * p, size_t * rank, gsl_vector * work)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
return gsl_linalg_COD_decomp_e(A, tau_Q, tau_Z, p, -1.0, rank, work);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
gsl_linalg_COD_lssolve()
|
|
Packit |
67cb25 |
Find the least squares solution to the overdetermined system
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
min ||b - A x||^2
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for M >= N using the COD factorization A P = Q R Z
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
Inputs: QRZT - matrix A, in COD compressed format, M-by-N
|
|
Packit |
67cb25 |
tau_Q - Householder scalars for Q, length min(M,N)
|
|
Packit |
67cb25 |
tau_Z - Householder scalars for Z, length min(M,N)
|
|
Packit |
67cb25 |
perm - permutation matrix
|
|
Packit |
67cb25 |
rank - rank of A
|
|
Packit |
67cb25 |
b - rhs vector, length M
|
|
Packit |
67cb25 |
x - (output) solution vector, length N
|
|
Packit |
67cb25 |
residual - (output) residual vector, b - A x, length M
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
int
|
|
Packit |
67cb25 |
gsl_linalg_COD_lssolve (const gsl_matrix * QRZT, const gsl_vector * tau_Q, const gsl_vector * tau_Z,
|
|
Packit |
67cb25 |
const gsl_permutation * perm, const size_t rank, const gsl_vector * b,
|
|
Packit |
67cb25 |
gsl_vector * x, gsl_vector * residual)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
const size_t M = QRZT->size1;
|
|
Packit |
67cb25 |
const size_t N = QRZT->size2;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (M < N)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("QRZT matrix must have M>=N", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (M != b->size)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (rank > GSL_MIN (M, N))
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("rank must be <= MIN(M,N)", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (N != x->size)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (M != residual->size)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("matrix size must match residual size", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_matrix_const_view R11 = gsl_matrix_const_submatrix (QRZT, 0, 0, rank, rank);
|
|
Packit |
67cb25 |
gsl_vector_view QTb1 = gsl_vector_subvector(residual, 0, rank);
|
|
Packit |
67cb25 |
gsl_vector_view x1 = gsl_vector_subvector(x, 0, rank);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_vector_set_zero(x);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute residual = Q^T b = [ c1 ; c2 ] */
|
|
Packit |
67cb25 |
gsl_vector_memcpy(residual, b);
|
|
Packit |
67cb25 |
gsl_linalg_QR_QTvec (QRZT, tau_Q, residual);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* solve x1 := R11^{-1} (Q^T b)(1:r) */
|
|
Packit |
67cb25 |
gsl_vector_memcpy(&(x1.vector), &(QTb1.vector));
|
|
Packit |
67cb25 |
gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, &(R11.matrix), &(x1.vector));
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute Z ( R11^{-1} x1; 0 ) */
|
|
Packit |
67cb25 |
cod_householder_Zvec(QRZT, tau_Z, rank, x);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute x = P Z^T ( R11^{-1} x1; 0 ) */
|
|
Packit |
67cb25 |
gsl_permute_vector_inverse(perm, x);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute residual = b - A x = Q (Q^T b - R [ R11^{-1} x1; 0 ]) = Q [ 0 ; c2 ] */
|
|
Packit |
67cb25 |
gsl_vector_set_zero(&(QTb1.vector));
|
|
Packit |
67cb25 |
gsl_linalg_QR_Qvec(QRZT, tau_Q, residual);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return GSL_SUCCESS;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
gsl_linalg_COD_lssolve2()
|
|
Packit |
67cb25 |
Find the least squares solution to the Tikhonov regularized
|
|
Packit |
67cb25 |
system in standard form:
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
min ||b - A x||^2 + lambda^2 ||x||^2
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for M >= N using the COD factorization A P = Q R Z
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
Inputs: lambda - parameter
|
|
Packit |
67cb25 |
QRZT - matrix A, in COD compressed format, M-by-N
|
|
Packit |
67cb25 |
tau_Q - Householder scalars for Q, length min(M,N)
|
|
Packit |
67cb25 |
tau_Z - Householder scalars for Z, length min(M,N)
|
|
Packit |
67cb25 |
perm - permutation matrix
|
|
Packit |
67cb25 |
rank - rank of A
|
|
Packit |
67cb25 |
b - rhs vector, length M
|
|
Packit |
67cb25 |
x - (output) solution vector, length N
|
|
Packit |
67cb25 |
residual - (output) residual vector, b - A x, length M
|
|
Packit |
67cb25 |
S - workspace, rank-by-rank
|
|
Packit |
67cb25 |
work - workspace, length rank
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
int
|
|
Packit |
67cb25 |
gsl_linalg_COD_lssolve2 (const double lambda, const gsl_matrix * QRZT, const gsl_vector * tau_Q, const gsl_vector * tau_Z,
|
|
Packit |
67cb25 |
const gsl_permutation * perm, const size_t rank, const gsl_vector * b,
|
|
Packit |
67cb25 |
gsl_vector * x, gsl_vector * residual, gsl_matrix * S, gsl_vector * work)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
const size_t M = QRZT->size1;
|
|
Packit |
67cb25 |
const size_t N = QRZT->size2;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (M < N)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("QRZT matrix must have M>=N", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (M != b->size)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (rank > GSL_MIN (M, N))
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("rank must be <= MIN(M,N)", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (N != x->size)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (M != residual->size)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("matrix size must match residual size", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (S->size1 != rank || S->size2 != rank)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("S must be rank-by-rank", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (work->size != rank)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("work must be length rank", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_matrix_const_view R11 = gsl_matrix_const_submatrix (QRZT, 0, 0, rank, rank);
|
|
Packit |
67cb25 |
gsl_vector_view c1 = gsl_vector_subvector(residual, 0, rank);
|
|
Packit |
67cb25 |
gsl_vector_view y1 = gsl_vector_subvector(x, 0, rank);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_vector_set_zero(x);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute residual = Q^T b = [ c1 ; c2 ]*/
|
|
Packit |
67cb25 |
gsl_vector_memcpy(residual, b);
|
|
Packit |
67cb25 |
gsl_linalg_QR_QTvec (QRZT, tau_Q, residual);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* solve [ R11 ; lambda*I ] y1 = [ (Q^T b)(1:r) ; 0 ] */
|
|
Packit |
67cb25 |
cod_trireg_solve(&(R11.matrix), lambda, &(c1.vector), S, &(y1.vector), work);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* save y1 for later residual calculation */
|
|
Packit |
67cb25 |
gsl_vector_memcpy(work, &(y1.vector));
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute Z [ y1; 0 ] */
|
|
Packit |
67cb25 |
cod_householder_Zvec(QRZT, tau_Z, rank, x);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute x = P Z^T ( y1; 0 ) */
|
|
Packit |
67cb25 |
gsl_permute_vector_inverse(perm, x);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute residual = b - A x = Q (Q^T b - [ R11 y1; 0 ]) = Q [ c1 - R11*y1 ; c2 ] */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* work = R11*y1 */
|
|
Packit |
67cb25 |
gsl_blas_dtrmv(CblasUpper, CblasNoTrans, CblasNonUnit, &(R11.matrix), work);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_vector_sub(&(c1.vector), work);
|
|
Packit |
67cb25 |
gsl_linalg_QR_Qvec(QRZT, tau_Q, residual);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return GSL_SUCCESS;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
gsl_linalg_COD_unpack()
|
|
Packit |
67cb25 |
Unpack encoded COD decomposition into the matrices Q,R,Z,P
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
Inputs: QRZT - encoded COD decomposition
|
|
Packit |
67cb25 |
tau_Q - Householder scalars for Q
|
|
Packit |
67cb25 |
tau_Z - Householder scalars for Z
|
|
Packit |
67cb25 |
rank - rank of matrix (as determined from gsl_linalg_COD_decomp)
|
|
Packit |
67cb25 |
Q - (output) M-by-M matrix Q
|
|
Packit |
67cb25 |
R - (output) M-by-N matrix R
|
|
Packit |
67cb25 |
Z - (output) N-by-N matrix Z
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
int
|
|
Packit |
67cb25 |
gsl_linalg_COD_unpack(const gsl_matrix * QRZT, const gsl_vector * tau_Q,
|
|
Packit |
67cb25 |
const gsl_vector * tau_Z, const size_t rank, gsl_matrix * Q,
|
|
Packit |
67cb25 |
gsl_matrix * R, gsl_matrix * Z)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
const size_t M = QRZT->size1;
|
|
Packit |
67cb25 |
const size_t N = QRZT->size2;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (tau_Q->size != GSL_MIN (M, N))
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("size of tau_Q must be MIN(M,N)", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (tau_Z->size != GSL_MIN (M, N))
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("size of tau_Z must be MIN(M,N)", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (rank > GSL_MIN (M, N))
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("rank must be <= MIN(M,N)", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (Q->size1 != M || Q->size2 != M)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("Q must by M-by-M", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (R->size1 != M || R->size2 != N)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("R must by M-by-N", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (Z->size1 != N || Z->size2 != N)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR ("Z must by N-by-N", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
size_t i;
|
|
Packit |
67cb25 |
gsl_matrix_view R11 = gsl_matrix_submatrix(R, 0, 0, rank, rank);
|
|
Packit |
67cb25 |
gsl_matrix_const_view QRZT11 = gsl_matrix_const_submatrix(QRZT, 0, 0, rank, rank);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* form Q matrix */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_matrix_set_identity(Q);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (i = GSL_MIN (M, N); i-- > 0;)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_vector_const_view h = gsl_matrix_const_subcolumn (QRZT, i, i, M - i);
|
|
Packit |
67cb25 |
gsl_matrix_view m = gsl_matrix_submatrix (Q, i, i, M - i, M - i);
|
|
Packit |
67cb25 |
double ti = gsl_vector_get (tau_Q, i);
|
|
Packit |
67cb25 |
gsl_linalg_householder_hm (ti, &h.vector, &m.matrix);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* form Z matrix */
|
|
Packit |
67cb25 |
gsl_matrix_set_identity(Z);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (rank < N)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_vector_view work = gsl_matrix_row(R, 0); /* temporary workspace, size N */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* multiply I by Z from the right */
|
|
Packit |
67cb25 |
gsl_linalg_COD_matZ(QRZT, tau_Z, rank, Z, &work.vector);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* copy rank-by-rank upper triangle of QRZT into R and zero the rest */
|
|
Packit |
67cb25 |
gsl_matrix_set_zero(R);
|
|
Packit |
67cb25 |
gsl_matrix_tricpy('U', 1, &R11.matrix, &QRZT11.matrix);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return GSL_SUCCESS;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
gsl_linalg_COD_matZ
|
|
Packit |
67cb25 |
Multiply an M-by-N matrix A on the right by Z (N-by-N)
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
Inputs: QRZT - encoded COD matrix
|
|
Packit |
67cb25 |
tau_Z - Householder scalars for Z
|
|
Packit |
67cb25 |
rank - matrix rank
|
|
Packit |
67cb25 |
A - on input, M-by-N matrix
|
|
Packit |
67cb25 |
on output, A * Z
|
|
Packit |
67cb25 |
work - workspace of length M
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
int
|
|
Packit |
67cb25 |
gsl_linalg_COD_matZ(const gsl_matrix * QRZT, const gsl_vector * tau_Z, const size_t rank,
|
|
Packit |
67cb25 |
gsl_matrix * A, gsl_vector * work)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
const size_t M = A->size1;
|
|
Packit |
67cb25 |
const size_t N = A->size2;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (tau_Z->size != GSL_MIN (QRZT->size1, QRZT->size2))
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR("tau_Z must be GSL_MIN(M,N)", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (QRZT->size2 != N)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR("QRZT must have N columns", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (work->size != M)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR("workspace must be length M", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
/* if rank == N, then Z = I and there is nothing to do */
|
|
Packit |
67cb25 |
if (rank < N)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
size_t i;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (i = rank; i > 0 && i--; )
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_vector_const_view h = gsl_matrix_const_subrow (QRZT, i, rank, N - rank);
|
|
Packit |
67cb25 |
gsl_matrix_view m = gsl_matrix_submatrix (A, 0, i, M, N - i);
|
|
Packit |
67cb25 |
double ti = gsl_vector_get (tau_Z, i);
|
|
Packit |
67cb25 |
cod_householder_mh (ti, &h.vector, &m.matrix, work);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return GSL_SUCCESS;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*********************************************
|
|
Packit |
67cb25 |
* INTERNAL ROUTINES *
|
|
Packit |
67cb25 |
*********************************************/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
cod_RZ()
|
|
Packit |
67cb25 |
Perform RZ decomposition of an upper trapezoidal matrix,
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
A = [ A11 A12 ] = [ R 0 ] Z
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
where A is M-by-N with N >= M, A11 is M-by-M upper triangular,
|
|
Packit |
67cb25 |
and A12 is M-by-(N-M). On output, Z is stored as Householder
|
|
Packit |
67cb25 |
reflectors in the A12 portion of A,
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
Z = Z(1) Z(2) ... Z(M)
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
Inputs: A - M-by-N matrix with N >= M
|
|
Packit |
67cb25 |
On input, upper trapezoidal matrix [ A11 A12 ]
|
|
Packit |
67cb25 |
On output, A11 is overwritten by R (subdiagonal elements
|
|
Packit |
67cb25 |
are not touched), and A12 is overwritten by Z in packed storage
|
|
Packit |
67cb25 |
tau - (output) Householder scalars, size M
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static int
|
|
Packit |
67cb25 |
cod_RZ(gsl_matrix * A, gsl_vector * tau)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
const size_t M = A->size1;
|
|
Packit |
67cb25 |
const size_t N = A->size2;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (tau->size != M)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR("tau has wrong size", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (N < M)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR("N must be >= M", GSL_EINVAL);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (M == N)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
/* quick return */
|
|
Packit |
67cb25 |
gsl_vector_set_all(tau, 0.0);
|
|
Packit |
67cb25 |
return GSL_SUCCESS;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
size_t k;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (k = M; k > 0 && k--; )
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double *alpha = gsl_matrix_ptr(A, k, k);
|
|
Packit |
67cb25 |
gsl_vector_view z = gsl_matrix_subrow(A, k, M, N - M);
|
|
Packit |
67cb25 |
double tauk;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute Householder reflection to zero [ A(k,k) A(k,M+1:N) ] */
|
|
Packit |
67cb25 |
tauk = cod_householder_transform(alpha, &z.vector);
|
|
Packit |
67cb25 |
gsl_vector_set(tau, k, tauk);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if ((tauk != 0) && (k > 0))
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_vector_view w = gsl_vector_subvector(tau, 0, k);
|
|
Packit |
67cb25 |
gsl_matrix_view B = gsl_matrix_submatrix(A, 0, k, k, N - k);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
cod_householder_mh(tauk, &z.vector, &B.matrix, &w.vector);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return GSL_SUCCESS;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static double
|
|
Packit |
67cb25 |
cod_householder_transform(double *alpha, gsl_vector * v)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double beta, tau;
|
|
Packit |
67cb25 |
double xnorm = gsl_blas_dnrm2(v);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (xnorm == 0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
return 0.0; /* tau = 0 */
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
beta = - (*alpha >= 0.0 ? +1.0 : -1.0) * gsl_hypot(*alpha, xnorm);
|
|
Packit |
67cb25 |
tau = (beta - *alpha) / beta;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double s = (*alpha - beta);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (fabs(s) > GSL_DBL_MIN)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_blas_dscal (1.0 / s, v);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_blas_dscal (GSL_DBL_EPSILON / s, v);
|
|
Packit |
67cb25 |
gsl_blas_dscal (1.0 / GSL_DBL_EPSILON, v);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
*alpha = beta;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return tau;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
cod_householder_hv
|
|
Packit |
67cb25 |
Apply Householder reflection H = (I - tau*v*v') to vector v from the left,
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
w' = H * w
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
Inputs: tau - Householder scalar
|
|
Packit |
67cb25 |
v - Householder vector, size M
|
|
Packit |
67cb25 |
w - on input, w vector, size M
|
|
Packit |
67cb25 |
on output, H * w
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
Notes:
|
|
Packit |
67cb25 |
1) Based on LAPACK routine DLARZ
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static int
|
|
Packit |
67cb25 |
cod_householder_hv(const double tau, const gsl_vector * v, gsl_vector * w)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
if (tau == 0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
return GSL_SUCCESS; /* H = I */
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
const size_t M = w->size;
|
|
Packit |
67cb25 |
const size_t L = v->size;
|
|
Packit |
67cb25 |
double w0 = gsl_vector_get(w, 0);
|
|
Packit |
67cb25 |
gsl_vector_view w1 = gsl_vector_subvector(w, M - L, L);
|
|
Packit |
67cb25 |
double d1, d;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* d1 := v . w(M-L:M) */
|
|
Packit |
67cb25 |
gsl_blas_ddot(v, &w1.vector, &d1;;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* d := w(1) + v . w(M-L:M) */
|
|
Packit |
67cb25 |
d = w0 + d1;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* w(1) = w(1) - tau * d */
|
|
Packit |
67cb25 |
gsl_vector_set(w, 0, w0 - tau * d);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* w(M-L:M) = w(M-L:M) - tau * d * v */
|
|
Packit |
67cb25 |
gsl_blas_daxpy(-tau * d, v, &w1.vector);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return GSL_SUCCESS;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
cod_householder_mh
|
|
Packit |
67cb25 |
Apply Householder reflection H = (I - tau*v*v') to matrix A from the right
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
Inputs: tau - Householder scalar
|
|
Packit |
67cb25 |
v - Householder vector, size N-M
|
|
Packit |
67cb25 |
A - matrix, size M-by-N
|
|
Packit |
67cb25 |
work - workspace, size M
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
Notes:
|
|
Packit |
67cb25 |
1) Based on LAPACK routine DLARZ
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static int
|
|
Packit |
67cb25 |
cod_householder_mh(const double tau, const gsl_vector * v, gsl_matrix * A,
|
|
Packit |
67cb25 |
gsl_vector * work)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
if (tau == 0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
return GSL_SUCCESS; /* H = I */
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
const size_t M = A->size1;
|
|
Packit |
67cb25 |
const size_t N = A->size2;
|
|
Packit |
67cb25 |
const size_t L = v->size;
|
|
Packit |
67cb25 |
gsl_vector_view A1 = gsl_matrix_subcolumn(A, 0, 0, M);
|
|
Packit |
67cb25 |
gsl_matrix_view C = gsl_matrix_submatrix(A, 0, N - L, M, L);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* work(1:M) = A(1:M,1) */
|
|
Packit |
67cb25 |
gsl_vector_memcpy(work, &A1.vector);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* work(1:M) = work(1:M) + A(1:M,M+1:N) * v(1:N-M) */
|
|
Packit |
67cb25 |
gsl_blas_dgemv(CblasNoTrans, 1.0, &C.matrix, v, 1.0, work);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* A(1:M,1) = A(1:M,1) - tau * work(1:M) */
|
|
Packit |
67cb25 |
gsl_blas_daxpy(-tau, work, &A1.vector);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* A(1:M,M+1:N) = A(1:M,M+1:N) - tau * work(1:M) * v(1:N-M)' */
|
|
Packit |
67cb25 |
gsl_blas_dger(-tau, work, v, &C.matrix);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return GSL_SUCCESS;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
cod_householder_Zvec
|
|
Packit |
67cb25 |
Multiply a vector by Z
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
Inputs: QRZT - encoded COD matrix
|
|
Packit |
67cb25 |
tau_Z - Householder scalars for Z
|
|
Packit |
67cb25 |
rank - matrix rank
|
|
Packit |
67cb25 |
v - on input, vector of length N
|
|
Packit |
67cb25 |
on output, Z^T * v
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static int
|
|
Packit |
67cb25 |
cod_householder_Zvec(const gsl_matrix * QRZT, const gsl_vector * tau_Z, const size_t rank,
|
|
Packit |
67cb25 |
gsl_vector * v)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
const size_t M = QRZT->size1;
|
|
Packit |
67cb25 |
const size_t N = QRZT->size2;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (tau_Z->size != GSL_MIN (M, N))
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR("tau_Z must be GSL_MIN(M,N)", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (v->size != N)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR("v must be length N", GSL_EBADLEN);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
if (rank < N)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
size_t i;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (i = 0; i < rank; ++i)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_vector_const_view h = gsl_matrix_const_subrow (QRZT, i, rank, N - rank);
|
|
Packit |
67cb25 |
gsl_vector_view w = gsl_vector_subvector (v, i, N - i);
|
|
Packit |
67cb25 |
double ti = gsl_vector_get (tau_Z, i);
|
|
Packit |
67cb25 |
cod_householder_hv(ti, &h.vector, &w.vector);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return GSL_SUCCESS;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
cod_trireg_solve()
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
This function computes the solution to the least squares system
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
[ R ] x = [ b ]
|
|
Packit |
67cb25 |
[ lambda*I ] [ 0 ]
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
where R is an N-by-N upper triangular matrix, lambda is a scalar parameter,
|
|
Packit |
67cb25 |
and b is a vector of length N. This is done by computing the QR factorization
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
[ R ] = W S^T
|
|
Packit |
67cb25 |
[ lambda*I ]
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
where S^T is upper triangular, and solving
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
S^T x = W^T [ b ]
|
|
Packit |
67cb25 |
[ 0 ]
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
Inputs: R - full rank upper triangular matrix; the diagonal
|
|
Packit |
67cb25 |
elements are modified but restored on output
|
|
Packit |
67cb25 |
lambda - scalar parameter lambda
|
|
Packit |
67cb25 |
b - right hand side vector b
|
|
Packit |
67cb25 |
S - workspace, N-by-N
|
|
Packit |
67cb25 |
x - (output) least squares solution of the system
|
|
Packit |
67cb25 |
work - workspace of length N
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static int
|
|
Packit |
67cb25 |
cod_trireg_solve (const gsl_matrix * R, const double lambda, const gsl_vector * b,
|
|
Packit |
67cb25 |
gsl_matrix * S, gsl_vector * x, gsl_vector * work)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
const size_t N = R->size2;
|
|
Packit |
67cb25 |
gsl_vector_const_view diag = gsl_matrix_const_diagonal(R);
|
|
Packit |
67cb25 |
size_t i, j, k;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (lambda <= 0.0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR("lambda must be positive", GSL_EINVAL);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* copy R and b to preserve input and initialise S; store diag(R) in work */
|
|
Packit |
67cb25 |
gsl_matrix_transpose_tricpy('U', 0, S, R);
|
|
Packit |
67cb25 |
gsl_vector_memcpy(work, &diag.vector);
|
|
Packit |
67cb25 |
gsl_vector_memcpy(x, b);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* eliminate the diagonal matrix lambda*I using Givens rotations */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (j = 0; j < N; j++)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double bj = 0.0;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_matrix_set (S, j, j, lambda);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (k = j + 1; k < N; k++)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_matrix_set (S, k, k, 0.0);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* the transformations to eliminate the row of lambda*I modify only a
|
|
Packit |
67cb25 |
single element of b beyond the first n, which is initially
|
|
Packit |
67cb25 |
zero */
|
|
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 lambda*I */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double sine, cosine;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double xk = gsl_vector_get (x, k);
|
|
Packit |
67cb25 |
double rkk = gsl_vector_get (work, k);
|
|
Packit |
67cb25 |
double skk = gsl_matrix_get (S, k, k);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (skk == 0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
continue;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (fabs (rkk) < fabs (skk))
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double cotangent = rkk / skk;
|
|
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 = skk / 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 [b,0] */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double new_rkk = cosine * rkk + sine * skk;
|
|
Packit |
67cb25 |
double new_xk = cosine * xk + sine * bj;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
bj = -sine * xk + cosine * bj;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_vector_set(work, k, new_rkk);
|
|
Packit |
67cb25 |
gsl_matrix_set(S, k, k, new_rkk);
|
|
Packit |
67cb25 |
gsl_vector_set(x, k, new_xk);
|
|
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 sik = gsl_matrix_get (S, i, k);
|
|
Packit |
67cb25 |
double sii = gsl_matrix_get (S, i, i);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double new_sik = cosine * sik + sine * sii;
|
|
Packit |
67cb25 |
double new_sii = -sine * sik + cosine * sii;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_matrix_set(S, i, k, new_sik);
|
|
Packit |
67cb25 |
gsl_matrix_set(S, i, i, new_sii);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* solve: S^T x = rhs in place */
|
|
Packit |
67cb25 |
gsl_blas_dtrsv(CblasLower, CblasTrans, CblasNonUnit, S, x);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return GSL_SUCCESS;
|
|
Packit |
67cb25 |
}
|