int
gsl_linalg_cholesky_invert(gsl_matrix * LLT)
{
if (LLT->size1 != LLT->size2)
{
GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
}
else
{
const size_t N = LLT->size1;
size_t i;
gsl_vector_view v1, v2;
/* invert the lower triangle of LLT */
gsl_linalg_tri_lower_invert(LLT);
/*
* The lower triangle of LLT now contains L^{-1}. Now compute
* A^{-1} = L^{-T} L^{-1}
*/
for (i = 0; i < N; ++i)
{
double aii = gsl_matrix_get(LLT, i, i);
if (i < N - 1)
{
double tmp;
v1 = gsl_matrix_subcolumn(LLT, i, i, N - i);
gsl_blas_ddot(&v1.vector, &v1.vector, &tmp);
gsl_matrix_set(LLT, i, i, tmp);
if (i > 0)
{
gsl_matrix_view m = gsl_matrix_submatrix(LLT, i + 1, 0, N - i - 1, i);
v1 = gsl_matrix_subcolumn(LLT, i, i + 1, N - i - 1);
v2 = gsl_matrix_subrow(LLT, i, 0, i);
gsl_blas_dgemv(CblasTrans, 1.0, &m.matrix, &v1.vector, aii, &v2.vector);
}
}
else
{
v1 = gsl_matrix_row(LLT, N - 1);
gsl_blas_dscal(aii, &v1.vector);
}
}
/* copy lower triangle to upper */
gsl_matrix_transpose_tricpy('L', 0, LLT, LLT);
return GSL_SUCCESS;
}
} /* gsl_linalg_cholesky_invert() */
/*
* FUNCTION
* Name: stationary
* Description: Given the dissipator in Bloch form, reduce to a 3x3 problem and store
* the stationary state in the 3x1 vector *X
*
* M X = 0
*
* | 0 0 0 0 | | 1 | 0
* | M10 M11 M12 M13 | | X1 | 0
* | M20 M21 M22 M23 | | X2 | = 0
* | M30 M31 M32 M33 | | X3 | 0
*
*
* A x = b
*
* | M11 M12 M13 | | X1 | | -M10 |
* | M21 M22 M23 | | X2 | = | -M20 |
* | M31 M32 M33 | | X3 | | -M30 |
*/
int stationary ( const gsl_matrix* M, gsl_vector* stat_state )
{
/* Store space for the stationary state */
gsl_vector* req = gsl_vector_calloc ( 4 ) ;
gsl_vector_set ( req, 0, 1 ) ;
/* Copy the dissipator matrix in a temporary local matrix m
* (because the algorithm destroys it...) */
gsl_matrix* m = gsl_matrix_calloc ( 4, 4 ) ;
gsl_matrix_memcpy ( m, M ) ;
/* Create a view of the spatial part of vector req */
gsl_vector_view x = gsl_vector_subvector ( req, 1, 3 ) ;
/* Create a submatrix view of the spatial part of m and a vector view
* of the spatial part of the 0-th column, which goes into -b in the system
* A x = b */
gsl_matrix_view A = gsl_matrix_submatrix ( m, 1, 1, 3, 3 ) ;
gsl_vector_view b = gsl_matrix_subcolumn ( m, 0, 1, 3 ) ;
int status1 = gsl_vector_scale ( &b.vector, -1.0 ) ;
/* Solve the system A x = b using Householder transformations.
* Changing the view x of req => also req is changed, in the spatial part */
int status2 = gsl_linalg_HH_solve ( &A.matrix, &b.vector, &x.vector ) ;
/* Set the returning value for the state stat_state */
*stat_state = *req ;
/* Free memory */
gsl_matrix_free(m) ;
return status1 + status2 ;
} /* ----- end of function stationary ----- */
static int
pcholesky_decomp (const int copy_uplo, gsl_matrix * A, gsl_permutation * p)
{
const size_t N = A->size1;
if (N != A->size2)
{
GSL_ERROR("LDLT decomposition requires square matrix", GSL_ENOTSQR);
}
else if (p->size != N)
{
GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
}
else
{
gsl_vector_view diag = gsl_matrix_diagonal(A);
size_t k;
if (copy_uplo)
{
/* save a copy of A in upper triangle (for later rcond calculation) */
gsl_matrix_transpose_tricpy('L', 0, A, A);
}
gsl_permutation_init(p);
for (k = 0; k < N; ++k)
{
gsl_vector_view w;
size_t j;
/* compute j = max_idx { A_kk, ..., A_nn } */
w = gsl_vector_subvector(&diag.vector, k, N - k);
j = gsl_vector_max_index(&w.vector) + k;
gsl_permutation_swap(p, k, j);
cholesky_swap_rowcol(A, k, j);
if (k < N - 1)
{
double alpha = gsl_matrix_get(A, k, k);
double alphainv = 1.0 / alpha;
/* v = A(k+1:n, k) */
gsl_vector_view v = gsl_matrix_subcolumn(A, k, k + 1, N - k - 1);
/* m = A(k+1:n, k+1:n) */
gsl_matrix_view m = gsl_matrix_submatrix(A, k + 1, k + 1, N - k - 1, N - k - 1);
/* m = m - v v^T / alpha */
gsl_blas_dsyr(CblasLower, -alphainv, &v.vector, &m.matrix);
/* v = v / alpha */
gsl_vector_scale(&v.vector, alphainv);
}
}
return GSL_SUCCESS;
}
}
static int
cod_householder_mh(const double tau, const gsl_vector * v, gsl_matrix * A,
gsl_vector * work)
{
if (tau == 0)
{
return GSL_SUCCESS; /* H = I */
}
else
{
const size_t M = A->size1;
const size_t N = A->size2;
const size_t L = v->size;
gsl_vector_view A1 = gsl_matrix_subcolumn(A, 0, 0, M);
gsl_matrix_view C = gsl_matrix_submatrix(A, 0, N - L, M, L);
/* work(1:M) = A(1:M,1) */
gsl_vector_memcpy(work, &A1.vector);
/* work(1:M) = work(1:M) + A(1:M,M+1:N) * v(1:N-M) */
gsl_blas_dgemv(CblasNoTrans, 1.0, &C.matrix, v, 1.0, work);
/* A(1:M,1) = A(1:M,1) - tau * work(1:M) */
gsl_blas_daxpy(-tau, work, &A1.vector);
/* A(1:M,M+1:N) = A(1:M,M+1:N) - tau * work(1:M) * v(1:N-M)' */
gsl_blas_dger(-tau, work, v, &C.matrix);
return GSL_SUCCESS;
}
}
int
gsl_eigen_gensymm_standardize(gsl_matrix *A, const gsl_matrix *B)
{
const size_t N = A->size1;
size_t i;
double a, b, c;
for (i = 0; i < N; ++i)
{
/* update lower triangle of A(i:n, i:n) */
a = gsl_matrix_get(A, i, i);
b = gsl_matrix_get(B, i, i);
a /= b * b;
gsl_matrix_set(A, i, i, a);
if (i < N - 1)
{
gsl_vector_view ai = gsl_matrix_subcolumn(A, i, i + 1, N - i - 1);
gsl_matrix_view ma =
gsl_matrix_submatrix(A, i + 1, i + 1, N - i - 1, N - i - 1);
gsl_vector_const_view bi =
gsl_matrix_const_subcolumn(B, i, i + 1, N - i - 1);
gsl_matrix_const_view mb =
gsl_matrix_const_submatrix(B, i + 1, i + 1, N - i - 1, N - i - 1);
gsl_blas_dscal(1.0 / b, &ai.vector);
c = -0.5 * a;
gsl_blas_daxpy(c, &bi.vector, &ai.vector);
gsl_blas_dsyr2(CblasLower, -1.0, &ai.vector, &bi.vector, &ma.matrix);
gsl_blas_daxpy(c, &bi.vector, &ai.vector);
gsl_blas_dtrsv(CblasLower,
CblasNoTrans,
CblasNonUnit,
&mb.matrix,
&ai.vector);
}
}
return GSL_SUCCESS;
} /* gsl_eigen_gensymm_standardize() */
int
gsl_linalg_cholesky_decomp1 (gsl_matrix * A)
{
const size_t M = A->size1;
const size_t N = A->size2;
if (M != N)
{
GSL_ERROR("cholesky decomposition requires square matrix", GSL_ENOTSQR);
}
else
{
size_t j;
/* save original matrix in upper triangle for later rcond calculation */
gsl_matrix_transpose_tricpy('L', 0, A, A);
for (j = 0; j < N; ++j)
{
double ajj;
gsl_vector_view v = gsl_matrix_subcolumn(A, j, j, N - j); /* A(j:n,j) */
if (j > 0)
{
gsl_vector_view w = gsl_matrix_subrow(A, j, 0, j); /* A(j,1:j-1)^T */
gsl_matrix_view m = gsl_matrix_submatrix(A, j, 0, N - j, j); /* A(j:n,1:j-1) */
gsl_blas_dgemv(CblasNoTrans, -1.0, &m.matrix, &w.vector, 1.0, &v.vector);
}
ajj = gsl_matrix_get(A, j, j);
if (ajj <= 0.0)
{
GSL_ERROR("matrix is not positive definite", GSL_EDOM);
}
ajj = sqrt(ajj);
gsl_vector_scale(&v.vector, 1.0 / ajj);
}
return GSL_SUCCESS;
}
}
static void
nonsymmv_get_right_eigenvectors(gsl_matrix *T, gsl_matrix *Z,
gsl_vector_complex *eval,
gsl_matrix_complex *evec,
gsl_eigen_nonsymmv_workspace *w)
{
const size_t N = T->size1;
const double smlnum = GSL_DBL_MIN * N / GSL_DBL_EPSILON;
const double bignum = (1.0 - GSL_DBL_EPSILON) / smlnum;
int i; /* looping */
size_t iu, /* looping */
ju,
ii;
gsl_complex lambda; /* current eigenvalue */
double lambda_re, /* Re(lambda) */
lambda_im; /* Im(lambda) */
gsl_matrix_view Tv, /* temporary views */
Zv;
gsl_vector_view y, /* temporary views */
y2,
ev,
ev2;
double dat[4], /* scratch arrays */
dat_X[4];
double scale; /* scale factor */
double xnorm; /* |X| */
gsl_vector_complex_view ecol, /* column of evec */
ecol2;
int complex_pair; /* complex eigenvalue pair? */
double smin;
/*
* Compute 1-norm of each column of upper triangular part of T
* to control overflow in triangular solver
*/
gsl_vector_set(w->work3, 0, 0.0);
for (ju = 1; ju < N; ++ju)
{
gsl_vector_set(w->work3, ju, 0.0);
for (iu = 0; iu < ju; ++iu)
{
gsl_vector_set(w->work3, ju,
gsl_vector_get(w->work3, ju) +
fabs(gsl_matrix_get(T, iu, ju)));
}
}
for (i = (int) N - 1; i >= 0; --i)
{
iu = (size_t) i;
/* get current eigenvalue and store it in lambda */
lambda_re = gsl_matrix_get(T, iu, iu);
if (iu != 0 && gsl_matrix_get(T, iu, iu - 1) != 0.0)
{
lambda_im = sqrt(fabs(gsl_matrix_get(T, iu, iu - 1))) *
sqrt(fabs(gsl_matrix_get(T, iu - 1, iu)));
}
else
{
lambda_im = 0.0;
}
GSL_SET_COMPLEX(&lambda, lambda_re, lambda_im);
smin = GSL_MAX(GSL_DBL_EPSILON * (fabs(lambda_re) + fabs(lambda_im)),
smlnum);
smin = GSL_MAX(smin, GSL_NONSYMMV_SMLNUM);
if (lambda_im == 0.0)
{
int k, l;
gsl_vector_view bv, xv;
/* real eigenvector */
/*
* The ordering of eigenvalues in 'eval' is arbitrary and
* does not necessarily follow the Schur form T, so store
* lambda in the right slot in eval to ensure it corresponds
* to the eigenvector we are about to compute
*/
gsl_vector_complex_set(eval, iu, lambda);
/*
* We need to solve the system:
*
* (T(1:iu-1, 1:iu-1) - lambda*I)*X = -T(1:iu-1,iu)
*/
/* construct right hand side */
for (k = 0; k < i; ++k)
{
gsl_vector_set(w->work,
(size_t) k,
-gsl_matrix_get(T, (size_t) k, iu));
}
gsl_vector_set(w->work, iu, 1.0);
for (l = i - 1; l >= 0; --l)
{
size_t lu = (size_t) l;
if (lu == 0)
complex_pair = 0;
else
complex_pair = gsl_matrix_get(T, lu, lu - 1) != 0.0;
if (!complex_pair)
{
double x;
/*
* 1-by-1 diagonal block - solve the system:
*
* (T_{ll} - lambda)*x = -T_{l(iu)}
*/
Tv = gsl_matrix_submatrix(T, lu, lu, 1, 1);
bv = gsl_vector_view_array(dat, 1);
gsl_vector_set(&bv.vector, 0,
gsl_vector_get(w->work, lu));
xv = gsl_vector_view_array(dat_X, 1);
gsl_schur_solve_equation(1.0,
&Tv.matrix,
lambda_re,
1.0,
1.0,
&bv.vector,
&xv.vector,
&scale,
&xnorm,
smin);
/* scale x to avoid overflow */
x = gsl_vector_get(&xv.vector, 0);
if (xnorm > 1.0)
{
if (gsl_vector_get(w->work3, lu) > bignum / xnorm)
{
x /= xnorm;
scale /= xnorm;
}
}
if (scale != 1.0)
{
gsl_vector_view wv;
wv = gsl_vector_subvector(w->work, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
}
gsl_vector_set(w->work, lu, x);
if (lu > 0)
{
gsl_vector_view v1, v2;
/* update right hand side */
v1 = gsl_matrix_subcolumn(T, lu, 0, lu);
v2 = gsl_vector_subvector(w->work, 0, lu);
gsl_blas_daxpy(-x, &v1.vector, &v2.vector);
} /* if (l > 0) */
} /* if (!complex_pair) */
else
{
double x11, x21;
/*
* 2-by-2 diagonal block
*/
Tv = gsl_matrix_submatrix(T, lu - 1, lu - 1, 2, 2);
bv = gsl_vector_view_array(dat, 2);
gsl_vector_set(&bv.vector, 0,
gsl_vector_get(w->work, lu - 1));
gsl_vector_set(&bv.vector, 1,
gsl_vector_get(w->work, lu));
xv = gsl_vector_view_array(dat_X, 2);
gsl_schur_solve_equation(1.0,
&Tv.matrix,
lambda_re,
1.0,
1.0,
&bv.vector,
&xv.vector,
&scale,
&xnorm,
smin);
/* scale X(1,1) and X(2,1) to avoid overflow */
x11 = gsl_vector_get(&xv.vector, 0);
x21 = gsl_vector_get(&xv.vector, 1);
if (xnorm > 1.0)
{
double beta;
beta = GSL_MAX(gsl_vector_get(w->work3, lu - 1),
gsl_vector_get(w->work3, lu));
if (beta > bignum / xnorm)
{
x11 /= xnorm;
x21 /= xnorm;
scale /= xnorm;
}
}
/* scale if necessary */
if (scale != 1.0)
{
gsl_vector_view wv;
wv = gsl_vector_subvector(w->work, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
}
gsl_vector_set(w->work, lu - 1, x11);
gsl_vector_set(w->work, lu, x21);
/* update right hand side */
if (lu > 1)
{
gsl_vector_view v1, v2;
v1 = gsl_matrix_subcolumn(T, lu - 1, 0, lu - 1);
v2 = gsl_vector_subvector(w->work, 0, lu - 1);
gsl_blas_daxpy(-x11, &v1.vector, &v2.vector);
v1 = gsl_matrix_subcolumn(T, lu, 0, lu - 1);
gsl_blas_daxpy(-x21, &v1.vector, &v2.vector);
}
--l;
} /* if (complex_pair) */
} /* for (l = i - 1; l >= 0; --l) */
/*
* At this point, w->work is an eigenvector of the
* Schur form T. To get an eigenvector of the original
* matrix, we multiply on the left by Z, the matrix of
* Schur vectors
*/
ecol = gsl_matrix_complex_column(evec, iu);
y = gsl_matrix_column(Z, iu);
if (iu > 0)
{
gsl_vector_view x;
Zv = gsl_matrix_submatrix(Z, 0, 0, N, iu);
x = gsl_vector_subvector(w->work, 0, iu);
/* compute Z * w->work and store it in Z(:,iu) */
gsl_blas_dgemv(CblasNoTrans,
1.0,
&Zv.matrix,
&x.vector,
gsl_vector_get(w->work, iu),
&y.vector);
} /* if (iu > 0) */
/* store eigenvector into evec */
ev = gsl_vector_complex_real(&ecol.vector);
ev2 = gsl_vector_complex_imag(&ecol.vector);
scale = 0.0;
for (ii = 0; ii < N; ++ii)
{
double a = gsl_vector_get(&y.vector, ii);
/* store real part of eigenvector */
gsl_vector_set(&ev.vector, ii, a);
/* set imaginary part to 0 */
gsl_vector_set(&ev2.vector, ii, 0.0);
if (fabs(a) > scale)
scale = fabs(a);
}
if (scale != 0.0)
scale = 1.0 / scale;
/* scale by magnitude of largest element */
gsl_blas_dscal(scale, &ev.vector);
} /* if (GSL_IMAG(lambda) == 0.0) */
else
{
gsl_vector_complex_view bv, xv;
size_t k;
int l;
gsl_complex lambda2;
/* complex eigenvector */
/*
* Store the complex conjugate eigenvalues in the right
* slots in eval
*/
GSL_SET_REAL(&lambda2, GSL_REAL(lambda));
GSL_SET_IMAG(&lambda2, -GSL_IMAG(lambda));
gsl_vector_complex_set(eval, iu - 1, lambda);
gsl_vector_complex_set(eval, iu, lambda2);
/*
* First solve:
*
* [ T(i:i+1,i:i+1) - lambda*I ] * X = 0
*/
if (fabs(gsl_matrix_get(T, iu - 1, iu)) >=
fabs(gsl_matrix_get(T, iu, iu - 1)))
{
gsl_vector_set(w->work, iu - 1, 1.0);
gsl_vector_set(w->work2, iu,
lambda_im / gsl_matrix_get(T, iu - 1, iu));
}
else
{
gsl_vector_set(w->work, iu - 1,
-lambda_im / gsl_matrix_get(T, iu, iu - 1));
gsl_vector_set(w->work2, iu, 1.0);
}
gsl_vector_set(w->work, iu, 0.0);
gsl_vector_set(w->work2, iu - 1, 0.0);
/* construct right hand side */
for (k = 0; k < iu - 1; ++k)
{
gsl_vector_set(w->work, k,
-gsl_vector_get(w->work, iu - 1) *
gsl_matrix_get(T, k, iu - 1));
gsl_vector_set(w->work2, k,
-gsl_vector_get(w->work2, iu) *
gsl_matrix_get(T, k, iu));
}
/*
* We must solve the upper quasi-triangular system:
*
* [ T(1:i-2,1:i-2) - lambda*I ] * X = s*(work + i*work2)
*/
for (l = i - 2; l >= 0; --l)
{
size_t lu = (size_t) l;
if (lu == 0)
complex_pair = 0;
else
complex_pair = gsl_matrix_get(T, lu, lu - 1) != 0.0;
if (!complex_pair)
{
gsl_complex bval;
gsl_complex x;
/*
* 1-by-1 diagonal block - solve the system:
*
* (T_{ll} - lambda)*x = work + i*work2
*/
Tv = gsl_matrix_submatrix(T, lu, lu, 1, 1);
bv = gsl_vector_complex_view_array(dat, 1);
xv = gsl_vector_complex_view_array(dat_X, 1);
GSL_SET_COMPLEX(&bval,
gsl_vector_get(w->work, lu),
gsl_vector_get(w->work2, lu));
gsl_vector_complex_set(&bv.vector, 0, bval);
gsl_schur_solve_equation_z(1.0,
&Tv.matrix,
&lambda,
1.0,
1.0,
&bv.vector,
&xv.vector,
&scale,
&xnorm,
smin);
if (xnorm > 1.0)
{
if (gsl_vector_get(w->work3, lu) > bignum / xnorm)
{
gsl_blas_zdscal(1.0/xnorm, &xv.vector);
scale /= xnorm;
}
}
/* scale if necessary */
if (scale != 1.0)
{
gsl_vector_view wv;
wv = gsl_vector_subvector(w->work, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
wv = gsl_vector_subvector(w->work2, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
}
x = gsl_vector_complex_get(&xv.vector, 0);
gsl_vector_set(w->work, lu, GSL_REAL(x));
gsl_vector_set(w->work2, lu, GSL_IMAG(x));
/* update the right hand side */
if (lu > 0)
{
gsl_vector_view v1, v2;
v1 = gsl_matrix_subcolumn(T, lu, 0, lu);
v2 = gsl_vector_subvector(w->work, 0, lu);
gsl_blas_daxpy(-GSL_REAL(x), &v1.vector, &v2.vector);
v2 = gsl_vector_subvector(w->work2, 0, lu);
gsl_blas_daxpy(-GSL_IMAG(x), &v1.vector, &v2.vector);
} /* if (lu > 0) */
} /* if (!complex_pair) */
else
{
gsl_complex b1, b2, x1, x2;
/*
* 2-by-2 diagonal block - solve the system
*/
Tv = gsl_matrix_submatrix(T, lu - 1, lu - 1, 2, 2);
bv = gsl_vector_complex_view_array(dat, 2);
xv = gsl_vector_complex_view_array(dat_X, 2);
GSL_SET_COMPLEX(&b1,
gsl_vector_get(w->work, lu - 1),
gsl_vector_get(w->work2, lu - 1));
GSL_SET_COMPLEX(&b2,
gsl_vector_get(w->work, lu),
gsl_vector_get(w->work2, lu));
gsl_vector_complex_set(&bv.vector, 0, b1);
gsl_vector_complex_set(&bv.vector, 1, b2);
gsl_schur_solve_equation_z(1.0,
&Tv.matrix,
&lambda,
1.0,
1.0,
&bv.vector,
&xv.vector,
&scale,
&xnorm,
smin);
x1 = gsl_vector_complex_get(&xv.vector, 0);
x2 = gsl_vector_complex_get(&xv.vector, 1);
if (xnorm > 1.0)
{
double beta;
beta = GSL_MAX(gsl_vector_get(w->work3, lu - 1),
gsl_vector_get(w->work3, lu));
if (beta > bignum / xnorm)
{
gsl_blas_zdscal(1.0/xnorm, &xv.vector);
scale /= xnorm;
}
}
/* scale if necessary */
if (scale != 1.0)
{
gsl_vector_view wv;
wv = gsl_vector_subvector(w->work, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
wv = gsl_vector_subvector(w->work2, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
}
gsl_vector_set(w->work, lu - 1, GSL_REAL(x1));
gsl_vector_set(w->work, lu, GSL_REAL(x2));
gsl_vector_set(w->work2, lu - 1, GSL_IMAG(x1));
gsl_vector_set(w->work2, lu, GSL_IMAG(x2));
/* update right hand side */
if (lu > 1)
{
gsl_vector_view v1, v2, v3, v4;
v1 = gsl_matrix_subcolumn(T, lu - 1, 0, lu - 1);
v4 = gsl_matrix_subcolumn(T, lu, 0, lu - 1);
v2 = gsl_vector_subvector(w->work, 0, lu - 1);
v3 = gsl_vector_subvector(w->work2, 0, lu - 1);
gsl_blas_daxpy(-GSL_REAL(x1), &v1.vector, &v2.vector);
gsl_blas_daxpy(-GSL_REAL(x2), &v4.vector, &v2.vector);
gsl_blas_daxpy(-GSL_IMAG(x1), &v1.vector, &v3.vector);
gsl_blas_daxpy(-GSL_IMAG(x2), &v4.vector, &v3.vector);
} /* if (lu > 1) */
--l;
} /* if (complex_pair) */
} /* for (l = i - 2; l >= 0; --l) */
/*
* At this point, work + i*work2 is an eigenvector
* of T - backtransform to get an eigenvector of the
* original matrix
*/
y = gsl_matrix_column(Z, iu - 1);
y2 = gsl_matrix_column(Z, iu);
if (iu > 1)
{
gsl_vector_view x;
/* compute real part of eigenvectors */
Zv = gsl_matrix_submatrix(Z, 0, 0, N, iu - 1);
x = gsl_vector_subvector(w->work, 0, iu - 1);
gsl_blas_dgemv(CblasNoTrans,
1.0,
&Zv.matrix,
&x.vector,
gsl_vector_get(w->work, iu - 1),
&y.vector);
/* now compute the imaginary part */
x = gsl_vector_subvector(w->work2, 0, iu - 1);
gsl_blas_dgemv(CblasNoTrans,
1.0,
&Zv.matrix,
&x.vector,
gsl_vector_get(w->work2, iu),
&y2.vector);
}
else
{
gsl_blas_dscal(gsl_vector_get(w->work, iu - 1), &y.vector);
gsl_blas_dscal(gsl_vector_get(w->work2, iu), &y2.vector);
}
/*
* Now store the eigenvectors into evec - the real parts
* are Z(:,iu - 1) and the imaginary parts are
* +/- Z(:,iu)
*/
/* get views of the two eigenvector slots */
ecol = gsl_matrix_complex_column(evec, iu - 1);
ecol2 = gsl_matrix_complex_column(evec, iu);
/*
* save imaginary part first as it may get overwritten
* when copying the real part due to our storage scheme
* in Z/evec
*/
ev = gsl_vector_complex_imag(&ecol.vector);
ev2 = gsl_vector_complex_imag(&ecol2.vector);
scale = 0.0;
for (ii = 0; ii < N; ++ii)
{
double a = gsl_vector_get(&y2.vector, ii);
scale = GSL_MAX(scale,
fabs(a) + fabs(gsl_vector_get(&y.vector, ii)));
gsl_vector_set(&ev.vector, ii, a);
gsl_vector_set(&ev2.vector, ii, -a);
}
/* now save the real part */
ev = gsl_vector_complex_real(&ecol.vector);
ev2 = gsl_vector_complex_real(&ecol2.vector);
for (ii = 0; ii < N; ++ii)
{
double a = gsl_vector_get(&y.vector, ii);
gsl_vector_set(&ev.vector, ii, a);
gsl_vector_set(&ev2.vector, ii, a);
}
if (scale != 0.0)
scale = 1.0 / scale;
/* scale by largest element magnitude */
gsl_blas_zdscal(scale, &ecol.vector);
gsl_blas_zdscal(scale, &ecol2.vector);
/*
* decrement i since we took care of two eigenvalues at
* the same time
*/
--i;
} /* if (GSL_IMAG(lambda) != 0.0) */
} /* for (i = (int) N - 1; i >= 0; --i) */
} /* nonsymmv_get_right_eigenvectors() */
static int
triangular_inverse(CBLAS_UPLO_t Uplo, CBLAS_DIAG_t Diag, gsl_matrix * T)
{
const size_t N = T->size1;
if (N != T->size2)
{
GSL_ERROR ("matrix must be square", GSL_ENOTSQR);
}
else
{
gsl_matrix_view m;
gsl_vector_view v;
size_t i;
if (Uplo == CblasUpper)
{
for (i = 0; i < N; ++i)
{
double aii;
if (Diag == CblasNonUnit)
{
double *Tii = gsl_matrix_ptr(T, i, i);
*Tii = 1.0 / *Tii;
aii = -(*Tii);
}
else
{
aii = -1.0;
}
if (i > 0)
{
m = gsl_matrix_submatrix(T, 0, 0, i, i);
v = gsl_matrix_subcolumn(T, i, 0, i);
gsl_blas_dtrmv(CblasUpper, CblasNoTrans, Diag,
&m.matrix, &v.vector);
gsl_blas_dscal(aii, &v.vector);
}
} /* for (i = 0; i < N; ++i) */
}
else
{
for (i = 0; i < N; ++i)
{
double ajj;
size_t j = N - i - 1;
if (Diag == CblasNonUnit)
{
double *Tjj = gsl_matrix_ptr(T, j, j);
*Tjj = 1.0 / *Tjj;
ajj = -(*Tjj);
}
else
{
ajj = -1.0;
}
if (j < N - 1)
{
m = gsl_matrix_submatrix(T, j + 1, j + 1,
N - j - 1, N - j - 1);
v = gsl_matrix_subcolumn(T, j, j + 1, N - j - 1);
gsl_blas_dtrmv(CblasLower, CblasNoTrans, Diag,
&m.matrix, &v.vector);
gsl_blas_dscal(ajj, &v.vector);
}
} /* for (i = 0; i < N; ++i) */
}
return GSL_SUCCESS;
}
}
int
gsl_linalg_hessenberg_decomp(gsl_matrix *A, gsl_vector *tau)
{
const size_t N = A->size1;
if (N != A->size2)
{
GSL_ERROR ("Hessenberg reduction requires square matrix",
GSL_ENOTSQR);
}
else if (N != tau->size)
{
GSL_ERROR ("tau vector must match matrix size", GSL_EBADLEN);
}
else if (N < 3)
{
/* nothing to do */
return GSL_SUCCESS;
}
else
{
size_t i; /* looping */
gsl_vector_view c, /* matrix column */
hv; /* householder vector */
gsl_matrix_view m;
double tau_i; /* beta in algorithm 7.4.2 */
for (i = 0; i < N - 2; ++i)
{
/*
* make a copy of A(i + 1:n, i) and store it in the section
* of 'tau' that we haven't stored coefficients in yet
*/
c = gsl_matrix_subcolumn(A, i, i + 1, N - i - 1);
hv = gsl_vector_subvector(tau, i + 1, N - (i + 1));
gsl_vector_memcpy(&hv.vector, &c.vector);
/* compute householder transformation of A(i+1:n,i) */
tau_i = gsl_linalg_householder_transform(&hv.vector);
/* apply left householder matrix (I - tau_i v v') to A */
m = gsl_matrix_submatrix(A, i + 1, i, N - (i + 1), N - i);
gsl_linalg_householder_hm(tau_i, &hv.vector, &m.matrix);
/* apply right householder matrix (I - tau_i v v') to A */
m = gsl_matrix_submatrix(A, 0, i + 1, N, N - (i + 1));
gsl_linalg_householder_mh(tau_i, &hv.vector, &m.matrix);
/* save Householder coefficient */
gsl_vector_set(tau, i, tau_i);
/*
* store Householder vector below the subdiagonal in column
* i of the matrix. hv(1) does not need to be stored since
* it is always 1.
*/
c = gsl_vector_subvector(&c.vector, 1, c.vector.size - 1);
hv = gsl_vector_subvector(&hv.vector, 1, hv.vector.size - 1);
gsl_vector_memcpy(&c.vector, &hv.vector);
}
return GSL_SUCCESS;
}
} /* gsl_linalg_hessenberg_decomp() */
static int
gmres_iterate(const gsl_spmatrix *A, const gsl_vector *b,
const double tol, gsl_vector *x,
void *vstate)
{
const size_t N = A->size1;
gmres_state_t *state = (gmres_state_t *) vstate;
if (N != A->size2)
{
GSL_ERROR("matrix must be square", GSL_ENOTSQR);
}
else if (N != b->size)
{
GSL_ERROR("matrix does not match right hand side", GSL_EBADLEN);
}
else if (N != x->size)
{
GSL_ERROR("matrix does not match solution vector", GSL_EBADLEN);
}
else if (N != state->n)
{
GSL_ERROR("matrix does not match workspace", GSL_EBADLEN);
}
else
{
int status = GSL_SUCCESS;
const size_t maxit = state->m;
const double normb = gsl_blas_dnrm2(b); /* ||b|| */
const double reltol = tol * normb; /* tol*||b|| */
double normr; /* ||r|| */
size_t m, k;
double tau; /* householder scalar */
gsl_matrix *H = state->H; /* Hessenberg matrix */
gsl_vector *r = state->r; /* residual vector */
gsl_vector *w = state->y; /* least squares RHS */
gsl_matrix_view Rm; /* R_m = H(1:m,2:m+1) */
gsl_vector_view ym; /* y(1:m) */
gsl_vector_view h0 = gsl_matrix_column(H, 0);
/*
* The Hessenberg matrix will have the following structure:
*
* H = [ ||r_0|| | v_1 v_2 ... v_m ]
* [ u_1 | u_2 u_3 ... u_{m+1} ]
*
* where v_j are the orthonormal vectors spanning the Krylov
* subpsace of length j + 1 and u_{j+1} are the householder
* vectors of length n - j - 1.
* In fact, u_{j+1} has length n - j since u_{j+1}[0] = 1,
* but this 1 is not stored.
*/
gsl_matrix_set_zero(H);
/* Step 1a: compute r = b - A*x_0 */
gsl_vector_memcpy(r, b);
gsl_spblas_dgemv(CblasNoTrans, -1.0, A, x, 1.0, r);
/* Step 1b */
gsl_vector_memcpy(&h0.vector, r);
tau = gsl_linalg_householder_transform(&h0.vector);
/* store tau_1 */
gsl_vector_set(state->tau, 0, tau);
/* initialize w (stored in state->y) */
gsl_vector_set_zero(w);
gsl_vector_set(w, 0, gsl_vector_get(&h0.vector, 0));
for (m = 1; m <= maxit; ++m)
{
size_t j = m - 1; /* C indexing */
double c, s; /* Givens rotation */
/* v_m */
gsl_vector_view vm = gsl_matrix_column(H, m);
/* v_m(m:end) */
gsl_vector_view vv = gsl_vector_subvector(&vm.vector, j, N - j);
/* householder vector u_m for projection P_m */
gsl_vector_view um = gsl_matrix_subcolumn(H, j, j, N - j);
/* Step 2a: form v_m = P_m e_m = e_m - tau_m w_m */
gsl_vector_set_zero(&vm.vector);
gsl_vector_memcpy(&vv.vector, &um.vector);
tau = gsl_vector_get(state->tau, j); /* tau_m */
gsl_vector_scale(&vv.vector, -tau);
gsl_vector_set(&vv.vector, 0, 1.0 - tau);
/* Step 2a: v_m <- P_1 P_2 ... P_{m-1} v_m */
for (k = j; k > 0 && k--; )
{
gsl_vector_view uk =
gsl_matrix_subcolumn(H, k, k, N - k);
gsl_vector_view vk =
gsl_vector_subvector(&vm.vector, k, N - k);
tau = gsl_vector_get(state->tau, k);
gsl_linalg_householder_hv(tau, &uk.vector, &vk.vector);
}
/* Step 2a: v_m <- A*v_m */
gsl_spblas_dgemv(CblasNoTrans, 1.0, A, &vm.vector, 0.0, r);
gsl_vector_memcpy(&vm.vector, r);
/* Step 2a: v_m <- P_m ... P_1 v_m */
for (k = 0; k <= j; ++k)
{
gsl_vector_view uk = gsl_matrix_subcolumn(H, k, k, N - k);
gsl_vector_view vk = gsl_vector_subvector(&vm.vector, k, N - k);
tau = gsl_vector_get(state->tau, k);
gsl_linalg_householder_hv(tau, &uk.vector, &vk.vector);
}
/* Steps 2c,2d: find P_{m+1} and set v_m <- P_{m+1} v_m */
if (m < N)
{
/* householder vector u_{m+1} for projection P_{m+1} */
gsl_vector_view ump1 = gsl_matrix_subcolumn(H, m, m, N - m);
tau = gsl_linalg_householder_transform(&ump1.vector);
gsl_vector_set(state->tau, j + 1, tau);
}
/* Step 2e: v_m <- J_{m-1} ... J_1 v_m */
for (k = 0; k < j; ++k)
{
gsl_linalg_givens_gv(&vm.vector, k, k + 1,
state->c[k], state->s[k]);
}
if (m < N)
{
/* Step 2g: find givens rotation J_m for v_m(m:m+1) */
gsl_linalg_givens(gsl_vector_get(&vm.vector, j),
gsl_vector_get(&vm.vector, j + 1),
&c, &s);
/* store givens rotation for later use */
state->c[j] = c;
state->s[j] = s;
/* Step 2h: v_m <- J_m v_m */
gsl_linalg_givens_gv(&vm.vector, j, j + 1, c, s);
/* Step 2h: w <- J_m w */
gsl_linalg_givens_gv(w, j, j + 1, c, s);
}
/*
* Step 2i: R_m = [ R_{m-1}, v_m ] - already taken care
* of due to our memory storage scheme
*/
/* Step 2j: check residual w_{m+1} for convergence */
normr = fabs(gsl_vector_get(w, j + 1));
if (normr <= reltol)
{
/*
* method has converged, break out of loop to compute
* update to solution vector x
*/
break;
}
}
/*
* At this point, we have either converged to a solution or
* completed all maxit iterations. In either case, compute
* an update to the solution vector x and test again for
* convergence.
*/
/* rewind m if we exceeded maxit iterations */
if (m > maxit)
m--;
/* Step 3a: solve triangular system R_m y_m = w, in place */
Rm = gsl_matrix_submatrix(H, 0, 1, m, m);
ym = gsl_vector_subvector(w, 0, m);
gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit,
&Rm.matrix, &ym.vector);
/*
* Step 3b: update solution vector x; the loop below
* uses a different but equivalent formulation from
* Saad, algorithm 6.10, step 14; store Krylov projection
* V_m y_m in 'r'
*/
gsl_vector_set_zero(r);
for (k = m; k > 0 && k--; )
{
double ymk = gsl_vector_get(&ym.vector, k);
gsl_vector_view uk = gsl_matrix_subcolumn(H, k, k, N - k);
gsl_vector_view rk = gsl_vector_subvector(r, k, N - k);
/* r <- n_k e_k + r */
gsl_vector_set(r, k, gsl_vector_get(r, k) + ymk);
/* r <- P_k r */
tau = gsl_vector_get(state->tau, k);
gsl_linalg_householder_hv(tau, &uk.vector, &rk.vector);
}
/* x <- x + V_m y_m */
gsl_vector_add(x, r);
/* compute new residual r = b - A*x */
gsl_vector_memcpy(r, b);
gsl_spblas_dgemv(CblasNoTrans, -1.0, A, x, 1.0, r);
normr = gsl_blas_dnrm2(r);
if (normr <= reltol)
status = GSL_SUCCESS; /* converged */
else
status = GSL_CONTINUE; /* not yet converged */
/* store residual norm */
state->normr = normr;
return status;
}
} /* gmres_iterate() */
int
gsl_multifit_linear_lcurve (const gsl_vector * y,
gsl_vector * reg_param,
gsl_vector * rho, gsl_vector * eta,
gsl_multifit_linear_workspace * work)
{
const size_t n = y->size;
const size_t N = rho->size; /* number of points on L-curve */
if (n != work->n)
{
GSL_ERROR("y vector does not match workspace", GSL_EBADLEN);
}
else if (N < 3)
{
GSL_ERROR ("at least 3 points are needed for L-curve analysis",
GSL_EBADLEN);
}
else if (N != eta->size)
{
GSL_ERROR ("size of rho and eta vectors do not match",
GSL_EBADLEN);
}
else if (reg_param->size != eta->size)
{
GSL_ERROR ("size of reg_param and eta vectors do not match",
GSL_EBADLEN);
}
else
{
int status = GSL_SUCCESS;
const size_t p = work->p;
size_t i, j;
gsl_matrix_view A = gsl_matrix_submatrix(work->A, 0, 0, n, p);
gsl_vector_view S = gsl_vector_subvector(work->S, 0, p);
gsl_vector_view xt = gsl_vector_subvector(work->xt, 0, p);
gsl_vector_view workp = gsl_matrix_subcolumn(work->QSI, 0, 0, p);
gsl_vector_view workp2 = gsl_vector_subvector(work->D, 0, p); /* D isn't used for regularized problems */
const double smax = gsl_vector_get(&S.vector, 0);
const double smin = gsl_vector_get(&S.vector, p - 1);
double dr; /* residual error from projection */
double normy = gsl_blas_dnrm2(y);
double normUTy;
/* compute projection xt = U^T y */
gsl_blas_dgemv (CblasTrans, 1.0, &A.matrix, y, 0.0, &xt.vector);
normUTy = gsl_blas_dnrm2(&xt.vector);
dr = normy*normy - normUTy*normUTy;
/* calculate regularization parameters */
gsl_multifit_linear_lreg(smin, smax, reg_param);
for (i = 0; i < N; ++i)
{
double lambda = gsl_vector_get(reg_param, i);
double lambda_sq = lambda * lambda;
for (j = 0; j < p; ++j)
{
double sj = gsl_vector_get(&S.vector, j);
double xtj = gsl_vector_get(&xt.vector, j);
double f = sj / (sj*sj + lambda_sq);
gsl_vector_set(&workp.vector, j, f * xtj);
gsl_vector_set(&workp2.vector, j, (1.0 - sj*f) * xtj);
}
gsl_vector_set(eta, i, gsl_blas_dnrm2(&workp.vector));
gsl_vector_set(rho, i, gsl_blas_dnrm2(&workp2.vector));
}
if (n > p && dr > 0.0)
{
/* add correction to residual norm (see eqs 6-7 of [1]) */
for (i = 0; i < N; ++i)
{
double rhoi = gsl_vector_get(rho, i);
double *ptr = gsl_vector_ptr(rho, i);
*ptr = sqrt(rhoi*rhoi + dr);
}
}
/* restore D to identity matrix */
gsl_vector_set_all(work->D, 1.0);
return status;
}
} /* gsl_multifit_linear_lcurve() */
int
gsl_multifit_linear_wstdform2 (const gsl_matrix * LQR,
const gsl_vector * Ltau,
const gsl_matrix * X,
const gsl_vector * w,
const gsl_vector * y,
gsl_matrix * Xs,
gsl_vector * ys,
gsl_matrix * M,
gsl_multifit_linear_workspace * work)
{
const size_t m = LQR->size1;
const size_t n = X->size1;
const size_t p = X->size2;
if (n > work->nmax || p > work->pmax)
{
GSL_ERROR("observation matrix larger than workspace", GSL_EBADLEN);
}
else if (p != LQR->size2)
{
GSL_ERROR("LQR and X matrices have different numbers of columns", GSL_EBADLEN);
}
else if (n != y->size)
{
GSL_ERROR("y vector does not match X", GSL_EBADLEN);
}
else if (w != NULL && n != w->size)
{
GSL_ERROR("weights vector must be length n", GSL_EBADLEN);
}
else if (m >= p) /* square or tall L matrix */
{
/* the sizes of Xs and ys depend on whether m >= p or m < p */
if (n != Xs->size1 || p != Xs->size2)
{
GSL_ERROR("Xs matrix must be n-by-p", GSL_EBADLEN);
}
else if (n != ys->size)
{
GSL_ERROR("ys vector must have length n", GSL_EBADLEN);
}
else
{
int status;
size_t i;
gsl_matrix_const_view R = gsl_matrix_const_submatrix(LQR, 0, 0, p, p);
/* compute Xs = sqrt(W) X and ys = sqrt(W) y */
status = gsl_multifit_linear_applyW(X, w, y, Xs, ys);
if (status)
return status;
/* compute X~ = X R^{-1} using QR decomposition of L */
for (i = 0; i < n; ++i)
{
gsl_vector_view v = gsl_matrix_row(Xs, i);
/* solve: R^T y = X_i */
gsl_blas_dtrsv(CblasUpper, CblasTrans, CblasNonUnit, &R.matrix, &v.vector);
}
return GSL_SUCCESS;
}
}
else /* L matrix with m < p */
{
const size_t pm = p - m;
const size_t npm = n - pm;
/*
* This code closely follows section 2.6.1 of Hansen's
* "Regularization Tools" manual
*/
if (npm != Xs->size1 || m != Xs->size2)
{
GSL_ERROR("Xs matrix must be (n-p+m)-by-m", GSL_EBADLEN);
}
else if (npm != ys->size)
{
GSL_ERROR("ys vector must be of length (n-p+m)", GSL_EBADLEN);
}
else if (n != M->size1 || p != M->size2)
{
GSL_ERROR("M matrix must be n-by-p", GSL_EBADLEN);
}
else
{
int status;
gsl_matrix_view A = gsl_matrix_submatrix(work->A, 0, 0, n, p);
gsl_vector_view b = gsl_vector_subvector(work->t, 0, n);
gsl_matrix_view LTQR = gsl_matrix_view_array(LQR->data, p, m); /* qr(L^T) */
gsl_matrix_view Rp = gsl_matrix_view_array(LQR->data, m, m); /* R factor of L^T */
gsl_vector_const_view LTtau = gsl_vector_const_subvector(Ltau, 0, m);
/*
* M(:,1:p-m) will hold QR decomposition of A K_o; M(:,p) will hold
* Householder scalars
*/
gsl_matrix_view MQR = gsl_matrix_submatrix(M, 0, 0, n, pm);
gsl_vector_view Mtau = gsl_matrix_subcolumn(M, p - 1, 0, GSL_MIN(n, pm));
gsl_matrix_view AKo, AKp, HqTAKp;
gsl_vector_view v;
size_t i;
/* compute A = sqrt(W) X and b = sqrt(W) y */
status = gsl_multifit_linear_applyW(X, w, y, &A.matrix, &b.vector);
if (status)
return status;
/* compute: A <- A K = [ A K_p ; A K_o ] */
gsl_linalg_QR_matQ(<QR.matrix, <tau.vector, &A.matrix);
AKp = gsl_matrix_submatrix(&A.matrix, 0, 0, n, m);
AKo = gsl_matrix_submatrix(&A.matrix, 0, m, n, pm);
/* compute QR decomposition [H,T] = qr(A * K_o) and store in M */
gsl_matrix_memcpy(&MQR.matrix, &AKo.matrix);
gsl_linalg_QR_decomp(&MQR.matrix, &Mtau.vector);
/* AKp currently contains A K_p; apply H^T from the left to get H^T A K_p */
gsl_linalg_QR_QTmat(&MQR.matrix, &Mtau.vector, &AKp.matrix);
/* the last npm rows correspond to H_q^T A K_p */
HqTAKp = gsl_matrix_submatrix(&AKp.matrix, pm, 0, npm, m);
/* solve: Xs R_p^T = H_q^T A K_p for Xs */
gsl_matrix_memcpy(Xs, &HqTAKp.matrix);
for (i = 0; i < npm; ++i)
{
gsl_vector_view x = gsl_matrix_row(Xs, i);
gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, &Rp.matrix, &x.vector);
}
/*
* compute: ys = H_q^T b; this is equivalent to computing
* the last q elements of H^T b (q = npm)
*/
v = gsl_vector_subvector(&b.vector, pm, npm);
gsl_linalg_QR_QTvec(&MQR.matrix, &Mtau.vector, &b.vector);
gsl_vector_memcpy(ys, &v.vector);
return GSL_SUCCESS;
}
}
}
int
gsl_linalg_hesstri_decomp(gsl_matrix * A, gsl_matrix * B, gsl_matrix * U,
gsl_matrix * V, gsl_vector * work)
{
const size_t N = A->size1;
if ((N != A->size2) || (N != B->size1) || (N != B->size2))
{
GSL_ERROR ("Hessenberg-triangular reduction requires square matrices",
GSL_ENOTSQR);
}
else if (N != work->size)
{
GSL_ERROR ("length of workspace must match matrix dimension",
GSL_EBADLEN);
}
else
{
double cs, sn; /* rotation parameters */
size_t i, j; /* looping */
gsl_vector_view xv, yv; /* temporary views */
/* B -> Q^T B = R (upper triangular) */
gsl_linalg_QR_decomp(B, work);
/* A -> Q^T A */
gsl_linalg_QR_QTmat(B, work, A);
/* initialize U and V if desired */
if (U)
{
gsl_linalg_QR_unpack(B, work, U, B);
}
else
{
/* zero out lower triangle of B */
for (j = 0; j < N - 1; ++j)
{
for (i = j + 1; i < N; ++i)
gsl_matrix_set(B, i, j, 0.0);
}
}
if (V)
gsl_matrix_set_identity(V);
if (N < 3)
return GSL_SUCCESS; /* nothing more to do */
/* reduce A and B */
for (j = 0; j < N - 2; ++j)
{
for (i = N - 1; i >= (j + 2); --i)
{
/* step 1: rotate rows i - 1, i to kill A(i,j) */
/*
* compute G = [ CS SN ] so that G^t [ A(i-1,j) ] = [ * ]
* [-SN CS ] [ A(i, j) ] [ 0 ]
*/
gsl_linalg_givens(gsl_matrix_get(A, i - 1, j),
gsl_matrix_get(A, i, j),
&cs,
&sn);
/* invert so drot() works correctly (G -> G^t) */
sn = -sn;
/* compute G^t A(i-1:i, j:n) */
xv = gsl_matrix_subrow(A, i - 1, j, N - j);
yv = gsl_matrix_subrow(A, i, j, N - j);
gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
/* compute G^t B(i-1:i, i-1:n) */
xv = gsl_matrix_subrow(B, i - 1, i - 1, N - i + 1);
yv = gsl_matrix_subrow(B, i, i - 1, N - i + 1);
gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
if (U)
{
/* accumulate U: U -> U G */
xv = gsl_matrix_column(U, i - 1);
yv = gsl_matrix_column(U, i);
gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
}
/* step 2: rotate columns i, i - 1 to kill B(i, i - 1) */
gsl_linalg_givens(-gsl_matrix_get(B, i, i),
gsl_matrix_get(B, i, i - 1),
&cs,
&sn);
/* invert so drot() works correctly (G -> G^t) */
sn = -sn;
/* compute B(1:i, i-1:i) G */
xv = gsl_matrix_subcolumn(B, i - 1, 0, i + 1);
yv = gsl_matrix_subcolumn(B, i, 0, i + 1);
gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
/* apply to A(1:n, i-1:i) */
xv = gsl_matrix_column(A, i - 1);
yv = gsl_matrix_column(A, i);
gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
if (V)
{
/* accumulate V: V -> V G */
xv = gsl_matrix_column(V, i - 1);
yv = gsl_matrix_column(V, i);
gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
}
}
}
return GSL_SUCCESS;
}
} /* gsl_linalg_hesstri_decomp() */
int
gsl_linalg_pcholesky_invert(const gsl_matrix * LDLT, const gsl_permutation * p,
gsl_matrix * Ainv)
{
const size_t M = LDLT->size1;
const size_t N = LDLT->size2;
if (M != N)
{
GSL_ERROR ("LDLT matrix must be square", GSL_ENOTSQR);
}
else if (LDLT->size1 != p->size)
{
GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
}
else if (Ainv->size1 != Ainv->size2)
{
GSL_ERROR ("Ainv matrix must be square", GSL_ENOTSQR);
}
else if (Ainv->size1 != M)
{
GSL_ERROR ("Ainv matrix has wrong dimensions", GSL_EBADLEN);
}
else
{
size_t i, j;
gsl_vector_view v1, v2;
/* invert the lower triangle of LDLT */
gsl_matrix_memcpy(Ainv, LDLT);
gsl_linalg_tri_lower_unit_invert(Ainv);
/* compute sqrt(D^{-1}) L^{-1} in the lower triangle of Ainv */
for (i = 0; i < N; ++i)
{
double di = gsl_matrix_get(LDLT, i, i);
double sqrt_di = sqrt(di);
for (j = 0; j < i; ++j)
{
double *Lij = gsl_matrix_ptr(Ainv, i, j);
*Lij /= sqrt_di;
}
gsl_matrix_set(Ainv, i, i, 1.0 / sqrt_di);
}
/*
* The lower triangle of Ainv now contains D^{-1/2} L^{-1}. Now compute
* A^{-1} = L^{-T} D^{-1} L^{-1}
*/
for (i = 0; i < N; ++i)
{
double aii = gsl_matrix_get(Ainv, i, i);
if (i < N - 1)
{
double tmp;
v1 = gsl_matrix_subcolumn(Ainv, i, i, N - i);
gsl_blas_ddot(&v1.vector, &v1.vector, &tmp);
gsl_matrix_set(Ainv, i, i, tmp);
if (i > 0)
{
gsl_matrix_view m = gsl_matrix_submatrix(Ainv, i + 1, 0, N - i - 1, i);
v1 = gsl_matrix_subcolumn(Ainv, i, i + 1, N - i - 1);
v2 = gsl_matrix_subrow(Ainv, i, 0, i);
gsl_blas_dgemv(CblasTrans, 1.0, &m.matrix, &v1.vector, aii, &v2.vector);
}
}
else
{
v1 = gsl_matrix_row(Ainv, N - 1);
gsl_blas_dscal(aii, &v1.vector);
}
}
/* copy lower triangle to upper */
gsl_matrix_transpose_tricpy('L', 0, Ainv, Ainv);
/* now apply permutation p to the matrix */
/* compute L^{-T} D^{-1} L^{-1} P^T */
for (i = 0; i < N; ++i)
{
v1 = gsl_matrix_row(Ainv, i);
gsl_permute_vector_inverse(p, &v1.vector);
}
/* compute P L^{-T} D^{-1} L^{-1} P^T */
for (i = 0; i < N; ++i)
{
v1 = gsl_matrix_column(Ainv, i);
gsl_permute_vector_inverse(p, &v1.vector);
}
return GSL_SUCCESS;
}
}
void updateR(gsl_matrix* R,double* factor,gsl_vector* p,gsl_vector* z, double* tau)
{
*factor = (*factor) / (1.0 - (*tau));
My_dscal(p,sqrt(GSL_MAX(*tau,- *tau)*(*factor)));
My_dscal(z,sqrt(GSL_MAX(*tau,- *tau)*(*factor)));
gsl_vector* w = gsl_vector_alloc(z->size); gsl_vector_memcpy(w, z);
gsl_vector* s = gsl_vector_calloc(w->size+1);
int n = w->size;
gsl_vector_set(s, n-1, gsl_vector_get(p, n-1)*gsl_vector_get(p, n-1));
for (int i=n-2; i>=0; i--) {
gsl_vector_set(s, i, gsl_vector_get(s, i+1)+gsl_vector_get(p, i)*gsl_vector_get(p, i));
}
double a = 1.0;
if (*tau < 0.0) {
a = -1.0;
}
double sigma = a/(1.0+sqrt(1.0+a*gsl_vector_get(s, 0)));
double q;
double theta;
double sigma1;
double beta;
double rho;
gsl_vector* d2 = gsl_vector_alloc(n);
gsl_vector_view d22 = gsl_matrix_diagonal(R);
gsl_vector_memcpy(d2, &d22.vector);
for (int j=0; j<n; j++) {
q = gsl_pow_2(gsl_vector_get(p, j));
theta = 1.0 + sigma * q;
gsl_vector_set(s, j+1, gsl_vector_get(s, j)-q);
rho = sqrt(theta*theta+sigma*sigma*q*gsl_vector_get(s, j+1));
beta = a * gsl_vector_get(p, j) * gsl_matrix_get(R, j, j);
gsl_matrix_set(R, j, j, rho * gsl_matrix_get(R, j, j));
beta = beta/gsl_matrix_get(R, j, j)/gsl_matrix_get(R, j, j);
a = a / rho/rho;
sigma1 = sigma* (1.0 + rho)/(rho*(theta + rho));
sigma = sigma1;
//for (int r = j+1; r<n; r++) {
// gsl_vector_set(w, r, gsl_vector_get(w, r)-gsl_vector_get(p, j)*gsl_matrix_get(R, r, j));
// gsl_matrix_set(R, r, j, gsl_matrix_get(R, r, j)/gsl_vector_get(d2, j)+beta*gsl_vector_get(w, r));
// gsl_matrix_set(R, r, j, gsl_matrix_get(R, r, j)*gsl_matrix_get(R, j, j));
//}
if (j<n-1) {
gsl_vector_view wr = gsl_vector_subvector(w, j+1, n-j-1);
gsl_vector_view Rr = gsl_matrix_subcolumn(R, j, j+1, n-j-1);
My_daxpy(&wr.vector, &Rr.vector, -gsl_vector_get(p, j));
My_dscal(&Rr.vector, 1.0/gsl_vector_get(d2, j));
My_daxpy(&Rr.vector, &wr.vector, beta);
My_dscal(&Rr.vector, gsl_matrix_get(R, j, j));
}
}
//clean up
gsl_vector_free(w);
gsl_vector_free(s);
gsl_vector_free(d2);
}
void fnIMIS(const size_t InitSamples, const size_t StepSamples, const size_t FinalResamples, const size_t MaxIter, const size_t NumParam, unsigned long int rng_seed, const char * runName)
{
// Declare and configure GSL RNG
gsl_rng * rng;
const gsl_rng_type * T;
gsl_rng_env_setup();
T = gsl_rng_default;
rng = gsl_rng_alloc (T);
gsl_rng_set(rng, rng_seed);
char strDiagnosticsFile[strlen(runName) + 15 +1];
char strResampleFile[strlen(runName) + 12 +1];
strcpy(strDiagnosticsFile, runName); strcat(strDiagnosticsFile, "Diagnostics.txt");
strcpy(strResampleFile, runName); strcat(strResampleFile, "Resample.txt");
FILE * diagnostics_file = fopen(strDiagnosticsFile, "w");
fprintf(diagnostics_file, "Seeded RNG: %zu\n", rng_seed);
fprintf(diagnostics_file, "Running IMIS. InitSamples: %zu, StepSamples: %zu, FinalResamples %zu, MaxIter %zu\n", InitSamples, StepSamples, FinalResamples, MaxIter);
// Setup IMIS arrays
gsl_matrix * Xmat = gsl_matrix_alloc(InitSamples + StepSamples*MaxIter, NumParam);
double * prior_all = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));
double * likelihood_all = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));
double * imp_weight_denom = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter)); // proportional to q(k) in stage 2c of Raftery & Bao
double * gaussian_sum = (double*) calloc(InitSamples + StepSamples*MaxIter, sizeof(double)); // sum of mixture distribution for mode
struct dst * distance = (struct dst *) malloc(sizeof(struct dst) * (InitSamples + StepSamples*MaxIter)); // Mahalanobis distance to most recent mode
double * imp_weights = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));
double * tmp_MVNpdf = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));
gsl_matrix * nearestX = gsl_matrix_alloc(StepSamples, NumParam);
double center_all[MaxIter][NumParam];
gsl_matrix * sigmaChol_all[MaxIter];
gsl_matrix * sigmaInv_all[MaxIter];
// Initial prior samples
sample_prior(rng, InitSamples, Xmat);
// Calculate prior covariance
double prior_invCov_diag[NumParam];
/*
The paper describing the algorithm uses the full prior covariance matrix.
This follows the code in the IMIS R package and diagonalizes the prior
covariance matrix to ensure invertibility.
*/
for(size_t i = 0; i < NumParam; i++){
gsl_vector_view tmpCol = gsl_matrix_subcolumn(Xmat, i, 0, InitSamples);
prior_invCov_diag[i] = gsl_stats_variance(tmpCol.vector.data, tmpCol.vector.stride, InitSamples);
prior_invCov_diag[i] = 1.0/prior_invCov_diag[i];
}
// IMIS steps
fprintf(diagnostics_file, "Step Var(w_i) MargLik Unique Max(w_i) ESS Time\n");
printf("Step Var(w_i) MargLik Unique Max(w_i) ESS Time\n");
time_t time1, time2;
time(&time1);
size_t imisStep = 0, numImisSamples;
for(imisStep = 0; imisStep < MaxIter; imisStep++){
numImisSamples = (InitSamples + imisStep*StepSamples);
// Evaluate prior and likelihood
if(imisStep == 0){ // initial stage
#pragma omp parallel for
for(size_t i = 0; i < numImisSamples; i++){
gsl_vector_const_view theta = gsl_matrix_const_row(Xmat, i);
prior_all[i] = prior(&theta.vector);
likelihood_all[i] = likelihood(&theta.vector);
}
} else { // imisStep > 0
#pragma omp parallel for
for(size_t i = InitSamples + (imisStep-1)*StepSamples; i < numImisSamples; i++){
gsl_vector_const_view theta = gsl_matrix_const_row(Xmat, i);
prior_all[i] = prior(&theta.vector);
likelihood_all[i] = likelihood(&theta.vector);
}
}
// Determine importance weights, find current maximum, calculate monitoring criteria
#pragma omp parallel for
for(size_t i = 0; i < numImisSamples; i++){
imp_weight_denom[i] = (InitSamples*prior_all[i] + StepSamples*gaussian_sum[i])/(InitSamples + StepSamples * imisStep);
imp_weights[i] = (prior_all[i] > 0)?likelihood_all[i]*prior_all[i]/imp_weight_denom[i]:0;
}
double sumWeights = 0.0;
for(size_t i = 0; i < numImisSamples; i++){
sumWeights += imp_weights[i];
}
double maxWeight = 0.0, varImpW = 0.0, entropy = 0.0, expectedUnique = 0.0, effSampSize = 0.0, margLik;
size_t maxW_idx;
#pragma omp parallel for reduction(+: varImpW, entropy, expectedUnique, effSampSize)
for(size_t i = 0; i < numImisSamples; i++){
imp_weights[i] /= sumWeights;
varImpW += pow(numImisSamples * imp_weights[i] - 1.0, 2.0);
entropy += imp_weights[i] * log(imp_weights[i]);
expectedUnique += (1.0 - pow((1.0 - imp_weights[i]), FinalResamples));
effSampSize += pow(imp_weights[i], 2.0);
}
for(size_t i = 0; i < numImisSamples; i++){
if(imp_weights[i] > maxWeight){
maxW_idx = i;
maxWeight = imp_weights[i];
}
}
for(size_t i = 0; i < NumParam; i++)
center_all[imisStep][i] = gsl_matrix_get(Xmat, maxW_idx, i);
varImpW /= numImisSamples;
entropy = -entropy / log(numImisSamples);
effSampSize = 1.0/effSampSize;
margLik = log(sumWeights/numImisSamples);
fprintf(diagnostics_file, "%4zu %8.2f %8.2f %8.2f %8.2f %8.2f %8.2f\n", imisStep, varImpW, margLik, expectedUnique, maxWeight, effSampSize, difftime(time(&time2), time1));
printf("%4zu %8.2f %8.2f %8.2f %8.2f %8.2f %8.2f\n", imisStep, varImpW, margLik, expectedUnique, maxWeight, effSampSize, difftime(time(&time2), time1));
time1 = time2;
// Check for convergence
if(expectedUnique > FinalResamples*(1.0 - exp(-1.0))){
break;
}
// Calculate Mahalanobis distance to current mode
GetMahalanobis_diag(Xmat, center_all[imisStep], prior_invCov_diag, numImisSamples, NumParam, distance);
// Find StepSamples nearest points
// (Note: this was a major bottleneck when InitSamples and StepResamples are large. qsort substantially outperformed GSL sort options.)
qsort(distance, numImisSamples, sizeof(struct dst), cmp_dst);
#pragma omp parallel for
for(size_t i = 0; i < StepSamples; i++){
gsl_vector_const_view tmpX = gsl_matrix_const_row(Xmat, distance[i].idx);
gsl_matrix_set_row(nearestX, i, &tmpX.vector);
}
// Calculate weighted covariance of nearestX
// (a) Calculate weights for nearest points 1...StepSamples
double weightsCov[StepSamples];
#pragma omp parallel for
for(size_t i = 0; i < StepSamples; i++){
weightsCov[i] = 0.5*(imp_weights[distance[i].idx] + 1.0/numImisSamples); // cov_wt function will normalize the weights
}
// (b) Calculate weighted covariance
sigmaChol_all[imisStep] = gsl_matrix_alloc(NumParam, NumParam);
covariance_weighted(nearestX, weightsCov, StepSamples, center_all[imisStep], NumParam, sigmaChol_all[imisStep]);
// (c) Do Cholesky decomposition and inverse of covariance matrix
gsl_linalg_cholesky_decomp(sigmaChol_all[imisStep]);
for(size_t j = 0; j < NumParam; j++) // Note: GSL outputs a symmetric matrix rather than lower tri, so have to set upper tri to zero
for(size_t k = j+1; k < NumParam; k++)
gsl_matrix_set(sigmaChol_all[imisStep], j, k, 0.0);
sigmaInv_all[imisStep] = gsl_matrix_alloc(NumParam, NumParam);
gsl_matrix_memcpy(sigmaInv_all[imisStep], sigmaChol_all[imisStep]);
gsl_linalg_cholesky_invert(sigmaInv_all[imisStep]);
// Sample new inputs
gsl_matrix_view newSamples = gsl_matrix_submatrix(Xmat, numImisSamples, 0, StepSamples, NumParam);
GenerateRandMVnorm(rng, StepSamples, center_all[imisStep], sigmaChol_all[imisStep], NumParam, &newSamples.matrix);
// Evaluate sampling probability from mixture distribution
// (a) For newly sampled points, sum over all previous centers
for(size_t pastStep = 0; pastStep < imisStep; pastStep++){
GetMVNpdf(&newSamples.matrix, center_all[pastStep], sigmaInv_all[pastStep], sigmaChol_all[pastStep], StepSamples, NumParam, tmp_MVNpdf);
#pragma omp parallel for
for(size_t i = 0; i < StepSamples; i++)
gaussian_sum[numImisSamples + i] += tmp_MVNpdf[i];
}
// (b) For all points, add weight for most recent center
gsl_matrix_const_view Xmat_curr = gsl_matrix_const_submatrix(Xmat, 0, 0, numImisSamples + StepSamples, NumParam);
GetMVNpdf(&Xmat_curr.matrix, center_all[imisStep], sigmaInv_all[imisStep], sigmaChol_all[imisStep], numImisSamples + StepSamples, NumParam, tmp_MVNpdf);
#pragma omp parallel for
for(size_t i = 0; i < numImisSamples + StepSamples; i++)
gaussian_sum[i] += tmp_MVNpdf[i];
} // loop over imisStep
//// FINISHED IMIS ROUTINE
fclose(diagnostics_file);
// Resample posterior outputs
int resampleIdx[FinalResamples];
walker_ProbSampleReplace(rng, numImisSamples, imp_weights, FinalResamples, resampleIdx); // Note: Random sampling routine used in R sample() function.
// Print results
FILE * resample_file = fopen(strResampleFile, "w");
for(size_t i = 0; i < FinalResamples; i++){
for(size_t j = 0; j < NumParam; j++)
fprintf(resample_file, "%.15e\t", gsl_matrix_get(Xmat, resampleIdx[i], j));
gsl_vector_const_view theta = gsl_matrix_const_row(Xmat, resampleIdx[i]);
fprintf(resample_file, "\n");
}
fclose(resample_file);
/*
// This outputs Xmat (parameter matrix), centers, and covariance matrices to files for debugging
FILE * Xmat_file = fopen("Xmat.txt", "w");
for(size_t i = 0; i < numImisSamples; i++){
for(size_t j = 0; j < NumParam; j++)
fprintf(Xmat_file, "%.15e\t", gsl_matrix_get(Xmat, i, j));
fprintf(Xmat_file, "%e\t%e\t%e\t%e\t%e\t\n", prior_all[i], likelihood_all[i], imp_weights[i], gaussian_sum[i], distance[i]);
}
fclose(Xmat_file);
FILE * centers_file = fopen("centers.txt", "w");
for(size_t i = 0; i < imisStep; i++){
for(size_t j = 0; j < NumParam; j++)
fprintf(centers_file, "%f\t", center_all[i][j]);
fprintf(centers_file, "\n");
}
fclose(centers_file);
FILE * sigmaInv_file = fopen("sigmaInv.txt", "w");
for(size_t i = 0; i < imisStep; i++){
for(size_t j = 0; j < NumParam; j++)
for(size_t k = 0; k < NumParam; k++)
fprintf(sigmaInv_file, "%f\t", gsl_matrix_get(sigmaInv_all[i], j, k));
fprintf(sigmaInv_file, "\n");
}
fclose(sigmaInv_file);
*/
// free memory allocated by IMIS
for(size_t i = 0; i < imisStep; i++){
gsl_matrix_free(sigmaChol_all[i]);
gsl_matrix_free(sigmaInv_all[i]);
}
// release RNG
gsl_rng_free(rng);
gsl_matrix_free(Xmat);
gsl_matrix_free(nearestX);
free(prior_all);
free(likelihood_all);
free(imp_weight_denom);
free(gaussian_sum);
free(distance);
free(imp_weights);
free(tmp_MVNpdf);
return;
}
