static int
fdfridge_f(const gsl_vector * x, void * params, gsl_vector * f)
{
int status;
gsl_multifit_fdfridge *w = (gsl_multifit_fdfridge *) params;
const size_t n = w->n;
const size_t p = w->p;
gsl_vector_view f_user = gsl_vector_subvector(f, 0, n);
gsl_vector_view f_tik = gsl_vector_subvector(f, n, p);
/* call user callback function to get residual vector f */
status = gsl_multifit_eval_wf(w->fdf, x, NULL, &f_user.vector);
if (status)
return status;
if (w->L_diag)
{
/* store diag(L_diag) x in Tikhonov portion of f~ */
gsl_vector_memcpy(&f_tik.vector, x);
gsl_vector_mul(&f_tik.vector, w->L_diag);
}
else if (w->L)
{
/* store Lx in Tikhonov portion of f~ */
gsl_blas_dgemv(CblasNoTrans, 1.0, w->L, x, 0.0, &f_tik.vector);
}
else
{
/* store \lambda x in Tikhonov portion of f~ */
gsl_vector_memcpy(&f_tik.vector, x);
gsl_vector_scale(&f_tik.vector, w->lambda);
}
return GSL_SUCCESS;
} /* fdfridge_f() */
ambisonicWeight::ambisonicWeight(int anOrder, int aVectorSize, std::string anOptimMode)
{
m_order = anOrder;
m_number_of_harmonics = m_order * 2 + 1;
m_number_of_outputs = m_number_of_harmonics;
m_number_of_inputs = m_number_of_harmonics;
m_vector_size = aVectorSize;
m_speakers_angles = new double[m_order];
m_index_of_harmonics = new int[m_number_of_harmonics];
m_optimVector = new double[m_number_of_harmonics];
m_input_vector = gsl_vector_alloc(m_number_of_harmonics * m_number_of_harmonics);
m_input_vector_view = new gsl_vector_view[m_number_of_harmonics];
m_output_vector = gsl_vector_alloc(m_number_of_harmonics * m_number_of_harmonics);
m_output_vector_view = new gsl_vector_view[m_number_of_harmonics];
gsl_vector_set_zero(m_input_vector);
gsl_vector_set_zero(m_output_vector);
for (int j = 0; j < m_number_of_harmonics; j++)
{
m_input_vector_view[j] = gsl_vector_subvector(m_input_vector, j * m_number_of_harmonics, m_number_of_harmonics);
m_output_vector_view[j] = gsl_vector_subvector(m_output_vector, j * m_number_of_harmonics, m_number_of_harmonics);
}
computeIndex();
computeAngles();
computePseudoInverse();
setOptimMode(anOptimMode);
}
static double
robust_madsigma(const gsl_vector *r, gsl_multifit_robust_workspace *w)
{
size_t n = r->size;
const size_t p = w->p;
double sigma;
size_t i;
/* allow for the possibility that r->size < w->n */
gsl_vector_view v1 = gsl_vector_subvector(w->workn, 0, n);
gsl_vector_view v2;
/* copy |r| into workn */
for (i = 0; i < n; ++i)
{
gsl_vector_set(&v1.vector, i, fabs(gsl_vector_get(r, i)));
}
gsl_sort_vector(&v1.vector);
/*
* ignore the smallest p residuals when computing the median
* (see Street et al 1988)
*/
v2 = gsl_vector_subvector(&v1.vector, p - 1, n - p + 1);
sigma = gsl_stats_median_from_sorted_data(v2.vector.data, v2.vector.stride, v2.vector.size) / 0.6745;
return sigma;
} /* robust_madsigma() */
GbA::GbA(int nb, int ns){
_nbands = nb;
_nsim = ns;
_status[TData::vertical] = false;
_status[TData::horizontal] = false;
_mags = gsl_vector_calloc(2*_nsim);
_dists = gsl_vector_calloc(2*_nsim);
_m[TData::vertical] = gsl_vector_subvector(_mags,0,_nsim);
_m[TData::horizontal] = gsl_vector_subvector(_mags,_nsim,_nsim);
_r[TData::vertical] = gsl_vector_subvector(_dists,0,_nsim);
_r[TData::horizontal] = gsl_vector_subvector(_dists,_nsim,_nsim);
pthread_mutex_init(&_process_lock,NULL);
/* Note that the magnitude and logarithmic distance samples are
* initially set to M=[2.0, 2.2, ..., 8.0] and R to [0.0, 0.1, ...,2.0]
*/
_nms = 31;
_nrs = 21;
_msamples = new double[_nms];
_rsamples = new double[_nrs];
for(int i=0;i<_nms;i++)
_msamples[i] = 2.0 + 0.2*i;
for(int i=0;i<_nrs;i++)
_rsamples[i] = 0.0 + 0.1*i;
};
int
gsl_multifit_wlinear (const gsl_matrix * X,
const gsl_vector * w,
const gsl_vector * y,
gsl_vector * c,
gsl_matrix * cov,
double *chisq, gsl_multifit_linear_workspace * work)
{
int status;
size_t rank = 0;
double rnorm, snorm;
gsl_vector_view b = gsl_vector_subvector(work->t, 0, y->size);
/* compute A = sqrt(W) X, b = sqrt(W) y */
status = gsl_multifit_linear_applyW(X, w, y, work->A, &b.vector);
if (status)
return status;
/* compute SVD of A */
status = gsl_multifit_linear_bsvd(work->A, work);
if (status)
return status;
status = multifit_linear_solve(X, &b.vector, GSL_DBL_EPSILON, 0.0, &rank,
c, &rnorm, &snorm, work);
if (status)
return status;
*chisq = rnorm * rnorm;
/* variance-covariance matrix cov = s2 * (Q S^-1) (Q S^-1)^T */
{
const size_t p = X->size2;
size_t i, j;
gsl_matrix_view QSI = gsl_matrix_submatrix(work->QSI, 0, 0, p, p);
gsl_vector_view D = gsl_vector_subvector(work->D, 0, p);
for (i = 0; i < p; i++)
{
gsl_vector_view row_i = gsl_matrix_row (&QSI.matrix, i);
double d_i = gsl_vector_get (&D.vector, i);
for (j = i; j < p; j++)
{
gsl_vector_view row_j = gsl_matrix_row (&QSI.matrix, j);
double d_j = gsl_vector_get (&D.vector, j);
double s;
gsl_blas_ddot (&row_i.vector, &row_j.vector, &s);
gsl_matrix_set (cov, i, j, s / (d_i * d_j));
gsl_matrix_set (cov, j, i, s / (d_i * d_j));
}
}
}
return GSL_SUCCESS;
}
int
gsl_linalg_COD_lssolve (const gsl_matrix * QRZ, const gsl_vector * tau_Q, const gsl_vector * tau_Z,
const gsl_permutation * perm, const size_t rank, const gsl_vector * b,
gsl_vector * x, gsl_vector * residual)
{
const size_t M = QRZ->size1;
const size_t N = QRZ->size2;
if (M < N)
{
GSL_ERROR ("QRZ matrix must have M>=N", GSL_EBADLEN);
}
else if (M != b->size)
{
GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
}
else if (rank > GSL_MIN (M, N))
{
GSL_ERROR ("rank must be <= MIN(M,N)", GSL_EBADLEN);
}
else if (N != x->size)
{
GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
}
else if (M != residual->size)
{
GSL_ERROR ("matrix size must match residual size", GSL_EBADLEN);
}
else
{
gsl_matrix_const_view R11 = gsl_matrix_const_submatrix (QRZ, 0, 0, rank, rank);
gsl_vector_view QTb1 = gsl_vector_subvector(residual, 0, rank);
gsl_vector_view x1 = gsl_vector_subvector(x, 0, rank);
gsl_vector_set_zero(x);
/* compute residual = Q^T b */
gsl_vector_memcpy(residual, b);
gsl_linalg_QR_QTvec (QRZ, tau_Q, residual);
/* solve x1 := R11^{-1} (Q^T b)(1:r) */
gsl_vector_memcpy(&(x1.vector), &(QTb1.vector));
gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, &(R11.matrix), &(x1.vector));
/* compute Z^T ( R11^{-1} x1; 0 ) */
cod_householder_ZTvec(QRZ, tau_Z, rank, x);
/* compute x = P Z^T ( R11^{-1} x1; 0 ) */
gsl_permute_vector_inverse(perm, x);
/* compute residual = b - A x = Q (Q^T b - R [ R11^{-1} x1; 0 ]) */
gsl_vector_set_zero(&(QTb1.vector));
gsl_linalg_QR_Qvec(QRZ, tau_Q, residual);
return GSL_SUCCESS;
}
}
int
gsl_linalg_symmtd_decomp (gsl_matrix * A, gsl_vector * tau)
{
if (A->size1 != A->size2)
{
GSL_ERROR ("symmetric tridiagonal decomposition requires square matrix",
GSL_ENOTSQR);
}
else if (tau->size + 1 != A->size1)
{
GSL_ERROR ("size of tau must be (matrix size - 1)", GSL_EBADLEN);
}
else
{
const size_t N = A->size1;
size_t i;
for (i = 0 ; i < N - 2; i++)
{
gsl_vector_view c = gsl_matrix_column (A, i);
gsl_vector_view v = gsl_vector_subvector (&c.vector, i + 1, N - (i + 1));
double tau_i = gsl_linalg_householder_transform (&v.vector);
/* Apply the transformation H^T A H to the remaining columns */
if (tau_i != 0.0)
{
gsl_matrix_view m = gsl_matrix_submatrix (A, i + 1, i + 1,
N - (i+1), N - (i+1));
double ei = gsl_vector_get(&v.vector, 0);
gsl_vector_view x = gsl_vector_subvector (tau, i, N-(i+1));
gsl_vector_set (&v.vector, 0, 1.0);
/* x = tau * A * v */
gsl_blas_dsymv (CblasLower, tau_i, &m.matrix, &v.vector, 0.0, &x.vector);
/* w = x - (1/2) tau * (x' * v) * v */
{
double xv, alpha;
gsl_blas_ddot(&x.vector, &v.vector, &xv);
alpha = - (tau_i / 2.0) * xv;
gsl_blas_daxpy(alpha, &v.vector, &x.vector);
}
/* apply the transformation A = A - v w' - w v' */
gsl_blas_dsyr2(CblasLower, -1.0, &v.vector, &x.vector, &m.matrix);
gsl_vector_set (&v.vector, 0, ei);
}
gsl_vector_set (tau, i, tau_i);
}
return GSL_SUCCESS;
}
}
int
gsl_linalg_QRPT_lssolve2 (const gsl_matrix * QR, const gsl_vector * tau, const gsl_permutation * p,
const gsl_vector * b, const size_t rank, gsl_vector * x, gsl_vector * residual)
{
const size_t M = QR->size1;
const size_t N = QR->size2;
if (M < N)
{
GSL_ERROR ("QR matrix must have M>=N", GSL_EBADLEN);
}
else if (M != b->size)
{
GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
}
else if (rank == 0 || rank > N)
{
GSL_ERROR ("rank must have 0 < rank <= N", GSL_EBADLEN);
}
else if (N != x->size)
{
GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
}
else if (M != residual->size)
{
GSL_ERROR ("matrix size must match residual size", GSL_EBADLEN);
}
else
{
gsl_matrix_const_view R11 = gsl_matrix_const_submatrix (QR, 0, 0, rank, rank);
gsl_vector_view QTb1 = gsl_vector_subvector(residual, 0, rank);
gsl_vector_view x1 = gsl_vector_subvector(x, 0, rank);
size_t i;
/* compute work = Q^T b */
gsl_vector_memcpy(residual, b);
gsl_linalg_QR_QTvec (QR, tau, residual);
/* solve R_{11} x(1:r) = [Q^T b](1:r) */
gsl_vector_memcpy(&(x1.vector), &(QTb1.vector));
gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, &(R11.matrix), &(x1.vector));
/* x(r+1:N) = 0 */
for (i = rank; i < N; ++i)
gsl_vector_set(x, i, 0.0);
/* compute x = P y */
gsl_permute_vector_inverse (p, x);
/* compute residual = b - A x = Q (Q^T b - R x) */
gsl_vector_set_zero(&(QTb1.vector));
gsl_linalg_QR_Qvec(QR, tau, residual);
return GSL_SUCCESS;
}
}
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;
}
}
int lsQRPT(gsl_matrix * A, gsl_vector * b, gsl_vector * x, double * sigma)
{
int i;
gsl_vector *tau, *res;
gsl_permutation *p;
gsl_vector_view norm;
if (A->size1 < A->size2) return -1;
if (A->size1 != b->size) return -1;
if (A->size2 != x->size) return -1;
tau = gsl_vector_alloc(x->size);
res = gsl_vector_alloc(b->size);
p = gsl_permutation_alloc(x->size);
norm = gsl_vector_subvector(res, 0, x->size);
gsl_linalg_QRPT_decomp(A, tau, p, &i, &norm.vector);
gsl_linalg_QR_lssolve(A, tau, b, x, res);
gsl_permute_vector_inverse(p, x);
*sigma = gsl_blas_dnrm2(res);
gsl_vector_free(tau);
gsl_vector_free(res);
gsl_permutation_free(p);
return 0;
}
int
gsl_linalg_QR_Qvec (const gsl_matrix * QR, const gsl_vector * tau, gsl_vector * v)
{
const size_t M = QR->size1;
const size_t N = QR->size2;
if (tau->size != GSL_MIN (M, N))
{
GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
}
else if (v->size != M)
{
GSL_ERROR ("vector size must be M", GSL_EBADLEN);
}
else
{
size_t i;
/* compute Q^T v */
for (i = GSL_MIN (M, N); i-- > 0;)
{
gsl_vector_const_view c = gsl_matrix_const_column (QR, i);
gsl_vector_const_view h = gsl_vector_const_subvector (&(c.vector),
i, M - i);
gsl_vector_view w = gsl_vector_subvector (v, i, M - i);
double ti = gsl_vector_get (tau, i);
gsl_linalg_householder_hv (ti, &h.vector, &w.vector);
}
return GSL_SUCCESS;
}
}
double mcmclib_mcar_model_phi_fcond(mcmclib_mcar_model* in_p, size_t i, gsl_vector* x) {
mcmclib_mcar_tilde_lpdf* lpdf = in_p->lpdf;
const size_t p = lpdf->p;
if(x->size != p) {
static char msg[1024];
sprintf(msg, "'x' vector size is %zd, it should be %zd", x->size, p);
GSL_ERROR(msg, GSL_FAILURE);
}
assert(x->size == p);
gsl_matrix* W = lpdf->M;
const double mi = 1.0 / gsl_vector_get(lpdf->m, i);
gsl_matrix* Lambdaij = lpdf->Lambda_ij;
gsl_vector* mean = gsl_vector_alloc(p);
gsl_vector_set_zero(mean);
for(size_t j=0; j < lpdf->n; j++) {
if(gsl_matrix_get(W, i, j) == 1.0) {
gsl_vector_view phij_v = gsl_vector_subvector(in_p->e, j*p, p);
gsl_blas_dgemv(CblasNoTrans, mi, Lambdaij, &phij_v.vector, 1.0, mean);
}
}
gsl_matrix* Gammai = lpdf->Gammai;
gsl_matrix_memcpy(Gammai, lpdf->Gamma);
gsl_matrix_scale(Gammai, mi);
mcmclib_mvnorm_lpdf* tmp = mcmclib_mvnorm_lpdf_alloc(mean, Gammai->data);
double ans = mcmclib_mvnorm_lpdf_compute(tmp, x);
gsl_vector_free(mean);
mcmclib_mvnorm_lpdf_free(tmp);
return ans;
}
static int
cod_householder_ZTvec(const gsl_matrix * QRZ, const gsl_vector * tau_Z, const size_t rank,
gsl_vector * v)
{
const size_t M = QRZ->size1;
const size_t N = QRZ->size2;
if (tau_Z->size != GSL_MIN (M, N))
{
GSL_ERROR("tau_Z must be GSL_MIN(M,N)", GSL_EBADLEN);
}
else if (v->size != N)
{
GSL_ERROR("v must be length N", GSL_EBADLEN);
}
else
{
if (rank < N)
{
size_t i;
for (i = 0; i < rank; ++i)
{
gsl_vector_const_view h = gsl_matrix_const_subrow (QRZ, i, rank, N - rank);
gsl_vector_view w = gsl_vector_subvector (v, i, N - i);
double ti = gsl_vector_get (tau_Z, i);
cod_householder_hv(ti, &h.vector, &w.vector);
}
}
return GSL_SUCCESS;
}
}
static int
cod_householder_hv(const double tau, const gsl_vector * v, gsl_vector * w)
{
if (tau == 0)
{
return GSL_SUCCESS; /* H = I */
}
else
{
const size_t M = w->size;
const size_t L = v->size;
double w0 = gsl_vector_get(w, 0);
gsl_vector_view w1 = gsl_vector_subvector(w, M - L, L);
double d1, d;
/* d1 := v . w(M-L:M) */
gsl_blas_ddot(v, &w1.vector, &d1);
/* d := w(1) + v . w(M-L:M) */
d = w0 + d1;
/* w(1) = w(1) - tau * d */
gsl_vector_set(w, 0, w0 - tau * d);
/* w(M-L:M) = w(M-L:M) - tau * d * v */
gsl_blas_daxpy(-tau * d, v, &w1.vector);
return GSL_SUCCESS;
}
}
int
gsl_linalg_LQ_vecQ (const gsl_matrix * LQ, const gsl_vector * tau, gsl_vector * v)
{
const size_t N = LQ->size1;
const size_t M = LQ->size2;
if (tau->size != GSL_MIN (M, N))
{
GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
}
else if (v->size != M)
{
GSL_ERROR ("vector size must be M", GSL_EBADLEN);
}
else
{
size_t i;
/* compute v Q^T */
for (i = GSL_MIN (M, N); i-- > 0;)
{
gsl_vector_const_view c = gsl_matrix_const_row (LQ, i);
gsl_vector_const_view h = gsl_vector_const_subvector (&(c.vector),
i, M - i);
gsl_vector_view w = gsl_vector_subvector (v, i, M - i);
double ti = gsl_vector_get (tau, i);
gsl_linalg_householder_hv (ti, &(h.vector), &(w.vector));
}
return GSL_SUCCESS;
}
}
int
gsl_linalg_householder_hv (double tau, const gsl_vector * v, gsl_vector * w)
{
/* applies a householder transformation v to vector w */
const size_t N = v->size;
if (tau == 0)
return GSL_SUCCESS ;
{
/* compute d = v'w */
double d0 = gsl_vector_get(w,0);
double d1, d;
gsl_vector_const_view v1 = gsl_vector_const_subvector(v, 1, N-1);
gsl_vector_view w1 = gsl_vector_subvector(w, 1, N-1);
gsl_blas_ddot (&v1.vector, &w1.vector, &d1);
d = d0 + d1;
/* compute w = w - tau (v) (v'w) */
{
double w0 = gsl_vector_get (w,0);
gsl_vector_set (w, 0, w0 - tau * d);
}
gsl_blas_daxpy (-tau * d, &v1.vector, &w1.vector);
}
return GSL_SUCCESS;
}
/*
* 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 ----- */
int
gsl_multifit_fdfridge_wset (gsl_multifit_fdfridge * w,
gsl_multifit_function_fdf * f,
const gsl_vector * x,
const double lambda,
const gsl_vector * wts)
{
const size_t n = w->n;
const size_t p = w->p;
if (n != f->n || p != f->p)
{
GSL_ERROR ("function size does not match solver", GSL_EBADLEN);
}
else if (p != x->size)
{
GSL_ERROR ("vector length does not match solver", GSL_EBADLEN);
}
else if (wts != NULL && n != wts->size)
{
GSL_ERROR ("weight vector length does not match solver", GSL_EBADLEN);
}
else
{
int status;
gsl_vector_view wv = gsl_vector_subvector(w->wts, 0, n);
/* save user defined fdf */
w->fdf = f;
/* build modified fdf for Tikhonov terms */
w->fdftik.f = &fdfridge_f;
w->fdftik.df = &fdfridge_df;
w->fdftik.n = n + p; /* add p for Tikhonov terms */
w->fdftik.p = p;
w->fdftik.params = (void *) w;
/* store damping parameter */
w->lambda = lambda;
w->L = NULL;
if (wts)
{
/* copy weight vector into user portion of w->wts */
gsl_vector_memcpy(&wv.vector, wts);
status = gsl_multifit_fdfsolver_wset(w->s, &(w->fdftik), x, w->wts);
}
else
{
status = gsl_multifit_fdfsolver_wset(w->s, &(w->fdftik), x, NULL);
}
/* update function/Jacobian evaluations */
f->nevalf = w->fdftik.nevalf;
f->nevaldf = w->fdftik.nevaldf;
return status;
}
} /* gsl_multifit_fdfridge_wset() */
Vector Vector::subVector ( const size_t dim, const size_t dims ) {
// Circumvent GSL limitation to allow 0-dimensional vectors
if ( 0 == dims ) {
return Vector( dims, vector.data + dim, 1u );
} else {
return Vector( gsl_vector_subvector( &vector, dim, dims ) );
}
}
int
gsl_linalg_COD_decomp_e(gsl_matrix * A, gsl_vector * tau_Q, gsl_vector * tau_Z,
gsl_permutation * p, double tol, size_t * rank, gsl_vector * work)
{
const size_t M = A->size1;
const size_t N = A->size2;
if (tau_Q->size != GSL_MIN (M, N))
{
GSL_ERROR ("size of tau_Q must be MIN(M,N)", GSL_EBADLEN);
}
else if (tau_Z->size != GSL_MIN (M, N))
{
GSL_ERROR ("size of tau_Z must be MIN(M,N)", GSL_EBADLEN);
}
else if (p->size != N)
{
GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
}
else if (work->size != N)
{
GSL_ERROR ("work size must be N", GSL_EBADLEN);
}
else
{
int status, signum;
size_t r;
/* decompose: A P = Q R */
status = gsl_linalg_QRPT_decomp(A, tau_Q, p, &signum, work);
if (status)
return status;
/* estimate rank of A */
r = gsl_linalg_QRPT_rank(A, tol);
if (r < N)
{
/*
* matrix is rank-deficient, so that the R factor is
*
* R = [ R11 R12 ] =~ [ R11 R12 ]
* [ 0 R22 ] [ 0 0 ]
*
* compute RZ decomposition of upper trapezoidal matrix
* [ R11 R12 ] = [ R11~ 0 ] Z
*/
gsl_matrix_view R_upper = gsl_matrix_submatrix(A, 0, 0, r, N);
gsl_vector_view t = gsl_vector_subvector(tau_Z, 0, r);
cod_RZ(&R_upper.matrix, &t.vector);
}
*rank = r;
return GSL_SUCCESS;
}
}
ssm_err_code_t ssm_kalman_update(ssm_fitness_t *fitness, ssm_X_t *X, ssm_row_t *row, double t, ssm_par_t *par, ssm_calc_t *calc, ssm_nav_t *nav)
{
int status;
int m = nav->states_sv_inc->length + nav->states_diff->length;
gsl_vector_view pred_error = gsl_vector_subvector(calc->_pred_error,0,row->ts_nonan_length);
gsl_vector_view zero = gsl_vector_subvector(calc->_zero,0,row->ts_nonan_length);
gsl_matrix_view Kt = gsl_matrix_submatrix(calc->_Kt,0,0,m,row->ts_nonan_length);
gsl_matrix_view Tmp = gsl_matrix_submatrix(calc->_Tmp_N_TS_N_SV,0,0,row->ts_nonan_length,m);
gsl_vector_view X_sv = gsl_vector_view_array(X->proj,m);
gsl_matrix_view Ht = gsl_matrix_submatrix(calc->_Ht,0,0,m,row->ts_nonan_length);
gsl_matrix_view Ct = gsl_matrix_view_array(&X->proj[m], m, m);
gsl_matrix_view St = gsl_matrix_submatrix(calc->_St,0,0,row->ts_nonan_length,row->ts_nonan_length);
ssm_err_code_t cum_status = ssm_kalman_gain_computation(row, t, X, par, calc, nav);
//////////////////
// state update //
//////////////////
// X_sv += Kt * pred_error
status = gsl_blas_dgemv(CblasNoTrans,1.0,&Kt.matrix,&pred_error.vector,1.0,&X_sv.vector);
cum_status |= (status != GSL_SUCCESS) ? SSM_ERR_KAL : SSM_SUCCESS;
///////////////////////
// covariance update //
///////////////////////
// Ct = Ct - Kt * Ht' * Ct
status = gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, &Ht.matrix, &Ct.matrix, 0.0, &Tmp.matrix);
cum_status |= (status != GSL_SUCCESS) ? SSM_ERR_KAL : SSM_SUCCESS;
status = gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &Kt.matrix, &Tmp.matrix, 1.0, &Ct.matrix);
cum_status |= (status != GSL_SUCCESS) ? SSM_ERR_KAL : SSM_SUCCESS;
// positivity and symmetry could have been lost when updating Ct
cum_status |= _ssm_check_and_correct_Ct(X, calc, nav);
// positivity of state variables and remainder could have been lost when updating X_sv
cum_status |= ssm_check_no_neg_sv_or_remainder(X, par, nav, calc, t);
// log_like
fitness->log_like += ssm_sanitize_log_likelihood(log(ssm_dmvnorm(row->ts_nonan_length, &pred_error.vector, &zero.vector, &St.matrix, 1.0)), row, fitness, nav);
return cum_status;
}
void StripedStructure::multByWInv( gsl_vector* p, long deg ) const {
size_t sum_np = 0;
gsl_vector sub_p;
for (size_t l = 0; l < getBlocksN(); sum_np += myStripe[l]->getNp(), l++) {
sub_p = gsl_vector_subvector(p, sum_np, myStripe[l]->getNp()).vector;
myStripe[l]->multByWInv(&sub_p, deg);
}
}
// Interpolate projection coefficients for amplitude and phase over the parameter space (q, chi).
// The multi-dimensional interpolation is carried out via a tensor product decomposition.
static int TP_Spline_interpolation_3d(
REAL8 q, // Input: q-value for which projection coefficients should be evaluated
REAL8 chi1, // Input: chi1-value for which projection coefficients should be evaluated
REAL8 chi2, // Input: chi2-value for which projection coefficients should be evaluated
gsl_vector *cvec_amp, // Input: data for spline coefficients for amplitude
gsl_vector *cvec_phi, // Input: data for spline coefficients for phase
gsl_vector *cvec_amp_pre, // Input: data for spline coefficients for amplitude prefactor
gsl_vector *c_amp, // Output: interpolated projection coefficients for amplitude
gsl_vector *c_phi, // Output: interpolated projection coefficients for phase
REAL8 *amp_pre // Output: interpolated amplitude prefactor
) {
SplineData *splinedata=NULL;
SplineData_Init(&splinedata);
gsl_bspline_workspace *bwx=splinedata->bwx;
gsl_bspline_workspace *bwy=splinedata->bwy;
gsl_bspline_workspace *bwz=splinedata->bwz;
int ncx = splinedata->ncx; // points in q
int ncy = splinedata->ncy; // points in chi1
int ncz = splinedata->ncz; // points in chi2
int N = ncx*ncy*ncz; // size of the data matrix for one SVD-mode
// Evaluate the TP spline for all SVD modes - amplitude
for (int k=0; k<nk_amp; k++) { // For each SVD mode
gsl_vector v = gsl_vector_subvector(cvec_amp, k*N, N).vector; // Pick out the coefficient matrix corresponding to the k-th SVD mode.
REAL8 csum = Interpolate_Coefficent_Tensor(&v, q, chi1, chi2, ncy, ncz, bwx, bwy, bwz);
gsl_vector_set(c_amp, k, csum);
}
// Evaluate the TP spline for all SVD modes - phase
for (int k=0; k<nk_phi; k++) { // For each SVD mode
gsl_vector v = gsl_vector_subvector(cvec_phi, k*N, N).vector; // Pick out the coefficient matrix corresponding to the k-th SVD mode.
REAL8 csum = Interpolate_Coefficent_Tensor(&v, q, chi1, chi2, ncy, ncz, bwx, bwy, bwz);
gsl_vector_set(c_phi, k, csum);
}
// Evaluate the TP spline for the amplitude prefactor
*amp_pre = Interpolate_Coefficent_Tensor(cvec_amp_pre, q, chi1, chi2, ncy, ncz, bwx, bwy, bwz);
SplineData_Destroy(splinedata);
return(0);
}
static double
cholesky_LDLT_norm1(const gsl_matrix * LDLT, const gsl_permutation * p, gsl_vector * work)
{
const size_t N = LDLT->size1;
gsl_vector_const_view D = gsl_matrix_const_diagonal(LDLT);
gsl_vector_view diagA = gsl_vector_subvector(work, N, N);
double max = 0.0;
size_t i, j;
/* reconstruct diagonal entries of original matrix A */
for (j = 0; j < N; ++j)
{
double Ajj;
/* compute diagonal (j,j) entry of A */
Ajj = gsl_vector_get(&D.vector, j);
for (i = 0; i < j; ++i)
{
double Di = gsl_vector_get(&D.vector, i);
double Lji = gsl_matrix_get(LDLT, j, i);
Ajj += Di * Lji * Lji;
}
gsl_vector_set(&diagA.vector, j, Ajj);
}
gsl_permute_vector_inverse(p, &diagA.vector);
for (j = 0; j < N; ++j)
{
double sum = 0.0;
double Ajj = gsl_vector_get(&diagA.vector, j);
for (i = 0; i < j; ++i)
{
double *wi = gsl_vector_ptr(work, i);
double Aij = gsl_matrix_get(LDLT, i, j);
double absAij = fabs(Aij);
sum += absAij;
*wi += absAij;
}
gsl_vector_set(work, j, sum + fabs(Ajj));
}
for (i = 0; i < N; ++i)
{
double wi = gsl_vector_get(work, i);
max = GSL_MAX(max, wi);
}
return max;
}
void cholUpdate(gsl_matrix* R, gsl_vector* x)
{
int n = R->size1;
int i = 0;
double c; double s;
for (i=0;i<n;i++) {
double* a = gsl_matrix_ptr(R,i,i);
double* b = gsl_vector_ptr(x, i);
My_drotg(a,b,&c,&s);
if ((*a)<0.0) {
*a = - (*a);
c = - c;
s = - s;
}
if (i<n-1) {
gsl_vector_view Ri = gsl_matrix_column(R, i);
gsl_vector_view Rii = gsl_vector_subvector(&Ri.vector, i+1, n-i-1);
gsl_vector_view xi = gsl_vector_subvector(x, i+1, n-i-1);
My_drot(&Rii.vector,&xi.vector,c,s);
}
}
}
void HLayeredBlWStructure::multByWInv( gsl_vector* p, long deg ) {
size_t l_1, k, sum_np = 0;
gsl_vector psub;
if (deg == 0) {
return;
}
for (l_1 = 0; l_1 < getQ(); sum_np += getLayerNp(l_1), ++l_1) {
psub = gsl_vector_subvector(p, sum_np, getLayerNp(l_1)).vector;
gsl_vector_scale(&psub, (deg == 2) ? getLayerInvWeight(l_1):
sqrt(getLayerInvWeight(l_1)));
}
}
static int
magcal_scale(const int dir, gsl_vector *m, magcal_workspace *w)
{
int s = 0;
gsl_vector_view v = gsl_vector_subvector(m, MAGCAL_IDX_OX, 3);
if (dir == 1) /* scale to dimensionless */
gsl_vector_scale(&v.vector, 1.0 / w->B_s);
else /* scale to nT */
gsl_vector_scale(&v.vector, w->B_s);
return s;
} /* magcal_scale() */
void StripedStructure::multByGtUnweighted( gsl_vector* p, const gsl_matrix *Rt,
const gsl_vector *y, double alpha, double beta, bool skipFixedBlocks ){
size_t n_row = 0, sum_np = 0, d = Rt->size2;
gsl_vector subp, suby;
for (size_t l = 0; l < getBlocksN();
sum_np += myStripe[l]->getNp(), n_row += getBlock(l)->getN() * d, l++) {
suby = gsl_vector_const_subvector(y, n_row, getBlock(l)->getN() * d).vector;
subp = gsl_vector_subvector(p, sum_np, myStripe[l]->getNp()).vector;
myStripe[l]->multByGtUnweighted(&subp, Rt, &suby, alpha, beta,
skipFixedBlocks);
}
}
void dc_eig(gsl_matrix *sym, gsl_vector *eval, gsl_matrix *evec ,size_t NCOMP){
// Divide and conquer Eigen decomposition
// gsl_matrix *evec = gsl_matrix_alloc(NSUB, NSUB);
// gsl_vector *eval = gsl_vector_alloc(NCOMP); //eigen values
size_t NSUB = sym->size1;
gsl_vector *eval_temp =gsl_vector_alloc(NSUB);
LAPACKE_dsyevd(LAPACK_ROW_MAJOR, 'V', 'U',
NSUB, sym->data, NSUB, eval_temp->data);
gsl_eigen_symmv_sort (eval_temp, sym, GSL_EIGEN_SORT_ABS_DESC);
gsl_matrix_view temp = gsl_matrix_submatrix(sym, 0,0 , NSUB, NCOMP);
gsl_matrix_memcpy(evec,&temp.matrix);
gsl_vector_view temp_vec = gsl_vector_subvector(eval_temp, 0, NCOMP);
gsl_vector_memcpy(eval, &temp_vec.vector);
gsl_vector_free(eval_temp);
}
int compute_itegral_r(const mu_data_fit *mu, const fit_params fp, gsl_vector *fftR_abs){
size_t vsize= mu->k->size;
gsl_vector *mu_tmp=gsl_vector_alloc(vsize);
gsl_vector_set_zero(mu_tmp);
size_t ikmin=search_min(mu->k, mu->kmin - 0.5*mu->dwk);
size_t ikmax=search_min(mu->k, mu->kmax + 0.5*mu->dwk);
gsl_vector_view kw = gsl_vector_subvector(mu->k, ikmin-1, ikmax-ikmin-1);
gsl_vector_view muw = gsl_vector_subvector(mu_tmp, ikmin-1, ikmax-ikmin-1);
gsl_vector *ktmp=gsl_vector_alloc((&kw.vector)->size);
gsl_vector_memcpy(ktmp, &kw.vector);
gsl_vector_add_constant(ktmp, fp.kshift);
compute_itegral(ktmp, &fp, &muw.vector);
hanning(mu_tmp, mu->k, mu->kmin, mu->kmax, mu->dwk);
//FFT transform
double *data = (double *) malloc(vsize*sizeof(double));
memcpy(data, mu_tmp->data, vsize*sizeof(double));
gsl_fft_real_radix2_transform(data, 1, vsize);
//Unpack complex vector
gsl_vector_complex *fourier_data = gsl_vector_complex_alloc (vsize);
gsl_fft_halfcomplex_radix2_unpack(data, fourier_data->data, 1, vsize);
gsl_vector *fftR_real = gsl_vector_alloc(vsize/2);
gsl_vector *fftR_imag = gsl_vector_alloc(vsize/2);
//gsl_vector *fftR_abs = gsl_vector_alloc(vsize/2);
complex_vector_parts(fourier_data, fftR_real, fftR_imag);
complex_vector_abs(fftR_abs, fftR_real, fftR_imag);
hanning(fftR_abs, mu->r, mu->rmin, mu->rmax, mu->dwr);
gsl_vector_free(fftR_real); gsl_vector_free(fftR_imag);
gsl_vector_complex_free(fourier_data); gsl_vector_free(mu_tmp);
free(data);
}