void QR_update (Matrix& Q, Matrix& R,
VectorT& w, const VectorT& v)
{
Assert(Q.m == R.m && Q.n == R.m);
Assert(w.n == R.m);
Assert(v.n == R.n);
int j, k;
T w0;
/* Apply Given's rotations to reduce w to (|w|, 0, 0, ... , 0)
J_1^T .... J_(n-1)^T w = +/- |w| e_1
simultaneously applied to R, H = J_1^T ... J^T_(n-1) R
so that H is upper Hessenberg. (12.5.2) */
for (k = R.m - 1; k > 0; k--) {
double c, s;
double wk = w(k);
double wkm1 = w(k-1);
create_givens (wkm1, wk, &c, &s);
apply_givens_vec (w, k - 1, k, c, s);
apply_givens_qr (R.m, R.n, Q, R, k - 1, k, c, s);
}
w0 = w(0);
/* Add in w v^T (Equation 12.5.3) */
for (j = 0; j < R.n; j++)
R(0,j) += w0 * v(j);
/* Apply Givens transformations R' = G_(n-1)^T ... G_1^T H
Equation 12.5.4 */
for (k=1; k<Min(R.m,R.n+1); k++) {
double c, s;
double diag = R(k-1,k-1);
double offdiag = R(k,k-1);
create_givens (diag, offdiag, &c, &s);
apply_givens_qr (R.m, R.n, Q, R, k - 1, k, c, s);
R(k,k-1)=0;
}
}
void chase_out_intermediate_zero (gsl_vector * d, gsl_vector * f,
gsl_matrix * U, size_t k0) {
const size_t M=U->size1;
const size_t n=d->size;
double c, s;
double x, y;
size_t k;
x=gsl_vector_get (f, k0);
y=gsl_vector_get (d, k0+1);
for (k=k0; k < n - 1; k++)
{
create_givens (y, -x, &c, &s);
/* Compute U <= U G */
{
size_t i;
for (i=0; i < M; i++)
{
double Uip=gsl_matrix_get (U, i, k0);
double Uiq=gsl_matrix_get (U, i, k + 1);
//std::cout << "Uip,Uiq: " << Uip << " " << Uiq << std::endl;
gsl_matrix_set (U, i, k0, c * Uip - s * Uiq);
gsl_matrix_set (U, i, k + 1, s * Uip + c * Uiq);
}
}
/* compute B <= G^T B */
gsl_vector_set (d, k + 1, s * x + c * y);
if (k == k0)
gsl_vector_set (f, k, c * x - s * y );
if (k < n - 2)
{
double z=gsl_vector_get (f, k + 1);
gsl_vector_set (f, k + 1, c * z);
x=-s * z ;
y=gsl_vector_get (d, k + 2);
}
}
}
void chase_out_trailing_zero (gsl_vector * d, gsl_vector * f,
gsl_matrix * V) {
const size_t N=V->size1;
const size_t n=d->size;
double c, s;
double x, y;
size_t k;
x=gsl_vector_get (d, n - 2);
y=gsl_vector_get (f, n - 2);
for (k=n - 1; k-- > 0;)
{
create_givens (x, y, &c, &s);
/* Compute V <= V G where G=[c, s ; -s, c] */
{
size_t i;
for (i=0; i < N; i++)
{
double Vip=gsl_matrix_get (V, i, k);
double Viq=gsl_matrix_get (V, i, n - 1);
gsl_matrix_set (V, i, k, c * Vip - s * Viq);
gsl_matrix_set (V, i, n - 1, s * Vip + c * Viq);
}
}
/* compute B <= B G */
gsl_vector_set (d, k, c * x - s * y);
if (k == n - 2)
gsl_vector_set (f, k, s * x + c * y );
if (k > 0)
{
double z=gsl_vector_get (f, k - 1);
gsl_vector_set (f, k - 1, c * z);
x=gsl_vector_get (d, k - 1);
y=s * z ;
}
}
}
int
gsl_linalg_PTLQ_update (gsl_matrix * Q, gsl_matrix * L,
const gsl_permutation * p,
const gsl_vector * v, gsl_vector * w)
{
if (Q->size1 != Q->size2 || L->size1 != L->size2)
{
return GSL_ENOTSQR;
}
else if (L->size1 != Q->size2 || v->size != Q->size2 || w->size != Q->size2)
{
return GSL_EBADLEN;
}
else
{
size_t j, k;
const size_t N = Q->size1;
const size_t M = Q->size2;
double w0;
/* Apply Given's rotations to reduce w to (|w|, 0, 0, ... , 0)
J_1^T .... J_(n-1)^T w = +/- |w| e_1
simultaneously applied to L, H = J_1^T ... J^T_(n-1) L
so that H is upper Hessenberg. (12.5.2) */
for (k = M - 1; k > 0; k--)
{
double c, s;
double wk = gsl_vector_get (w, k);
double wkm1 = gsl_vector_get (w, k - 1);
create_givens (wkm1, wk, &c, &s);
apply_givens_vec (w, k - 1, k, c, s);
apply_givens_lq (M, N, Q, L, k - 1, k, c, s);
}
w0 = gsl_vector_get (w, 0);
/* Add in v w^T (Equation 12.5.3) */
for (j = 0; j < N; j++)
{
double lj0 = gsl_matrix_get (L, j, 0);
size_t p_j = gsl_permutation_get (p, j);
double vj = gsl_vector_get (v, p_j);
gsl_matrix_set (L, j, 0, lj0 + w0 * vj);
}
/* Apply Givens transformations L' = G_(n-1)^T ... G_1^T H
Equation 12.5.4 */
for (k = 1; k < N; k++)
{
double c, s;
double diag = gsl_matrix_get (L, k - 1, k - 1);
double offdiag = gsl_matrix_get (L, k - 1, k );
create_givens (diag, offdiag, &c, &s);
apply_givens_lq (M, N, Q, L, k - 1, k, c, s);
}
return GSL_SUCCESS;
}
}
void qrstep (gsl_vector * d, gsl_vector * f, gsl_matrix * U,
gsl_matrix * V) {
const size_t M=U->size1;
const size_t N=V->size1;
const size_t n=d->size;
double y, z;
double ak, bk, zk, ap, bp, aq, bq;
size_t i, k;
//std::cout << "M,N,n: " << M << " " << N << " " << n << std::endl;
if (n == 1)
return; /* shouldn't happen */
/* Compute 2x2 svd directly */
if (n == 2)
{
svd2 (d, f, U, V);
return;
}
/* Chase out any zeroes on the diagonal */
for (i=0; i < n - 1; i++)
{
double d_i=gsl_vector_get (d, i);
//std::cout << "d_i: " << i << " " << n << " "
//<< d_i << std::endl;
if (d_i == 0.0)
{
chase_out_intermediate_zero (d, f, U, i);
return;
}
}
/* Chase out any zero at the end of the diagonal */
{
double d_nm1=gsl_vector_get (d, n - 1);
//std::cout << "d_nm1: " << d_nm1 << std::endl;
if (d_nm1 == 0.0)
{
chase_out_trailing_zero (d, f, V);
return;
}
}
/* Apply QR reduction steps to the diagonal and offdiagonal */
{
double d0=gsl_vector_get (d, 0);
double f0=gsl_vector_get (f, 0);
double d1=gsl_vector_get (d, 1);
double f1=gsl_vector_get (f, 1);
//std::cout << "d0,f0,d1,f1: " << d0 << " " << f0 << " " << d1 << " "
//<< f1 << std::endl;
{
double mu=trailing_eigenvalue (d, f);
y=d0 * d0 - mu;
z=d0 * f0;
}
/* Set up the recurrence for Givens rotations on a bidiagonal matrix */
ak=0;
bk=0;
ap=d0;
bp=f0;
aq=d1;
bq=f1;
}
for (k=0; k < n - 1; k++)
{
double c, s;
create_givens (y, z, &c, &s);
/* Compute V <= V G */
for (i=0; i < N; i++)
{
double Vip=gsl_matrix_get (V, i, k);
double Viq=gsl_matrix_get (V, i, k + 1);
//std::cout << "Vip,Viq: " << Vip << " " << Viq << std::endl;
gsl_matrix_set (V, i, k, c * Vip - s * Viq);
gsl_matrix_set (V, i, k + 1, s * Vip + c * Viq);
}
/* compute B <= B G */
{
double bk1=c * bk - s * z;
double ap1=c * ap - s * bp;
double bp1=s * ap + c * bp;
double zp1=-s * aq;
double aq1=c * aq;
if (k > 0)
{
gsl_vector_set (f, k - 1, bk1);
}
ak=ap1;
bk=bp1;
zk=zp1;
ap=aq1;
if (k < n - 2)
{
bp=gsl_vector_get (f, k + 1);
}
else
{
bp=0.0;
}
y=ak;
z=zk;
}
create_givens (y, z, &c, &s);
/* Compute U <= U G */
for (i=0; i < M; i++)
{
double Uip=gsl_matrix_get (U, i, k);
double Uiq=gsl_matrix_get (U, i, k + 1);
//std::cout << "Uip2,Uiq2: " << Uip << " " << Uiq << std::endl;
gsl_matrix_set (U, i, k, c * Uip - s * Uiq);
gsl_matrix_set (U, i, k + 1, s * Uip + c * Uiq);
}
/* compute B <= G^T B */
//std::cout << "k,bk,ap2: " << k << " " << bk << " " << ap << std::endl;
//std::cout << "ak,zk,bp: " << ak << " " << zk << " "
// << bp << std::endl;
{
//std::cout << "prod1: " << c*ak << " " << s*zk << std::endl;
//std::cout << "prod2: " << c*bk << " " << s*ap << std::endl;
//std::cout << "prod3: " << s*bk << " " << c*ap << std::endl;
double ak1=c * ak - s * zk;
double bk1=c * bk - s * ap;
double zk1=-s * bp;
double ap1=s * bk + c * ap;
double bp1=c * bp;
gsl_vector_set (d, k, ak1);
ak=ak1;
bk=bk1;
zk=zk1;
ap=ap1;
bp=bp1;
//std::cout << "c,s: " << c << " " << s << std::endl;
//std::cout << "k,bk,ap: " << k << " " << bk << " " << ap << std::endl;
if (k < n - 2)
{
aq=gsl_vector_get (d, k + 2);
}
else
{
aq=0.0;
}
y=bk;
z=zk;
}
}
gsl_vector_set (f, n - 2, bk);
gsl_vector_set (d, n - 1, ap);
//std::cout << "bk,ap: " << bk << " " << ap << std::endl;
}
void svd2 (gsl_vector * d, gsl_vector * f, gsl_matrix * U,
gsl_matrix * V) {
size_t i;
double c, s, a11, a12, a21, a22;
const size_t M=U->size1;
const size_t N=V->size1;
double d0=gsl_vector_get (d, 0);
double f0=gsl_vector_get (f, 0);
double d1=gsl_vector_get (d, 1);
if (d0 == 0.0)
{
/* Eliminate off-diagonal element in [0,f0;0,d1] to make [d,0;0,0] */
create_givens (f0, d1, &c, &s);
/* compute B <= G^T B X, where X=[0,1;1,0] */
gsl_vector_set (d, 0, c * f0 - s * d1);
gsl_vector_set (f, 0, s * f0 + c * d1);
gsl_vector_set (d, 1, 0.0);
/* Compute U <= U G */
for (i=0; i < M; i++)
{
double Uip=gsl_matrix_get (U, i, 0);
double Uiq=gsl_matrix_get (U, i, 1);
gsl_matrix_set (U, i, 0, c * Uip - s * Uiq);
gsl_matrix_set (U, i, 1, s * Uip + c * Uiq);
}
/* Compute V <= V X */
gsl_matrix_swap_columns (V, 0, 1);
return;
}
else if (d1 == 0.0)
{
/* Eliminate off-diagonal element in [d0,f0;0,0] */
create_givens (d0, f0, &c, &s);
/* compute B <= B G */
gsl_vector_set (d, 0, d0 * c - f0 * s);
gsl_vector_set (f, 0, 0.0);
/* Compute V <= V G */
for (i=0; i < N; i++)
{
double Vip=gsl_matrix_get (V, i, 0);
double Viq=gsl_matrix_get (V, i, 1);
gsl_matrix_set (V, i, 0, c * Vip - s * Viq);
gsl_matrix_set (V, i, 1, s * Vip + c * Viq);
}
return;
}
else
{
/* Make columns orthogonal, A=[d0, f0; 0, d1] * G */
create_schur (d0, f0, d1, &c, &s);
/* compute B <= B G */
a11=c * d0 - s * f0;
a21=- s * d1;
a12=s * d0 + c * f0;
a22=c * d1;
/* Compute V <= V G */
for (i=0; i < N; i++)
{
double Vip=gsl_matrix_get (V, i, 0);
double Viq=gsl_matrix_get (V, i, 1);
gsl_matrix_set (V, i, 0, c * Vip - s * Viq);
gsl_matrix_set (V, i, 1, s * Vip + c * Viq);
}
/* Eliminate off-diagonal elements, bring column with largest
norm to first column */
if (hypot(a11, a21) < hypot(a12,a22))
{
double t1, t2;
/* B <= B X */
t1=a11; a11=a12; a12=t1;
t2=a21; a21=a22; a22=t2;
/* V <= V X */
gsl_matrix_swap_columns(V, 0, 1);
}
create_givens (a11, a21, &c, &s);
/* compute B <= G^T B */
gsl_vector_set (d, 0, c * a11 - s * a21);
gsl_vector_set (f, 0, c * a12 - s * a22);
gsl_vector_set (d, 1, s * a12 + c * a22);
/* Compute U <= U G */
for (i=0; i < M; i++)
{
double Uip=gsl_matrix_get (U, i, 0);
double Uiq=gsl_matrix_get (U, i, 1);
gsl_matrix_set (U, i, 0, c * Uip - s * Uiq);
gsl_matrix_set (U, i, 1, s * Uip + c * Uiq);
}
return;
}
}
int
gsl_linalg_LQ_update (gsl_matrix * Q, gsl_matrix * L,
const gsl_vector * v, gsl_vector * w)
{
const size_t N = L->size1;
const size_t M = L->size2;
if (Q->size1 != M || Q->size2 != M)
{
GSL_ERROR ("Q matrix must be N x N if L is M x N", GSL_ENOTSQR);
}
else if (w->size != M)
{
GSL_ERROR ("w must be length N if L is M x N", GSL_EBADLEN);
}
else if (v->size != N)
{
GSL_ERROR ("v must be length M if L is M x N", GSL_EBADLEN);
}
else
{
size_t j, k;
double w0;
/* Apply Given's rotations to reduce w to (|w|, 0, 0, ... , 0)
J_1^T .... J_(n-1)^T w = +/- |w| e_1
simultaneously applied to L, H = J_1^T ... J^T_(n-1) L
so that H is upper Hessenberg. (12.5.2) */
for (k = M - 1; k > 0; k--) /* loop from k = M-1 to 1 */
{
double c, s;
double wk = gsl_vector_get (w, k);
double wkm1 = gsl_vector_get (w, k - 1);
create_givens (wkm1, wk, &c, &s);
apply_givens_vec (w, k - 1, k, c, s);
apply_givens_lq (M, N, Q, L, k - 1, k, c, s);
}
w0 = gsl_vector_get (w, 0);
/* Add in v w^T (Equation 12.5.3) */
for (j = 0; j < N; j++)
{
double lj0 = gsl_matrix_get (L, j, 0);
double vj = gsl_vector_get (v, j);
gsl_matrix_set (L, j, 0, lj0 + w0 * vj);
}
/* Apply Givens transformations L' = G_(n-1)^T ... G_1^T H
Equation 12.5.4 */
for (k = 1; k < GSL_MIN(M,N+1); k++)
{
double c, s;
double diag = gsl_matrix_get (L, k - 1, k - 1);
double offdiag = gsl_matrix_get (L, k - 1 , k);
create_givens (diag, offdiag, &c, &s);
apply_givens_lq (M, N, Q, L, k - 1, k, c, s);
gsl_matrix_set (L, k - 1, k, 0.0); /* exact zero of G^T */
}
return GSL_SUCCESS;
}
}
int
gsl_linalg_QRPT_update (gsl_matrix * Q, gsl_matrix * R,
const gsl_permutation * p,
gsl_vector * w, const gsl_vector * v)
{
const size_t M = R->size1;
const size_t N = R->size2;
if (Q->size1 != M || Q->size2 != M)
{
GSL_ERROR ("Q matrix must be M x M if R is M x N", GSL_ENOTSQR);
}
else if (w->size != M)
{
GSL_ERROR ("w must be length M if R is M x N", GSL_EBADLEN);
}
else if (v->size != N)
{
GSL_ERROR ("v must be length N if R is M x N", GSL_EBADLEN);
}
else
{
size_t j, k;
double w0;
/* Apply Given's rotations to reduce w to (|w|, 0, 0, ... , 0)
J_1^T .... J_(n-1)^T w = +/- |w| e_1
simultaneously applied to R, H = J_1^T ... J^T_(n-1) R
so that H is upper Hessenberg. (12.5.2) */
for (k = M - 1; k > 0; k--)
{
double c, s;
double wk = gsl_vector_get (w, k);
double wkm1 = gsl_vector_get (w, k - 1);
create_givens (wkm1, wk, &c, &s);
apply_givens_vec (w, k - 1, k, c, s);
apply_givens_qr (M, N, Q, R, k - 1, k, c, s);
}
w0 = gsl_vector_get (w, 0);
/* Add in w v^T (Equation 12.5.3) */
for (j = 0; j < N; j++)
{
double r0j = gsl_matrix_get (R, 0, j);
size_t p_j = gsl_permutation_get (p, j);
double vj = gsl_vector_get (v, p_j);
gsl_matrix_set (R, 0, j, r0j + w0 * vj);
}
/* Apply Givens transformations R' = G_(n-1)^T ... G_1^T H
Equation 12.5.4 */
for (k = 1; k < GSL_MIN(M,N+1); k++)
{
double c, s;
double diag = gsl_matrix_get (R, k - 1, k - 1);
double offdiag = gsl_matrix_get (R, k, k - 1);
create_givens (diag, offdiag, &c, &s);
apply_givens_qr (M, N, Q, R, k - 1, k, c, s);
gsl_matrix_set (R, k, k - 1, 0.0); /* exact zero of G^T */
}
return GSL_SUCCESS;
}
}
static void
qrstep (const size_t n, double d[], double sd[], double gc[], double gs[])
{
double x, z;
double ak, bk, zk, ap, bp, aq, bq;
size_t k;
double mu = trailing_eigenvalue (n, d, sd);
x = d[0] - mu;
z = sd[0];
ak = 0;
bk = 0;
zk = 0;
ap = d[0];
bp = sd[0];
aq = d[1];
if (n == 2)
{
double c, s;
create_givens (x, z, &c, &s);
if (gc != NULL)
gc[0] = c;
if (gs != NULL)
gs[0] = s;
{
double ap1 = c * (c * ap - s * bp) + s * (s * aq - c * bp);
double bp1 = c * (s * ap + c * bp) - s * (s * bp + c * aq);
double aq1 = s * (s * ap + c * bp) + c * (s * bp + c * aq);
ak = ap1;
bk = bp1;
ap = aq1;
}
d[0] = ak;
sd[0] = bk;
d[1] = ap;
return;
}
bq = sd[1];
for (k = 0; k < n - 1; k++)
{
double c, s;
create_givens (x, z, &c, &s);
/* store Givens rotation */
if (gc != NULL)
gc[k] = c;
if (gs != NULL)
gs[k] = s;
/* compute G' T G */
{
double bk1 = c * bk - s * zk;
double ap1 = c * (c * ap - s * bp) + s * (s * aq - c * bp);
double bp1 = c * (s * ap + c * bp) - s * (s * bp + c * aq);
double zp1 = -s * bq;
double aq1 = s * (s * ap + c * bp) + c * (s * bp + c * aq);
double bq1 = c * bq;
ak = ap1;
bk = bp1;
zk = zp1;
ap = aq1;
bp = bq1;
if (k < n - 2)
aq = d[k + 2];
if (k < n - 3)
bq = sd[k + 2];
d[k] = ak;
if (k > 0)
sd[k - 1] = bk1;
if (k < n - 2)
sd[k + 1] = bp;
x = bk;
z = zk;
}
}
/* k = n - 1 */
d[k] = ap;
sd[k - 1] = bk;
}
