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;
}
/* test if A Z = Q S */
void
test_eigen_schur(const gsl_matrix * A, const gsl_matrix * S,
const gsl_matrix * Q, const gsl_matrix * Z,
size_t count, const char * desc,
const char * desc2)
{
const size_t N = A->size1;
size_t i, j;
gsl_matrix * T1 = gsl_matrix_alloc(N, N);
gsl_matrix * T2 = gsl_matrix_alloc(N, N);
/* compute T1 = A Z */
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, A, Z, 0.0, T1);
/* compute T2 = Q S */
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Q, S, 0.0, T2);
for (i = 0; i < N; ++i)
{
for (j = 0; j < N; ++j)
{
double x = gsl_matrix_get(T1, i, j);
double y = gsl_matrix_get(T2, i, j);
gsl_test_abs(x, y, 1.0e8 * GSL_DBL_EPSILON,
"%s(N=%u,cnt=%u), %s, schur(%d,%d)", desc, N, count, desc2, i, j);
}
}
gsl_matrix_free (T1);
gsl_matrix_free (T2);
} /* test_eigen_schur() */
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);
}
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::computeGammaSr( const gsl_matrix *Rt, gsl_matrix *PhiTRt,
gsl_vector *Sr, bool regularize_gamma ) {
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, myPhi, Rt, 0, PhiTRt);
myGam->calcGammaCholesky(PhiTRt, regularize_gamma ? myReggamma : 0);
gsl_matrix SrMat = gsl_matrix_view_vector(Sr, getN(), getD()).matrix;
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, myMatr, PhiTRt, 0, &SrMat);
}
void VarproFunction::computeGradFromYr( const gsl_vector* yr,
const gsl_matrix *Rorig, const gsl_matrix *perm, gsl_matrix *gradR ) {
gsl_matrix_const_view yr_matr = gsl_matrix_const_view_vector(yr, getN(), getD());
myDeriv->calcYtDgammaY(myTmpGradR, Rorig, &yr_matr.matrix);
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 2.0, myMatr, &yr_matr.matrix,
-1.0, myTmpGradR);
if (myPhi != NULL) {
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, myPhi, myTmpGradR,
0.0, myTmpGradR2);
} else {
gsl_matrix_memcpy(myTmpGradR2, myTmpGradR);
}
if (perm != NULL) {
if (perm->size1 == getNrow()) {
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, perm, myTmpGradR2, 0.0, gradR);
} else {
gsl_vector vecTmpGradR2 = gsl_vector_view_array(myTmpGradR2->data,
myTmpGradR2->size1 * myTmpGradR2->size2).vector,
vecGradR = gsl_vector_view_array(gradR->data,
gradR->size1 * gradR->size2).vector;
gsl_blas_dgemv(CblasTrans, 1.0, perm, &vecTmpGradR2, 0.0, &vecGradR);
}
} else {
gsl_matrix_memcpy(gradR, myTmpGradR2);
}
}
gsl_matrix *pseudo_inverse(gsl_matrix* input) {
int m = input->size1;
int n = input->size2;
gsl_matrix *U = gsl_matrix_calloc(m,n);
gsl_matrix_memcpy(U, input);
gsl_matrix *V = gsl_matrix_calloc(n,n);
gsl_vector *sigma = gsl_vector_calloc(n);
gsl_vector *tmp = gsl_vector_calloc(n);
gsl_linalg_SV_decomp(U, V, sigma, tmp);
for (int i = 0; i < n; i++) {
double s = gsl_vector_get(sigma, i);
if (s > 0.0000000001) {
gsl_vector_set(sigma, i, 1/s);
} else if (s > 0) {
gsl_vector_set(sigma, i, 0);
}
}
gsl_matrix *tmpa = gsl_matrix_calloc(n,n);
gsl_matrix *tmpb = gsl_matrix_calloc(n,m);
gsl_matrix *tmpc = gsl_matrix_calloc(n,m);
for (int i = 0; i < n; i++) {
gsl_matrix_set(tmpa, i, i, gsl_vector_get(sigma, i));
}
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, tmpa, U, 0, tmpb);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, V, tmpb, 0, tmpc);
return tmpc;
}
// if A is M x N, then A_ps is NxM
void mygsl_linalg_pseudoinverse(const gsl_matrix * A, gsl_matrix * A_ps)
{
size_t M = A->size1, N = A->size2;
// A = U D V' where U is MxN and V is NxN
gsl_matrix * U = gsl_matrix_alloc(M, N), * V = gsl_matrix_alloc(N, N);
gsl_vector * D_diag = gsl_vector_alloc(N),
* work = gsl_vector_alloc(N);
gsl_matrix_memcpy(U, A);
gsl_linalg_SV_decomp(U, V, D_diag, work);
// V D^(-1)
gsl_matrix * VDinv = gsl_matrix_alloc(N, N);
mygsl_vector_pow(D_diag, -1.0);
gsl_matrix * D_inv = mygsl_matrix_diagalloc(D_diag, 0.0);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, V, D_inv, 0.0, VDinv);
// A_ps = VDinv U'
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, VDinv, U, 0.0, A_ps);
gsl_matrix_free(U);
gsl_matrix_free(V);
gsl_vector_free(D_diag);
gsl_vector_free(work);
gsl_matrix_free(VDinv);
gsl_matrix_free(D_inv);
}
/// Assign this matrix to a product of three other matrices
/// @param mult3 :: Matrix multiplication helper object.
GSLMatrix& GSLMatrix::operator=(const GSLMatrixMult3& mult3)
{
// sizes of the result matrix
size_t n1 = mult3.tr1 ? mult3.m_1.size2() : mult3.m_1.size1();
size_t n2 = mult3.tr3 ? mult3.m_3.size1() : mult3.m_3.size2();
this->resize(n1,n2);
// intermediate matrix
GSLMatrix AB( n1, mult3.m_2.size2() );
CBLAS_TRANSPOSE tr1 = mult3.tr1? CblasTrans : CblasNoTrans;
CBLAS_TRANSPOSE tr2 = mult3.tr2? CblasTrans : CblasNoTrans;
CBLAS_TRANSPOSE tr3 = mult3.tr3? CblasTrans : CblasNoTrans;
// AB = m_1 * m_2
gsl_blas_dgemm (tr1, tr2,
1.0, mult3.m_1.gsl(), mult3.m_2.gsl(),
0.0, AB.gsl());
// this = AB * m_3
gsl_blas_dgemm (CblasNoTrans, tr3,
1.0, AB.gsl(), mult3.m_3.gsl(),
0.0, gsl());
return *this;
}
static int
test_COD_decomp_eps(const gsl_matrix * m, const double eps, const char *desc)
{
int s = 0;
size_t i, j, M = m->size1, N = m->size2;
size_t rank;
gsl_matrix * QRZT = gsl_matrix_alloc(M, N);
gsl_matrix * Q = gsl_matrix_alloc(M, M);
gsl_matrix * R = gsl_matrix_alloc(M, N);
gsl_matrix * QR = gsl_matrix_alloc(M, N);
gsl_matrix * Z = gsl_matrix_alloc(N, N);
gsl_vector * tau_Q = gsl_vector_alloc(GSL_MIN(M, N));
gsl_vector * tau_Z = gsl_vector_alloc(GSL_MIN(M, N));
gsl_vector * work = gsl_vector_alloc(N);
gsl_matrix * lhs = gsl_matrix_alloc(M, N);
gsl_matrix * rhs = gsl_matrix_alloc(M, N);
gsl_permutation * perm = gsl_permutation_alloc(N);
gsl_matrix_memcpy(QRZT, m);
s += gsl_linalg_COD_decomp(QRZT, tau_Q, tau_Z, perm, &rank, work);
s += gsl_linalg_COD_unpack(QRZT, tau_Q, tau_Z, rank, Q, R, Z);
/* compute lhs = m P */
gsl_matrix_memcpy(lhs, m);
gsl_permute_matrix(perm, lhs);
/* compute rhs = Q R Z^T */
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, Q, R, 0.0, QR);
gsl_blas_dgemm (CblasNoTrans, CblasTrans, 1.0, QR, Z, 0.0, rhs);
for (i = 0; i < M; i++)
{
for (j = 0; j < N; j++)
{
double aij = gsl_matrix_get(rhs, i, j);
double bij = gsl_matrix_get(lhs, i, j);
gsl_test_rel(aij, bij, eps, "%s (%3lu,%3lu)[%lu,%lu]: %22.18g %22.18g\n",
desc, M, N, i, j, aij, bij);
}
}
gsl_permutation_free (perm);
gsl_vector_free(work);
gsl_vector_free(tau_Q);
gsl_vector_free(tau_Z);
gsl_matrix_free(QRZT);
gsl_matrix_free(lhs);
gsl_matrix_free(rhs);
gsl_matrix_free(QR);
gsl_matrix_free(Q);
gsl_matrix_free(R);
gsl_matrix_free(Z);
return s;
}
/*Cuad. Form C = b^T A b*/
double CuadForm(Matrix &A, Matrix &b) {
//Make the auxiliar (vector) matrix
Matrix Aux(A.nRow());
Matrix res(1);
gsl_blas_dgemm( CblasNoTrans, CblasNoTrans, 1.0, A.Ma(), b.Ma(), 0.0, Aux.Ma());
gsl_blas_dgemm( CblasTrans, CblasNoTrans, 1.0, b.Ma(), Aux.Ma(), 0.0, res.Ma());
return res(0);
}
/* P = U*S*V^T */
void form_svd_product_matrix(gsl_matrix *U, gsl_matrix *S, gsl_matrix *V, gsl_matrix *P){
int k,m,n;
m = P->size1;
n = P->size2;
k = S->size1;
gsl_matrix * SVt = gsl_matrix_alloc(k,n);
// form Svt = S*V^T
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, S, V, 0.0, SVt);
// form P = U*S*V^T
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, U, SVt, 0.0, P);
}
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
kalman_meas (Kalman * k, const double * z, int M, double dt,
KalmanMeasFunc meas_func, KalmanMeasJacobFunc meas_jacob_func,
KalmanMeasCovFunc meas_cov_func)
{
kalman_pred (k, dt);
double K[k->N * M];
double PHt[k->N * M];
double H[M * k->N];
double R[M * M];
double I[M * M];
double h[M];
gsl_matrix_view Kv = gsl_matrix_view_array (K, k->N, M);
gsl_matrix_view PHtv = gsl_matrix_view_array (PHt, k->N, M);
gsl_matrix_view Hv = gsl_matrix_view_array (H, M, k->N);
gsl_matrix_view Rv = gsl_matrix_view_array (R, M, M);
gsl_matrix_view Iv = gsl_matrix_view_array (I, M, M);
gsl_vector_view hv = gsl_vector_view_array (h, M);
meas_jacob_func (H, M, k->x, k->N, k->user);
/* K = P_*H'*inv(H*P_*H' + R) */
gsl_blas_dgemm (CblasNoTrans, CblasTrans, 1.0, &k->Pv.matrix,
&Hv.matrix, 0.0, &PHtv.matrix);
meas_cov_func (R, M, k->user);
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, &Hv.matrix,
&PHtv.matrix, 1.0, &Rv.matrix);
size_t permv[M];
gsl_permutation perm = { M, permv };
int signum;
gsl_linalg_LU_decomp (&Rv.matrix, &perm, &signum);
gsl_linalg_LU_invert (&Rv.matrix, &perm, &Iv.matrix);
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, &PHtv.matrix,
&Iv.matrix, 0.0, &Kv.matrix);
/* x = x + K*(z - h(x)) */
meas_func (h, M, k->x, k->N, k->user);
vector_sub_nd (z, h, M, h);
gsl_blas_dgemv (CblasNoTrans, 1.0, &Kv.matrix, &hv.vector, 1.0,
&k->xv.vector);
/* P = P_ - K*H*P_ */
gsl_blas_dgemm (CblasNoTrans, CblasTrans, -1.0, &Kv.matrix,
&PHtv.matrix, 1.0, &k->Pv.matrix);
return 0;
}
void SeriesSmoother::computeCoeffs()
{
gsl_matrix * X = gsl_matrix_alloc( m_pointCount, m_coeffCount );
// Shift between the x index and the row number
int delta = (m_pointCount-1)/2;
for( int i = 0; i < m_pointCount; ++i )
for (int j = 0; j < m_coeffCount; ++j)
gsl_matrix_set( X, i, j, pow( i-delta, j ) );
// [X]^T [X]
gsl_matrix * XtX = gsl_matrix_alloc( m_coeffCount, m_coeffCount );
gsl_blas_dgemm( CblasTrans, CblasNoTrans, 1, X, X, 0, XtX );
// ([X]^T [X])^-1
gsl_matrix * XtXi = gsl_matrix_alloc( m_coeffCount, m_coeffCount );
int iXtXi; //the calculation needs an int pointer
gsl_permutation * pXtX = gsl_permutation_alloc( m_coeffCount );
gsl_linalg_LU_decomp( XtX, pXtX, &iXtXi );
gsl_linalg_LU_invert( XtX, pXtX, XtXi );
// [X] ([X]^t [X])^-1
gsl_matrix * XXtXi = gsl_matrix_alloc( m_pointCount, m_coeffCount );
gsl_blas_dgemm( CblasNoTrans, CblasNoTrans, 1, X, XtXi, 0, XXtXi );
// [coeffs] = [X] ([X]^t [X])^-1 [X]^t
gsl_matrix * coeffs = gsl_matrix_alloc( m_pointCount, m_pointCount );
gsl_blas_dgemm( CblasNoTrans, CblasTrans, 1, XXtXi, X, 0, coeffs );
// Move the coeffs matrix to m_coeffs
m_coeffs.resize(m_pointCount);
for (int i = 0; i < m_coeffs.size(); ++i) {
m_coeffs[i].resize(m_pointCount);
for (int j = 0; j < m_pointCount; ++j)
m_coeffs[i][j] = gsl_matrix_get( coeffs, i, j );
}
// Delete the matrices
gsl_matrix_free(X);
gsl_matrix_free(XtX);
gsl_matrix_free(XtXi);
gsl_permutation_free(pXtX);
gsl_matrix_free(XXtXi);
gsl_matrix_free(coeffs);
}
static gsl_matrix *augmented_param_mat_A(gsl_matrix *R11, gsl_matrix *R12)
{
gsl_matrix *t_aug_A, *aug_A, *LU, *R11_inv;
gsl_permutation *p;
int signum;
p = gsl_permutation_alloc(R11->size2);
LU = gsl_matrix_alloc(R11->size1, R11->size2);
gsl_matrix_memcpy(LU, R11);
R11_inv = gsl_matrix_alloc(R11->size1, R11->size2);
gsl_linalg_LU_decomp(LU, p, &signum);
gsl_linalg_LU_invert(LU, p, R11_inv);
aug_A = gsl_matrix_alloc(R12->size1, R12->size2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, R11_inv, R12, 0.0, aug_A);
t_aug_A = gsl_matrix_alloc(aug_A->size2, aug_A->size1);
gsl_matrix_transpose_memcpy(t_aug_A, aug_A);
gsl_permutation_free(p);
gsl_matrix_free(LU);
gsl_matrix_free(aug_A);
gsl_matrix_free(R11_inv);
return t_aug_A;
}
double iwishpdf(gsl_matrix *X, gsl_matrix *Scale, gsl_matrix *inv, double dof)
{
double X_det, scale_det, denom, pdf, trace, numerator;
int m = X->size1;
int n = X->size2;
// Allocate matrix for inv(X)
gsl_matrix *for_mult = gsl_matrix_alloc(m, n);
// Get determinant of X and Scale matrix
X_det = matrix_determ(X);
scale_det = matrix_determ(Scale);
// Invert X
inv_matrix(X,inv);
// Multiple Scale * inv(X)
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Scale, inv, 1.0, for_mult);
// Get trace of above.
trace = matrix_trace(for_mult);
numerator = pow(scale_det, dof / 2.0) * pow(X_det, (-dof-m-1)/ 2.0) * exp(-0.5 * trace);
denom = pow(2,dof * m / 2) * mv_gamma(dof/2, m);
pdf = (numerator/denom);
return(pdf);
}
matrix operator*(const matrix& m1, const matrix& m2) {
if (m1.ptr_->size2 != m2.ptr_->size1) {
std::stringstream ss("");
ss << "incompatible matrix m1("
<< m1.ptr_->size1
<< ", "
<< m1.ptr_->size2
<< ") * m2("
<< m2.ptr_->size1
<< ", "
<< m2.ptr_->size2
<< ")";
throw std::runtime_error(ss.str());
}
matrix out(m1.ptr_->size1, m2.ptr_->size2);
gsl_blas_dgemm(
CblasNoTrans,
CblasNoTrans,
1.0,
m1.ptr_,
m2.ptr_,
0.0,
out.ptr_);
return out;
}
EigenCovariance3 EigenCovariance3::rotate(double angle) const{
static gsl_matrix * m_rmat=NULL;
static gsl_matrix * m_vmat=NULL;
static gsl_matrix * m_result=NULL;
if (m_rmat==NULL){
m_rmat=gsl_matrix_alloc(3,3);
m_vmat=gsl_matrix_alloc(3,3);
m_result=gsl_matrix_alloc(3,3);
}
double c=cos(angle);
double s=sin(angle);
gsl_matrix_set(m_rmat,0,0, c ); gsl_matrix_set(m_rmat,0,1, -s); gsl_matrix_set(m_rmat,0,2, 0.);
gsl_matrix_set(m_rmat,1,0, s ); gsl_matrix_set(m_rmat,1,1, c); gsl_matrix_set(m_rmat,1,2, 0.);
gsl_matrix_set(m_rmat,2,0, 0.); gsl_matrix_set(m_rmat,2,1, 0.); gsl_matrix_set(m_rmat,2,2, 1.);
for (unsigned int i=0; i<3; i++)
for (unsigned int j=0; j<3; j++)
gsl_matrix_set(m_vmat,i,j,evec[i][j]);
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1., m_rmat, m_vmat, 0., m_result);
EigenCovariance3 ecov(*this);
for (int i=0; i<3; i++){
for (int j=0; j<3; j++)
ecov.evec[i][j]=gsl_matrix_get(m_result,i,j);
}
return ecov;
}
/* test if A is orthogonal */
int
test_eigen_orthog(gsl_matrix *A, size_t count, const char *desc,
const char *desc2)
{
size_t N = A->size1;
gsl_matrix *M = gsl_matrix_alloc(A->size1, A->size2);
size_t i, j;
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, A, A, 0.0, M);
for (i = 0; i < A->size1; ++i)
{
for (j = 0; j < A->size2; ++j)
{
double val;
double mij = gsl_matrix_get(M, i, j);
if (i == j)
val = 1.0;
else
val = 0.0;
gsl_test_abs(mij, val, 1.0e8 * GSL_DBL_EPSILON,
"%s(N=%u,cnt=%u), %s, orthog(%d,%d)", desc, N, count, desc2, i, j);
}
}
gsl_matrix_free(M);
return 1;
} /* test_eigen_orthog() */
// 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);
}
//C = A^trA B^trB
int MatMul(Matrix &C, int trA, Matrix &A, int trB, Matrix &B) {
CBLAS_TRANSPOSE_t TransA = ((trA == 0) ? CblasNoTrans : CblasTrans);
CBLAS_TRANSPOSE_t TransB = ((trB == 0) ? CblasNoTrans : CblasTrans);
return gsl_blas_dgemm( TransA, TransB, 1.0, A.Ma(), B.Ma(), 0.0, C.Ma());
}
int
main (void)
{
double a[] = { 0.11, 0.12, 0.13,
0.21, 0.22, 0.23 };
double b[] = { 1011, 1012,
1021, 1022,
1031, 1032 };
double c[] = { 0.00, 0.00,
0.00, 0.00 };
gsl_matrix_view A = gsl_matrix_view_array(a, 2, 3);
gsl_matrix_view B = gsl_matrix_view_array(b, 3, 2);
gsl_matrix_view C = gsl_matrix_view_array(c, 2, 2);
/* Compute C = A B */
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
1.0, &A.matrix, &B.matrix,
0.0, &C.matrix);
printf ("[ %g, %g\n", c[0], c[1]);
printf (" %g, %g ]\n", c[2], c[3]);
return 0;
}
//Wishart distribution random number generator
int ran_wishart(const gsl_rng *r, const double nu,
const gsl_matrix *V, gsl_matrix *X)
{
const int k = V->size1;
int i, j;
gsl_matrix *A = gsl_matrix_calloc(k, k);
gsl_matrix *L = gsl_matrix_alloc(k, k);
for(i=0; i<k; i++)
{
gsl_matrix_set(A, i, i, sqrt(gsl_ran_chisq(r, (nu-i))));
for (j=0; j<i; j++){
gsl_matrix_set(A, i, j, gsl_ran_gaussian(r, 1));
}
}
gsl_matrix_memcpy(L, V);
gsl_linalg_cholesky_decomp(L);
gsl_blas_dtrmm(CblasLeft, CblasLower, CblasNoTrans, CblasNonUnit, 1.0, L, A);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, A, A, 0.0, X);
gsl_matrix_free(A);
gsl_matrix_free(L);
return 0;
}
int main(int argc, char **argv){
FILE *fp;
if(argc != 2) {
printf("Niepoprawna liczba argumentów!\n");
exit(0);
}
N = atoi(argv[1]);
int **A, **B, **C;
A = calloc(N, sizeof(int*));
B = calloc(N, sizeof(int*));
C = calloc(N, sizeof(int*));
for(i = 0; i<N; i++){
A[i] = calloc(N, sizeof(int));
B[i] = calloc(N, sizeof(int));
C[i] = calloc(N, sizeof(int));
}
gsl_matrix *matrix1 = gsl_matrix_calloc(N, N);
gsl_matrix *matrix2 = gsl_matrix_calloc(N, N);
gsl_matrix *result = gsl_matrix_calloc(N, N);
CBLAS_TRANSPOSE_t TransA = CblasNoTrans;
srand(time(NULL));
fp = fopen("result.txt", "a");
//fill matrix
for(i=0; i<N; i++){
for(j=0; j<N; j++){
tmp = rand() % 100;
A[i][j] = tmp;
gsl_matrix_set(matrix1, j, k, tmp);
tmp = rand() % 100;
B[i][j] = tmp;
gsl_matrix_set(matrix2, j, k, tmp);
}
}
for(l = 0; l < 20; l++){
//algorithms
start = clock();
naive(A,B,C);
fprintf(fp, "%d,alg1,%f\n", N, (double)(clock() - start)/CLOCKS_PER_SEC);
//printf("Algorytm 1: %g [s]\n", (double)(clock() - start)/CLOCKS_PER_SEC);
start = clock();
ver2(A,B,C);
fprintf(fp, "%d,alg2,%f\n", N, (double)(clock() - start)/CLOCKS_PER_SEC);
//printf("Algorytm 2: %g [s]\n", (double)(clock() - start)/CLOCKS_PER_SEC);
start = clock();
gsl_blas_dgemm (TransA, TransA, 1, matrix1, matrix2, 1, result);
fprintf(fp, "%d,blas,%f\n", N, (double)(clock() - start)/CLOCKS_PER_SEC);
//printf("Algorytm 3: %g [s]\n", (double)(clock() - start)/CLOCKS_PER_SEC);
}
return 0;
}
//gsl_matrix * gsl_matrix_mult(gsl_matrix *a, gsl_matrix *b)
gsl_matrix * gsl_matrix_mult(const gsl_matrix *a, const gsl_matrix *b)
{
gsl_matrix *c;
c = gsl_matrix_alloc(a->size1, b->size2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, a, b, 0.0, c);
return c;
}
val egsl_mult(val v1, val v2){
gsl_matrix * a = egsl_gslm(v1);
gsl_matrix * b = egsl_gslm(v2);
val v = egsl_alloc(a->size1,b->size2);
gsl_matrix * ab = egsl_gslm(v);
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,a,b,0.0,ab);
return v;
}
/* 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 is_mirror_image(double* X, int X_dim0, int X_dim1, int X_dim1_mem,
double* Y, int Y_dim0, int Y_dim1, int Y_dim1_mem) {
// Check if two configurations X and Y are mirror images
// (i.e. does their optimal superposition involve a reflection?)
//
// Parameters
// ----------
// X : double*, shape=(X_dim0, X_dim1)
// Pointer to the upper left corner of matrix X.
// X_dim0 : int
// The number of rows in matrix X. Should be 3.
// X_dim1 : int
// The number of columns in matrix X. Corresponds to number of atoms
// X_dim1_mem : int
// number of columns of X in memory. corresponds to the number of padded atoms.
// such that the (i,j)-th element of X is accessed at X[i*X_dim1*mem + j]
// Y : double*, shape=(X_dim0, X_dim1)
// Pointer to the upper left corner of matrix X.
// Y_dim0 : int
// The number of rows in matrix Y. Should be 3.
// Y_dim1 : int
// The number of columns in matrix Y. Corresponds to number of atoms
// Y_dim1_mem : int
// number of columns of Y in memory. corresponds to the number of padded atoms.
// such that the (i,j)-th element of Y is accessed at Y[i*Y_dim1*mem + j] //
//
// Returns
// -------
// mirror : int
// = 1 if they are mirror images
// = 0 if they are not mirror images
if ((X_dim0 != Y_dim0) || (X_dim1 != Y_dim1) || (X_dim0 != 3)){
fprintf(stderr, "is_mirror_image called with wrong shape\n");
exit(1);
}
// covariance = np.dot(X, Y.T)
gsl_matrix* covariance = gsl_matrix_alloc(3, 3);
gsl_matrix_view mX = gsl_matrix_view_array_with_tda(X, X_dim0, X_dim1, X_dim1_mem);
gsl_matrix_view mY = gsl_matrix_view_array_with_tda(Y, Y_dim0, Y_dim1, Y_dim1_mem);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &mX.matrix, &mY.matrix, 0.0, covariance);
gsl_matrix* U = gsl_matrix_alloc(3, 3);
gsl_vector* S = gsl_vector_alloc(3);
gsl_vector* work = gsl_vector_alloc(3);
gsl_linalg_SV_decomp(covariance, U, S, work);
double determinant = ddet(covariance->data) * ddet(U->data);
gsl_matrix_free(covariance);
gsl_matrix_free(U);
gsl_vector_free(S);
gsl_vector_free(work);
return determinant < 0;
}
///
/// Apply a transform \f$\mathbf{A} = (a_{ij})\f$ to a metric \f$\mathbf{G} = (g_{ij})\f$ such that
/// \f$ \mathbf{G} \rightarrow \mathbf{G}^{\prime} = \mathbf{A}^{\mathrm{T}} \mathbf{G} \mathbf{A}
/// \f$, or equivalently \f$ g_{ij} \rightarrow g^{\prime}_{kl} = g_{ij} a_{ik} a_{jl} \f$.
///
/// \note \c *p_gpr_ij will be allocated if \c NULL. \c *p_gpr_ij and \c g_ij may point to the same
/// matrix.
///
int XLALTransformMetric(
gsl_matrix **p_gpr_ij, ///< [in,out] Pointer to transformed matrix \f$\mathbf{G}^{\prime}\f$
const gsl_matrix *transform, ///< [in] Transform to apply \f$\mathbf{A}\f$
const gsl_matrix *g_ij ///< [in] Matrix to transform \f$\mathbf{G}\f$
)
{
// Check input
XLAL_CHECK( g_ij != NULL, XLAL_EFAULT );
XLAL_CHECK( transform != NULL, XLAL_EFAULT );
XLAL_CHECK( g_ij->size1 == g_ij->size2, XLAL_ESIZE );
XLAL_CHECK( g_ij->size2 == transform->size1, XLAL_ESIZE );
XLAL_CHECK( p_gpr_ij != NULL, XLAL_EFAULT );
if ( *p_gpr_ij != NULL ) {
XLAL_CHECK( (*p_gpr_ij)->size1 == transform->size2, XLAL_ESIZE );
XLAL_CHECK( (*p_gpr_ij)->size2 == transform->size2, XLAL_ESIZE );
} else {
GAMAT( *p_gpr_ij, transform->size2, transform->size2 );
}
// Allocate temporary matrix
gsl_matrix *tmp = gsl_matrix_alloc( g_ij->size1, transform->size2 );
XLAL_CHECK( tmp != NULL, XLAL_ENOMEM );
// Perform transform
gsl_blas_dgemm( CblasNoTrans, CblasNoTrans, 1.0, g_ij, transform, 0.0, tmp );
gsl_blas_dgemm( CblasTrans, CblasNoTrans, 1.0, transform, tmp, 0.0, *p_gpr_ij );
// Ensure transformed g_ij is exactly symmetric
for( size_t i = 0; i < (*p_gpr_ij)->size1; ++i ) {
for( size_t j = i + 1; j < (*p_gpr_ij)->size2; ++j ) {
const double gij = gsl_matrix_get( *p_gpr_ij, i, j );
const double gji = gsl_matrix_get( *p_gpr_ij, j, i );
const double g = 0.5 * ( gij + gji );
gsl_matrix_set( *p_gpr_ij, i, j, g );
gsl_matrix_set( *p_gpr_ij, j, i, g );
}
}
// Cleanup
gsl_matrix_free( tmp );
return XLAL_SUCCESS;
} // XLALTransformMetric()
