void mcmclib_Givens_representation(gsl_matrix* M,
const gsl_vector* alpha_sigma) {
size_t n = M->size1;
size_t offset = n * (n-1) / 2;
gsl_vector* alpha1 = gsl_vector_alloc(offset);
for(size_t i=0; i<offset; i++)
gsl_vector_set(alpha1, i, gsl_vector_get(alpha_sigma, i));
gsl_vector* sigma1 = gsl_vector_alloc(n);
for(size_t i=0; i<n; i++) {
double bi = exp(gsl_vector_get(alpha_sigma, i + offset));
gsl_vector_set(sigma1, i, bi);
}
vSortDesc(sigma1);
gsl_matrix* A = gsl_matrix_alloc(n, n);
mcmclib_Givens_rotations(A, alpha1);
gsl_matrix* D = gsl_matrix_alloc(n, n);
gsl_matrix_set_zero(D);
for(size_t i=0; i<n; i++)
gsl_matrix_set(D, i, i, gsl_vector_get(sigma1, i));
gsl_matrix* AD = gsl_matrix_alloc(n, n);
gsl_matrix_set_zero(AD);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, A, D, 0.0, AD);
gsl_matrix_set_zero(M);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, AD, A, 0.0, M);
gsl_matrix_free(AD);
gsl_matrix_free(D);
gsl_matrix_free(A);
gsl_vector_free(alpha1);
gsl_vector_free(sigma1);
}
void VarproFunction::computeJacobianOfCorrection( const gsl_vector* yr,
const gsl_matrix *Rorig, const gsl_matrix *perm, gsl_matrix *jac ) {
size_t nrow = perm != NULL ? perm->size2 : getNrow();
gsl_matrix_set_zero(jac);
gsl_matrix_set_zero(myTmpGradR);
fillZmatTmpJac(myTmpJac, yr, Rorig, 1);
for (size_t i = 0; i < perm->size2; i++) {
for (size_t j = 0; j < getD(); j++) {
gsl_vector_view jac_col = gsl_matrix_column(jac, i * getD() + j);
/* Compute first term (correction of Gam^{-1} z_{ij}) */
gsl_vector_set_zero(myTmpCorr);
mulZmatPerm(myTmpJacobianCol, myTmpJac, perm, i, j);
myGam->multInvGammaVector(myTmpJacobianCol);
myStruct->multByGtUnweighted(myTmpCorr, Rorig, myTmpJacobianCol, -1, 1);
/* Compute second term (gamma * dG_{ij} * yr) */
gsl_matrix_set_zero(myTmpGradR);
setPhiPermCol(i, perm, myPhiPermCol);
gsl_matrix_set_col(myTmpGradR, j, myPhiPermCol);
myStruct->multByGtUnweighted(myTmpCorr, myTmpGradR, yr, -1, 1);
myStruct->multByWInv(myTmpCorr, 1);
gsl_vector_memcpy(&jac_col.vector, myTmpCorr);
}
}
}
/*
This code returns 1 if and only if SC_EIG is an eigenvalue
of the partitioned matrix of our current graph.
*/
unsigned partAMcond(void) {
unsigned i,j;
unsigned total_edges = 0;
gsl_matrix_set_zero(par_adj);
for (i = 0; i < N+1; i++) {
gsl_matrix_set(par_adj, i, i, -SC_EIG);
for (j = i+1; j < N+1; j++) {
if (nbrs[i] & (1U << j)) {
gsl_matrix_set(par_adj, i, j, 1);
gsl_matrix_set(par_adj, j, i, 1);
}
}
unsigned deg = bitCount[ nbrs[i] ];
gsl_matrix_set(par_adj, i, N+1, VAL-deg);
gsl_matrix_set(par_adj, N+1, i, (double)(VAL-deg)/(NTOT-N-1));
total_edges += deg;
}
gsl_matrix_set(par_adj, N+1, N+1, -SC_EIG + (double) 2*(NTOT*VAL/2 + total_edges/2 - (N+1)*VAL)/(NTOT-N-1));
int signum;
gsl_linalg_LU_decomp(par_adj, par_perm, &signum);
double det = gsl_linalg_LU_det(par_adj, signum);
/* The determinant is not zero. Hence SC_EIG is not an eigenvalue of our graph. */
if (fabs(det) > 0.0000001) {
return 0;
}
return 1;
}
gsl_matrix * SgFilter::pseudoInverse(gsl_matrix *A, int n_row, int n_col){
gsl_matrix * A_t = gsl_matrix_alloc (n_col, n_row); //A_t is transpose
gsl_matrix_transpose_memcpy (A_t, A);
gsl_matrix * U = gsl_matrix_alloc (n_col, n_row);
gsl_matrix * V= gsl_matrix_alloc (n_row, n_row);
gsl_vector * S = gsl_vector_alloc (n_row);
// Computing the SVD of the transpose of A
gsl_vector * work = gsl_vector_alloc (n_row);
gsl_linalg_SV_decomp (A_t, V, S, work);
gsl_vector_free(work);
gsl_matrix_memcpy (U, A_t);
//Inverting S
gsl_matrix * Sp = gsl_matrix_alloc (n_row, n_row);
gsl_matrix_set_zero (Sp);
for (int i = 0; i < n_row; i++)
gsl_matrix_set (Sp, i, i, gsl_vector_get(S, i)); // Vector 'S' to matrix 'Sp'
gsl_permutation * p = gsl_permutation_alloc (n_row);
int signum;
gsl_linalg_LU_decomp (Sp, p, &signum); // Computing the LU decomposition
// Compute the inverse
gsl_matrix * SI = gsl_matrix_calloc (n_row, n_row);
for (int i = 0; i < n_row; i++) {
if (gsl_vector_get (S, i) > 0.0000000001)
gsl_matrix_set (SI, i, i, 1.0 / gsl_vector_get (S, i));
}
gsl_matrix * VT = gsl_matrix_alloc (n_row, n_row);
gsl_matrix_transpose_memcpy (VT, V); // Tranpose of V
//THE PSEUDOINVERSE
//Computation of the pseudoinverse of trans(A) as pinv(A) = U·inv(S).trans(V) with trans(A) = U.S.trans(V)
gsl_matrix * SIpVT = gsl_matrix_alloc (n_row, n_row);
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, // Calculating inv(S).trans(V)
1.0, SI, VT,
0.0, SIpVT);
gsl_matrix * pinv = gsl_matrix_alloc (n_col, n_row); // Calculating U·inv(S).trans(V)
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
1.0, U, SIpVT,
0.0, pinv);
gsl_matrix_free(VT);
gsl_matrix_free(SI);
gsl_matrix_free(SIpVT);
gsl_matrix_free(A_t);
gsl_matrix_free(U);
gsl_matrix_free(A);
gsl_matrix_free(V);
gsl_vector_free(S);
return pinv;
}
// Find B s.t. B B^T = A. This is useful for generating vectors from a multivariate normal distribution.
// Operates on A in-place if sqrt_A == NULL.
void sqrt_matrix(gsl_matrix* A, gsl_matrix* sqrt_A, gsl_eigen_symmv_workspace* esv, gsl_vector *eival, gsl_matrix *eivec, gsl_matrix* sqrt_eival) {
size_t N = A->size1;
assert(A->size2 == N);
assert(sqrt_eival->size1 == N);
assert(sqrt_eival->size2 == N);
gsl_matrix_set_zero(sqrt_eival);
if(sqrt_A == NULL) {
sqrt_A = A;
} else {
assert(sqrt_A->size1 == N);
assert(sqrt_A->size2 == N);
gsl_matrix_memcpy(sqrt_A, A);
}
// Calculate the eigendecomposition of the covariance matrix
gsl_eigen_symmv(sqrt_A, eival, eivec, esv);
double tmp;
for(size_t i=0; i<N; i++) {
tmp = gsl_vector_get(eival, i);
gsl_matrix_set(sqrt_eival, i, i, sqrt(fabs(tmp)));
if(tmp < 0.) {
for(size_t j=0; j<N; j++) { gsl_matrix_set(eivec, j, i, -gsl_matrix_get(eivec, j, i)); }
}
}
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1., eivec, sqrt_eival, 0., sqrt_A);
}
tnn_error tnn_module_bprop_linear(tnn_module *m){
tnn_error ret;
gsl_matrix w;
gsl_matrix dw;
//Routine check
if(m->t != TNN_MODULE_TYPE_LINEAR){
return TNN_ERROR_MODULE_MISTYPE;
}
if(m->input->valid != true || m->output->valid != true || m->w.valid != true){
return TNN_ERROR_STATE_INVALID;
}
//Transform the matrix
TNN_MACRO_ERRORTEST(tnn_numeric_v2m(&m->w.x, &w, m->output->size, m->input->size),ret);
TNN_MACRO_ERRORTEST(tnn_numeric_v2m(&m->w.dx, &dw, m->output->size, m->input->size), ret);
//bprop to input
TNN_MACRO_GSLTEST(gsl_blas_dgemv(CblasTrans, 1.0, &w, &m->output->dx, 0.0, &m->input->dx));
//bprop to dw
gsl_matrix_set_zero(&dw);
TNN_MACRO_GSLTEST(gsl_blas_dger(1.0, &m->output->dx, &m->input->x, &dw));
return TNN_ERROR_SUCCESS;
}
int
Matrix::multiplyWithMatrix(Matrix* b)
{
if(col!=b->getRowCnt())
{
return -1;
}
gsl_matrix* res=gsl_matrix_alloc(row,b->getColCnt());
gsl_matrix_set_zero(res);
gsl_matrix_float* resmatrix=gsl_matrix_float_alloc(row,b->getColCnt());
convertToFloat(resmatrix,res,row,b->getColCnt());
gsl_matrix_float* cmatrix=gsl_matrix_float_alloc(row,col);
convertToFloat(cmatrix,matrix,row,col);
gsl_matrix_float* bmatrix=gsl_matrix_float_alloc(b->getRowCnt(),b->getColCnt());
convertToFloat(bmatrix,b->matrix,b->getRowCnt(),b->getColCnt());
gsl_blas_sgemm(CblasNoTrans, CblasNoTrans, 1, cmatrix, bmatrix, 0, resmatrix);
convertFromFloat(resmatrix,res,row,b->getColCnt());
gsl_matrix_float_free(resmatrix);
gsl_matrix_float_free(cmatrix);
gsl_matrix_float_free(bmatrix);
gsl_matrix_memcpy(matrix, res);
gsl_matrix_free(res);
return 0;
}
static int
wood_df (CBLAS_TRANSPOSE_t TransJ, const gsl_vector * x,
const gsl_vector * u, void * params, gsl_vector * v,
gsl_matrix * JTJ)
{
gsl_matrix_view J = gsl_matrix_view_array(wood_J, wood_N, wood_P);
double x1 = gsl_vector_get(x, 0);
double x3 = gsl_vector_get(x, 2);
double s90 = sqrt(90.0);
double s10 = sqrt(10.0);
gsl_matrix_set_zero(&J.matrix);
gsl_matrix_set(&J.matrix, 0, 0, -20.0*x1);
gsl_matrix_set(&J.matrix, 0, 1, 10.0);
gsl_matrix_set(&J.matrix, 1, 0, -1.0);
gsl_matrix_set(&J.matrix, 2, 2, -2.0*s90*x3);
gsl_matrix_set(&J.matrix, 2, 3, s90);
gsl_matrix_set(&J.matrix, 3, 2, -1.0);
gsl_matrix_set(&J.matrix, 4, 1, s10);
gsl_matrix_set(&J.matrix, 4, 3, s10);
gsl_matrix_set(&J.matrix, 5, 1, 1.0/s10);
gsl_matrix_set(&J.matrix, 5, 3, -1.0/s10);
if (v)
gsl_blas_dgemv(TransJ, 1.0, &J.matrix, u, 0.0, v);
if (JTJ)
gsl_blas_dsyrk(CblasLower, CblasTrans, 1.0, &J.matrix, 0.0, JTJ);
(void)params; /* avoid unused parameter warning */
return GSL_SUCCESS;
}
/**
* setup the decomp, allocs the variable grid
*/
void setup_fdecomp_(int* ngrid)
{
nptsGrid = *ngrid;
printf("#(c) npts:%d\n", nptsGrid);
grid = gsl_matrix_alloc(nptsGrid, nptsGrid);
gsl_matrix_set_zero(grid); // clear the grid
}
void kjg_fpca_XTXA (
const gsl_matrix *A1,
gsl_matrix *B,
gsl_matrix *A2) {
size_t m = get_ncols();
size_t n = get_nrows();
size_t i, r; // row index
double *Y = malloc(sizeof(double) * n * KJG_FPCA_ROWS); // normalized
gsl_matrix_view Bi, Xi;
gsl_matrix_set_zero(A2);
for (i = 0; i < m; i += KJG_FPCA_ROWS) {
r = kjg_geno_get_normalized_rows(i, KJG_FPCA_ROWS, Y);
Xi = gsl_matrix_view_array(Y, r, n);
Bi = gsl_matrix_submatrix(B, i, 0, r, B->size2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, &Xi.matrix, A1, 0,
&Bi.matrix);
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1, &Xi.matrix, &Bi.matrix,
1, A2);
}
free(Y);
}
//Multiply this with b
Matrix*
Matrix::multiplyMatrix(Matrix* b)
{
if(col!=b->getRowCnt())
{
return NULL;
}
Matrix* res=new Matrix(row,b->getColCnt());
gsl_matrix_set_zero (res->matrix);
gsl_matrix_float* resmatrix=gsl_matrix_float_alloc(row,b->getColCnt());
convertToFloat(resmatrix,res->matrix,row,b->getColCnt());
gsl_matrix_float* cmatrix=gsl_matrix_float_alloc(row,col);
convertToFloat(cmatrix,matrix,row,col);
gsl_matrix_float* bmatrix=gsl_matrix_float_alloc(b->getRowCnt(),b->getColCnt());
convertToFloat(bmatrix,b->matrix,b->getRowCnt(),b->getColCnt());
gsl_blas_sgemm (CblasNoTrans, CblasNoTrans, 1, cmatrix, bmatrix, 0, resmatrix);
convertFromFloat(resmatrix,res->matrix,row,b->getColCnt());
gsl_matrix_float_free(resmatrix);
gsl_matrix_float_free(cmatrix);
gsl_matrix_float_free(bmatrix);
return res;
}
gsl_matrix* matrix_multiply(gsl_matrix* matrix1, gsl_matrix* matrix2){
gsl_matrix *matrix3;
int i, j, k;
// checks to see if the dimensions of matrix 1 and 2 are compatible for matrix multiplication
if (matrix1->size2 != matrix2->size1){
printf("Cannot multiply due to incompatitble matrix dimensions.\n\n");
matrix3 = NULL;
}
else {
// the resulting matrix has the same number of rows as matrix 1 and the same
// number of columns as matrix 2
matrix3 = gsl_matrix_alloc(matrix1->size1, matrix2->size2);
gsl_matrix_set_zero(matrix3);
// apply the definition of matrix multiplication
for (k=0; k<(matrix2->size2);k++){
for (j=0; j<(matrix1->size1);j++){
for (i=0; i<(matrix1->size2);i++){
gsl_matrix_set(matrix3,j,k, gsl_matrix_get(matrix3,j,k) + (gsl_matrix_get(matrix1,j,i)*gsl_matrix_get(matrix2,i,k)));
}
}
}
}
return matrix3;
}
static int
wood_df (const gsl_vector * x, void *params, gsl_matrix * J)
{
double x1 = gsl_vector_get(x, 0);
double x3 = gsl_vector_get(x, 2);
double s90 = sqrt(90.0);
double s10 = sqrt(10.0);
gsl_matrix_set_zero(J);
gsl_matrix_set(J, 0, 0, -20.0*x1);
gsl_matrix_set(J, 0, 1, 10.0);
gsl_matrix_set(J, 1, 0, -1.0);
gsl_matrix_set(J, 2, 2, -2.0*s90*x3);
gsl_matrix_set(J, 2, 3, s90);
gsl_matrix_set(J, 3, 2, -1.0);
gsl_matrix_set(J, 4, 1, s10);
gsl_matrix_set(J, 4, 3, s10);
gsl_matrix_set(J, 5, 1, 1.0/s10);
gsl_matrix_set(J, 5, 3, -1.0/s10);
(void)params; /* avoid unused parameter warning */
return GSL_SUCCESS;
}
void Testmcar_model(CuTest* tc) {
gsl_matrix* W = gsl_matrix_alloc(N, N);
gsl_matrix_set_zero(W);
for(size_t i=0; i<(N-1); i++)
DECL_AD(i, i+1);
mcmclib_mcar_tilde_lpdf* llik = mcmclib_mcar_tilde_lpdf_alloc(P, W);
gsl_vector* e = gsl_vector_alloc(N * P);
gsl_vector_set_all(e, 2.0);
p = mcmclib_mcar_model_alloc(llik, e);
double l1 = lpdf_alpha12sigma(-2.0);
double l2 = lpdf_alpha12sigma(-5.0);
CuAssertTrue(tc, gsl_finite(l1));
CuAssertTrue(tc, gsl_finite(l2));
CuAssertTrue(tc, l1 > l2);
CuAssertTrue(tc, l1 == lpdf_alpha12sigma(-2.0));
gsl_vector* alphasigma = gsl_vector_alloc(P * (P-1) / 2 + P);
gsl_vector_set_all(llik->alpha12sigma, -1.0);
gsl_vector_set_all(alphasigma, 0.0);
l1 = mcmclib_mcar_model_alphasigma_lpdf(p, alphasigma);
CuAssertTrue(tc, gsl_finite(l1));
gsl_vector_set(alphasigma, P+1, 1.0);
CuAssertTrue(tc, !gsl_finite(mcmclib_mcar_model_alphasigma_lpdf(p, alphasigma)));
gsl_vector_free(alphasigma);
mcmclib_mcar_model_free(p);
gsl_vector_free(e);
mcmclib_mcar_tilde_lpdf_free(llik);
gsl_matrix_free(W);
}
int
gsl_multifit_linear_Lk(const size_t p, const size_t k, gsl_matrix *L)
{
if (p <= k)
{
GSL_ERROR("p must be larger than derivative order", GSL_EBADLEN);
}
else if (k >= GSL_MULTIFIT_MAXK - 1)
{
GSL_ERROR("derivative order k too large", GSL_EBADLEN);
}
else if (p - k != L->size1 || p != L->size2)
{
GSL_ERROR("L matrix must be (p-k)-by-p", GSL_EBADLEN);
}
else
{
double c_data[GSL_MULTIFIT_MAXK];
gsl_vector_view cv = gsl_vector_view_array(c_data, k + 1);
size_t i, j;
/* zeroth derivative */
if (k == 0)
{
gsl_matrix_set_identity(L);
return GSL_SUCCESS;
}
gsl_matrix_set_zero(L);
gsl_vector_set_zero(&cv.vector);
gsl_vector_set(&cv.vector, 0, -1.0);
gsl_vector_set(&cv.vector, 1, 1.0);
for (i = 1; i < k; ++i)
{
double cjm1 = 0.0;
for (j = 0; j < k + 1; ++j)
{
double cj = gsl_vector_get(&cv.vector, j);
gsl_vector_set(&cv.vector, j, cjm1 - cj);
cjm1 = cj;
}
}
/* build L, the c_i are the entries on the diagonals */
for (i = 0; i < k + 1; ++i)
{
gsl_vector_view v = gsl_matrix_superdiagonal(L, i);
double ci = gsl_vector_get(&cv.vector, i);
gsl_vector_set_all(&v.vector, ci);
}
return GSL_SUCCESS;
}
} /* gsl_multifit_linear_Lk() */
/* rebuild asymm. matrix from its SVD decomposition */
static void anti_SVD(gsl_matrix* A,
const gsl_matrix* P1,
const gsl_matrix* P2,
const gsl_vector* sigma) {
const size_t p = A->size1;
gsl_matrix* Delta = gsl_matrix_alloc(p, p);
gsl_matrix_set_zero(Delta);
for(size_t i=0; i<p; i++)
gsl_matrix_set(Delta, i, i, exp(gsl_vector_get(sigma, i)));
gsl_matrix* P1Delta = gsl_matrix_alloc(p, p);
gsl_matrix_set_zero(P1Delta);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, P1, Delta, 0.0, P1Delta);
gsl_matrix_set_zero(A);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, P1Delta, P2, 0.0, A);
gsl_matrix_free(Delta);
gsl_matrix_free(P1Delta);
}
int
lls_reset(lls_workspace *w)
{
gsl_matrix_set_zero(w->ATA);
gsl_vector_set_zero(w->ATb);
w->bTb = 0.0;
return 0;
}
/* The following functions handles the correlation
tensor in the exciton matrix */
void bath_js03_init_OpQ(int ndim, gsl_matrix *U)
{
int nops,i;
size_t n,m,a,b,c,d;
size_t idxa,idxb,idxc;
double dd;
/* we first count the number of bath operators */
nops=0;
while(BATH_JS03OpBra[nops]>0) {
nops++;
}
/* allocate the matrix, totally n^2*n^2 rows... */
BATH_JS03OpQ=gsl_matrix_alloc(ndim*ndim*ndim*ndim,nops);
gsl_matrix_set_zero(BATH_JS03OpQ);
BATH_JS03Ct=gsl_vector_alloc(nops); // as a workspace
/* construct matrix elements, no tricks here */
for(i=0;i<nops;i++) {
n=BATH_JS03OpKet[i]-1;
m=BATH_JS03OpBra[i]-1; // note: m>=n
for(a=0,idxa=0;a<ndim;a++,idxa+=ndim*ndim*ndim) {
for(b=0,idxb=0;b<ndim;b++,idxb+=ndim*ndim) {
for(c=0,idxc=0;c<ndim;c++,idxc+=ndim) {
for(d=0;d<ndim;d++) {
// printf("(n,m,a,b,c,d) = (%d,%d,%d,%d,%d,%d)\n",n,m,a,b,c,d);
if(n==m) {
// diagonal term
dd=fgsl_matrix_get(U,n,a)*fgsl_matrix_get(U,n,b)*
fgsl_matrix_get(U,n,c)*fgsl_matrix_get(U,n,d);
fgsl_matrix_set(BATH_JS03OpQ,idxa+idxb+idxc+d,i,dd);
} else {
// off-diagonal term
dd=fgsl_matrix_get(U,n,a)*fgsl_matrix_get(U,m,b)*
fgsl_matrix_get(U,n,c)*fgsl_matrix_get(U,m,d);
dd+=fgsl_matrix_get(U,n,a)*fgsl_matrix_get(U,m,b)*
fgsl_matrix_get(U,m,c)*fgsl_matrix_get(U,n,d);
dd+=fgsl_matrix_get(U,m,a)*fgsl_matrix_get(U,n,b)*
fgsl_matrix_get(U,m,c)*fgsl_matrix_get(U,n,d);
dd+=fgsl_matrix_get(U,m,a)*fgsl_matrix_get(U,n,b)*
fgsl_matrix_get(U,n,c)*fgsl_matrix_get(U,m,d);
fgsl_matrix_set(BATH_JS03OpQ,idxa+idxb+idxc+d,i,dd);
}
}
}
}
}
}
printf("OpQ =\n");
gsl_matrix_lprint(BATH_JS03OpQ);
printf("\n");
printf("\n");
}
void StripedDGamma::calcYrtDgammaYr( gsl_matrix *grad, const gsl_matrix *R,
const gsl_vector *yr ) {
size_t n_row = 0, k;
gsl_matrix_set_zero(grad);
for (k = 0; k < myS->getBlocksN(); n_row += myS->getBlock(k)->getN(), k++) {
gsl_vector_const_view sub_yr = gsl_vector_const_subvector(yr, n_row * R->size2,
myS->getBlock(k)->getN() * R->size2);
myLHDGamma[k]->calcYrtDgammaYr(myTmpGrad, R, &sub_yr.vector);
gsl_matrix_add(grad, myTmpGrad);
}
}
static void compute_fg_removal(void)
{
int ii;
gsl_matrix *id=gsl_matrix_alloc(glob_n_nu,glob_n_nu);
gsl_matrix *dum=gsl_matrix_alloc(glob_n_nu,glob_n_nu);
fg_removal=gsl_matrix_alloc(glob_n_nu,glob_n_nu);
gsl_matrix_set_identity(id);
gsl_matrix_set_zero(dum);
gsl_matrix_set_zero(fg_removal);
//Eliminating the first n_remove principal eigenvectors
for(ii=0;ii<n_remove;ii++)
gsl_matrix_set(id,glob_n_nu-ii-1,glob_n_nu-ii-1,0);
//Computing FG=U*S*U^T
gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,id,eigenvecs,0.0,dum);
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,eigenvecs,dum,0.0,fg_removal);
gsl_matrix_free(id);
gsl_matrix_free(dum);
}
void Compute_Forces(gsl_matrix * Positions, gsl_matrix * Velocities, gsl_matrix * Neighbors,
gsl_vector * ListHead, gsl_vector * List, int type1, int type2,
gsl_matrix * Forces, gsl_vector * Energy, gsl_vector * Kinetic )
{
// RESET MATRICES AND VECTORS
// TODO: Redundant?
gsl_matrix_set_zero(Forces);
gsl_vector_set_zero(Energy);
gsl_vector_set_zero(Kinetic);
// Begin of parallel region
int omp_get_max_threads();
int chunks = NParticles / omp_get_max_threads();
#pragma omp parallel
{
#pragma omp for schedule (dynamic,chunks)
for (int i=0;i<NParticles;i++)
{
gsl_vector_view vi = gsl_matrix_row(Velocities, i);
double * fij = malloc(3*sizeof(double));
// Compute the kinetic energy of particle i (0.5 mi vi^2)
double ei = KineticEnergy(&vi.vector, (int) gsl_matrix_get(Positions,i,0));
gsl_vector_set(Kinetic,i,ei);
// Obtain the list of neighboring cells to iCell (the cell i belongs to)
int iCell = FindParticle(Positions,i);
gsl_vector_view NeighboringCells = gsl_matrix_row(Neighbors, iCell);
// Obtain the list of neighboring particles that interacts with i
// i interacts with all Verlet[j] particles (j = 0 .. NNeighbors-1)
int * Verlet = malloc(27 * NParticles * sizeof(int) / (Mx*My*Mz));
int NNeighbors = Compute_VerletList(Positions, i, &NeighboringCells.vector, iCell, ListHead, List, Verlet);
// Loop over all the j-neighbors of i-particle
for (int j=0;j<NNeighbors;j++)
{
ei = Compute_Force_ij(Positions, i, Verlet[j], type1, type2, fij);
Forces->data[i*Forces->tda + 0] += fij[0];
Forces->data[i*Forces->tda + 1] += fij[1];
Forces->data[i*Forces->tda + 2] += fij[2];
Energy->data[i*Energy->stride] += ei;
}
free(Verlet);
free(fij);
}
}
// End of parallel region
}
void Nav_MotionEstimator::Nav_PKPropagator_Unicycle( double * Uk, double T)
{
double thPose = gsl_matrix_get(MeanPose, 2, 0);
/** todo: move uncertanty values to configuration file */
double Sig1VtError = 0.01;
double Sig2VthError = 0.01;
double Sig3VtError = 0.01;
double Sig4VthError = 0.01;
double M11 = Sig1VtError * fabs(Uk[0]) + Sig2VthError * fabs(Uk[1]);
double M22 = Sig3VtError * fabs(Uk[0]) + Sig4VthError * fabs(Uk[1]); //on the ekf use systematic error
gsl_matrix_set(Nk, 0, 0, pow(M11, 2));
gsl_matrix_set(Nk, 0, 1, 0);
gsl_matrix_set(Nk, 1, 0, 0);
gsl_matrix_set(Nk, 1, 1, pow(M22, 2));
/**
position gradient
Fx =[[ 1 0 -vt*T*sin(theta) ]
[ 0 1 vt*T*cos(theta) ]
[ 0 0 1 ]]
*/
gsl_matrix_set_identity(Fx);
gsl_matrix_set(Fx, 0, 2, -Uk[0] * T * sin(thPose));
gsl_matrix_set(Fx, 1, 2, Uk[0] * T * cos(thPose));
gsl_matrix_transpose_memcpy(FxT, Fx); //F transpose
/**
velocities gradient
Fu = [ [ T*cos(theta) 0 ]
[ T*sin(theta) 0 ]
[ 0 T ]]
*/
gsl_matrix_set(Fu, 0, 0, T * cos(thPose));
gsl_matrix_set(Fu, 0, 1, 0);
gsl_matrix_set(Fu, 1, 0, T * sin(thPose));
gsl_matrix_set(Fu, 1, 1, 0);
gsl_matrix_set(Fu, 2, 0, 0);
gsl_matrix_set(Fu, 2, 1, T);
gsl_matrix_transpose_memcpy(FuT, Fu); //F transpose
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Fu, Nk, 0.0, Qk_temp);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qk_temp, FuT, 0.0, Qk);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Fx, CovPose, 0.0, Pk_temp);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Pk_temp, FxT, 0.0, res3x3);
gsl_matrix_set_zero(CovPose);
gsl_matrix_add(CovPose, Qk);
gsl_matrix_add(CovPose, res3x3);
}
int UpdateAssignment(const gsl_matrix_uint * const mask,
gsl_matrix * const Assignment)
{
unsigned int i, j;
gsl_matrix_set_zero(Assignment);
for(i = 0; i < mask->size1; i++)
{
for(j = 0; j < mask->size2; j++)
{
if(gsl_matrix_uint_get(mask, i, j) == STAR)
gsl_matrix_set(Assignment, i, j, 1);
}
}
}
void StationaryCholesky::computeGammak( const gsl_matrix *Rt, double reg ) {
gsl_matrix_view submat;
for (size_t k = 0; k < getMu(); k++) {
submat = gsl_matrix_submatrix(myGammaK, 0, k * getD(), getD(), getD());
myStStruct->AtVkB(&submat.matrix, k, Rt, Rt, myTempVijtRt);
if (reg > 0) {
gsl_vector diag = gsl_matrix_diagonal(&submat.matrix).vector;
gsl_vector_add_constant(&diag, reg);
}
}
submat = gsl_matrix_submatrix(myGammaK, 0, getMu() * getD(), getD(), getD());
gsl_matrix_set_zero(&submat.matrix);
}
void Compute_NeighborMatrix(gsl_matrix * Neighbors)
{
// RESET THE NEIGHBORING MATRIX
gsl_matrix_set_zero(Neighbors);
// TODO: May we obtain a speedup using vector_view instead
// of matrix_set_row?
gsl_vector * neighborVector = gsl_vector_calloc (27);
for (int cell=0;cell<Mx*My*Mz;cell++)
{
Compute_NeighborCells(cell, neighborVector);
gsl_matrix_set_row(Neighbors, cell, neighborVector);
}
gsl_vector_free(neighborVector);
}
static int
brown3_df (const gsl_vector * x, void *params, gsl_matrix * J)
{
double x1 = gsl_vector_get(x, 0);
double x2 = gsl_vector_get(x, 1);
gsl_matrix_set_zero(J);
gsl_matrix_set(J, 0, 0, 1.0);
gsl_matrix_set(J, 1, 1, 1.0);
gsl_matrix_set(J, 2, 0, x2);
gsl_matrix_set(J, 2, 1, x1);
return GSL_SUCCESS;
}
static int
fdfridge_df(const gsl_vector * x, void * params, gsl_matrix * J)
{
int status;
gsl_multifit_fdfridge *w = (gsl_multifit_fdfridge *) params;
const size_t n = w->n;
const size_t p = w->p;
gsl_matrix_view J_user = gsl_matrix_submatrix(J, 0, 0, n, p);
gsl_matrix_view J_tik = gsl_matrix_submatrix(J, n, 0, p, p);
gsl_vector_view diag = gsl_matrix_diagonal(&J_tik.matrix);
/* compute user supplied Jacobian */
status = gsl_multifit_eval_wdf(w->fdf, x, NULL, &J_user.matrix);
if (status)
return status;
if (w->L_diag)
{
/* store diag(L_diag) in Tikhonov portion of J */
gsl_matrix_set_zero(&J_tik.matrix);
gsl_vector_memcpy(&diag.vector, w->L_diag);
}
else if (w->L)
{
/* store L in Tikhonov portion of J */
gsl_matrix_memcpy(&J_tik.matrix, w->L);
}
else
{
/* store \lambda I_p in Tikhonov portion of J */
gsl_matrix_set_zero(&J_tik.matrix);
gsl_vector_set_all(&diag.vector, w->lambda);
}
return GSL_SUCCESS;
} /* fdfridge_df() */
// Find B s.t. B B^T = A. This is useful for generating vectors from a multivariate normal distribution.
void sqrt_matrix(gsl_matrix* A, gsl_matrix* sqrt_A, gsl_eigen_symmv_workspace* esv, gsl_vector *eival, gsl_matrix *eivec, gsl_matrix* sqrt_eival) {
size_t N = A->size1;
assert(A->size2 == N);
// Allocate workspaces if none are provided
bool del_esv = false;
if(esv == NULL) { esv = gsl_eigen_symmv_alloc(N); del_esv = true; }
bool del_eival = false;
if(eival == NULL) { eival = gsl_vector_alloc(N); del_eival = true; }
bool del_eivec = false;
if(eivec == NULL) { eivec = gsl_matrix_alloc(N, N); del_eival = true; }
bool del_sqrt_eival = false;
if(sqrt_eival == NULL) {
sqrt_eival = gsl_matrix_calloc(N, N);
del_sqrt_eival = true;
} else {
assert(sqrt_eival->size1 == N);
assert(sqrt_eival->size2 == N);
gsl_matrix_set_zero(sqrt_eival);
}
if(sqrt_A == NULL) {
sqrt_A = A;
} else {
assert(sqrt_A->size1 == N);
assert(sqrt_A->size2 == N);
gsl_matrix_memcpy(sqrt_A, A);
}
// Calculate the eigendecomposition of the covariance matrix
gsl_eigen_symmv(sqrt_A, eival, eivec, esv);
double tmp;
for(size_t i=0; i<N; i++) {
tmp = gsl_vector_get(eival, i);
gsl_matrix_set(sqrt_eival, i, i, sqrt(fabs(tmp)));
if(tmp < 0.) {
for(size_t j=0; j<N; j++) { gsl_matrix_set(eivec, j, i, -gsl_matrix_get(eivec, j, i)); }
}
}
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1., eivec, sqrt_eival, 0., sqrt_A);
// Free workspaces if none were provided
if(del_sqrt_eival) { gsl_matrix_free(sqrt_eival); }
if(del_esv) { gsl_eigen_symmv_free(esv); }
if(del_eivec) { gsl_matrix_free(eivec); }
if(del_eival) { gsl_vector_free(eival); }
}
///
/// Decompose a metric \f$\mathbf{G}\f$ as \f$ \mathbf{G} = \mathbf{L} \mathbf{D}
/// \mathbf{L}^{\mathrm{T}} \f$, where \f$\mathbf{L}\f$ is a lower-triangular matrix
/// with unit diagonal, and \f$\mathbf{D}\f$ is a diagonal matrix. This decomposition
/// may be useful if the metric cannot yet be guaranteed to be positive definite.
///
int XLALCholeskyLDLTDecompMetric(
gsl_matrix **p_cholesky, ///< [in,out] Pointer to decomposition; stores \f$\mathbf{L}\f$ in lower triangular part \f$\mathbf{D}\f$ on diagonal
const gsl_matrix *g_ij ///< [in] Matrix to decompose \f$\mathbf{G}\f$
)
{
// Check input
XLAL_CHECK( g_ij != NULL, XLAL_EFAULT );
XLAL_CHECK( g_ij->size1 == g_ij->size2, XLAL_ESIZE );
XLAL_CHECK( p_cholesky != NULL, XLAL_EFAULT );
if ( *p_cholesky != NULL ) {
XLAL_CHECK( (*p_cholesky)->size1 == g_ij->size1, XLAL_ESIZE );
XLAL_CHECK( (*p_cholesky)->size2 == g_ij->size2, XLAL_ESIZE );
} else {
GAMAT( *p_cholesky, g_ij->size1, g_ij->size2 );
}
// Straightforward implementation of Cholesky–Banachiewicz algorithm
gsl_matrix_set_zero( *p_cholesky );
#define A(i,j) *gsl_matrix_const_ptr( g_ij, i, j )
#define D(j) *gsl_matrix_ptr( *p_cholesky, j, j )
#define L(i,j) *gsl_matrix_ptr( *p_cholesky, i, j )
for ( size_t i = 0; i < g_ij->size1; ++i ) {
for ( size_t j = 0; j <= i; ++j ) {
if ( i == j ) {
D(j) = A(j, j);
for ( size_t k = 0; k < j; ++k ) {
D(j) -= L(j, k) * L(j, k) * D(k);
}
} else {
L(i, j) = A(i, j);
for ( size_t k = 0; k < j; ++k ) {
L(i, j) -= L(i, k) * L(j, k) * D(k);
}
L(i, j) /= D(j);
}
}
}
#undef A
#undef D
#undef L
return XLAL_SUCCESS;
}
static int
brown3_df (const gsl_vector * x, void *params, gsl_matrix * J)
{
double x1 = gsl_vector_get(x, 0);
double x2 = gsl_vector_get(x, 1);
gsl_matrix_set_zero(J);
gsl_matrix_set(J, 0, 0, 1.0);
gsl_matrix_set(J, 1, 1, 1.0);
gsl_matrix_set(J, 2, 0, x2);
gsl_matrix_set(J, 2, 1, x1);
(void)params; /* avoid unused parameter warning */
return GSL_SUCCESS;
}
