/* Calculate Hamiltonian energy equation (13) of Girolami and Calderhead (2011).
* Arguments:
* N: number of parameters.
* Lx: unormalised posterior density at current parameter values.
* cholM: cholesky factor of the metric tensor evaluated at current parameter values.
* invM: inverse of metric tensor evaluated at current parameter values.
* p: value of the momentum parameters.
* state: a pointer to internal working storage for RMHMC.
* Result:
* returns the log of the Hamiltonian energy.
*/
static double calculateHamiltonian(int N, double Lx, const double* cholM, const double* invM,
const double* p, const rmhmc_params* state){
double logDetM = 0;
int d;
gsl_vector_view a_v = gsl_vector_view_array(state->atmp, N);
gsl_vector_const_view p_v = gsl_vector_const_view_array(p, N);
gsl_matrix_const_view invM_v = gsl_matrix_const_view_array(invM, N, N);
gsl_matrix_const_view cholM_v = gsl_matrix_const_view_array(cholM, N, N);
//gsl_blas_dsymv(CblasUpper, 1.0, &invM_v.matrix, &p_v.vector, 0.0, &a_v.vector);
gsl_blas_dgemv(CblasNoTrans, 1.0, &invM_v.matrix, &p_v.vector, 0.0, &a_v.vector);
for (d = 0; d < N; d++)
logDetM += log(gsl_matrix_get(&cholM_v.matrix, d, d));
double quadTerm = 0;
gsl_blas_ddot(&p_v.vector, &a_v.vector, &quadTerm);
return Lx - logDetM - 0.5*quadTerm;
}
int gslutils_invert_3x3(const double* A, double* B) {
gsl_matrix* LU;
gsl_permutation *p;
gsl_matrix_view mB;
int rtn = 0;
int signum;
p = gsl_permutation_alloc(3);
gsl_matrix_const_view mA = gsl_matrix_const_view_array(A, 3, 3);
mB = gsl_matrix_view_array(B, 3, 3);
LU = gsl_matrix_alloc(3, 3);
gsl_matrix_memcpy(LU, &mA.matrix);
if (gsl_linalg_LU_decomp(LU, p, &signum) ||
gsl_linalg_LU_invert(LU, p, &mB.matrix)) {
ERROR("gsl_linalg_LU_decomp() or _invert() failed.");
rtn = -1;
}
gsl_permutation_free(p);
gsl_matrix_free(LU);
return rtn;
}
static int CheckSuperskyMetrics(
const gsl_matrix *rssky_metric,
const double rssky_metric_ref[4][4],
const gsl_matrix *rssky_transf,
const double rssky_transf_ref[6][3],
const double phys_mismatch[NUM_POINTS][NUM_POINTS],
const double phys_mismatch_tol
)
{
// Check supersky metrics
{
gsl_matrix_const_view rssky_metric_ref_view = gsl_matrix_const_view_array( ( const double * )rssky_metric_ref, 4, 4 );
const double err = XLALCompareMetrics( rssky_metric, &rssky_metric_ref_view.matrix ), err_tol = 1e-6;
XLAL_CHECK( err <= err_tol, XLAL_ETOL, "'rssky_metric' check failed: err = %0.3e > %0.3e = err_tol", err, err_tol );
}
{
XLAL_CHECK( rssky_transf->size1 == 6 && rssky_transf->size2 == 3, XLAL_ESIZE );
const double err_tol = 1e-5;
for ( size_t i = 0; i < rssky_transf->size1; ++i ) {
for ( size_t j = 0; j < rssky_transf->size2; ++j ) {
const double rssky_transf_ij = gsl_matrix_get( rssky_transf, i, j );
const double rssky_transf_ref_ij = rssky_transf_ref[i][j];
CHECK_RELERR( rssky_transf_ij, rssky_transf_ref_ij, err_tol );
}
}
}
// Check round-trip conversions of each test point
{
gsl_matrix *GAMAT( rssky_points, 4, NUM_POINTS );
for ( size_t j = 0; j < NUM_POINTS; ++j ) {
gsl_vector_view rssky_point = gsl_matrix_column( rssky_points, j );
PulsarDopplerParams XLAL_INIT_DECL( new_phys_point );
XLAL_CHECK( XLALConvertPhysicalToSuperskyPoint( &rssky_point.vector, &phys_points[j], rssky_transf ) == XLAL_SUCCESS, XLAL_EFUNC );
XLAL_CHECK( XLALConvertSuperskyToPhysicalPoint( &new_phys_point, &rssky_point.vector, rssky_transf ) == XLAL_SUCCESS, XLAL_EFUNC );
XLAL_CHECK( CompareDoppler( &phys_points[j], &new_phys_point ) == EXIT_SUCCESS, XLAL_EFUNC );
}
gsl_matrix *intm_phys_points = NULL;
gsl_matrix *new_rssky_points = NULL;
XLAL_CHECK( XLALConvertSuperskyToPhysicalPoints( &intm_phys_points, rssky_points, rssky_transf ) == XLAL_SUCCESS, XLAL_EFUNC );
XLAL_CHECK( XLALConvertPhysicalToSuperskyPoints( &new_rssky_points, intm_phys_points, rssky_transf ) == XLAL_SUCCESS, XLAL_EFUNC );
const double err_tol = 1e-6;
for ( size_t i = 0; i < 4; ++i ) {
for ( size_t j = 0; j < NUM_POINTS; ++j ) {
const double rssky_points_ij = gsl_matrix_get( rssky_points, i, j );
const double new_rssky_points_ij = gsl_matrix_get( new_rssky_points, i, j );
CHECK_RELERR( rssky_points_ij, new_rssky_points_ij, err_tol );
}
}
GFMAT( rssky_points, intm_phys_points, new_rssky_points );
}
// Check mismatches between pairs of points
{
gsl_vector *GAVEC( rssky_point_i, 4 );
gsl_vector *GAVEC( rssky_point_j, 4 );
gsl_vector *GAVEC( temp, 4 );
for ( size_t i = 0; i < NUM_POINTS; ++i ) {
XLAL_CHECK( XLALConvertPhysicalToSuperskyPoint( rssky_point_i, &phys_points[i], rssky_transf ) == XLAL_SUCCESS, XLAL_EFUNC );
for ( size_t j = 0; j < NUM_POINTS; ++j ) {
XLAL_CHECK( XLALConvertPhysicalToSuperskyPoint( rssky_point_j, &phys_points[j], rssky_transf ) == XLAL_SUCCESS, XLAL_EFUNC );
gsl_vector_sub( rssky_point_j, rssky_point_i );
gsl_blas_dgemv( CblasNoTrans, 1.0, rssky_metric, rssky_point_j, 0.0, temp );
double mismatch = 0.0;
gsl_blas_ddot( rssky_point_j, temp, &mismatch );
CHECK_RELERR( mismatch, phys_mismatch[i][j], phys_mismatch_tol );
}
}
GFVEC( rssky_point_i, rssky_point_j, temp );
}
return XLAL_SUCCESS;
}
int
ssc_inverse (double X_ji[6], double J_minus[6*6], const double X_ij[6])
{
GSLU_MATRIX_VIEW (R_ij, 3, 3);
so3_rotxyz (R_ij.data, SSC_RPH (X_ij));
// X_ji
const double h_ji = atan2 (R_ij(0,1), R_ij(0,0));
#ifdef _GNU_SOURCE
double sh_ji, ch_ji;
sincos (h_ji, &sh_ji, &ch_ji);
#else
double sh_ji = sin (h_ji), ch_ji = cos(h_ji);
#endif
const double x_ji = -(R_ij(0,0)*X_ij(0) + R_ij(1,0)*X_ij(1) + R_ij(2,0)*X_ij(2));
const double y_ji = -(R_ij(0,1)*X_ij(0) + R_ij(1,1)*X_ij(1) + R_ij(2,1)*X_ij(2));
const double z_ji = -(R_ij(0,2)*X_ij(0) + R_ij(1,2)*X_ij(1) + R_ij(2,2)*X_ij(2));
const double r_ji = atan2 (R_ij(2,0)*sh_ji - R_ij(2,1)*ch_ji, -R_ij(1,0)*sh_ji + R_ij(1,1)*ch_ji);
const double p_ji = atan2 (-R_ij(0,2), R_ij(0,0)*ch_ji + R_ij(0,1)*sh_ji);
X_ji(SSC_DOF_X) = x_ji;
X_ji(SSC_DOF_Y) = y_ji;
X_ji(SSC_DOF_Z) = z_ji;
X_ji(SSC_DOF_R) = r_ji;
X_ji(SSC_DOF_P) = p_ji;
X_ji(SSC_DOF_H) = h_ji;
if (J_minus != NULL) {
const double x_ij=X_ij(SSC_DOF_X), y_ij=X_ij(SSC_DOF_Y), z_ij=X_ij(SSC_DOF_Z);
const double r_ij=X_ij(SSC_DOF_R), p_ij=X_ij(SSC_DOF_P), h_ij=X_ij(SSC_DOF_H);
#ifdef _GNU_SOURCE
double sr_ij, cr_ij, sp_ij, cp_ij, sh_ij, ch_ij;
sincos (r_ij, &sr_ij, &cr_ij);
sincos (p_ij, &sp_ij, &cp_ij);
sincos (h_ij, &sh_ij, &ch_ij);
#else
double sr_ij = sin (r_ij), cr_ij = cos (r_ij);
double sp_ij = sin (p_ij), cp_ij = cos (p_ij);
double sh_ij = sin (h_ij), ch_ij = cos (h_ij);
#endif
// N
double tmp = x_ij*ch_ij + y_ij*sh_ij;
const double N_data[] = {
0, -R_ij(2,0)*tmp + z_ij*cp_ij, R_ij(1,0)*x_ij - R_ij(0,0)*y_ij,
z_ji, -R_ij(2,1)*tmp + z_ij*sp_ij*sr_ij, R_ij(1,1)*x_ij - R_ij(0,1)*y_ij,
-y_ji, -R_ij(2,2)*tmp + z_ij*sp_ij*cr_ij, R_ij(1,2)*x_ij - R_ij(0,2)*y_ij
};
gsl_matrix_const_view N = gsl_matrix_const_view_array (N_data, 3, 3);
// Q
tmp = sqrt (1-R_ij(0,2)*R_ij(0,2));
double Q_data[] = {
-R_ij(0,0), -R_ij(0,1)*cr_ij, R_ij(0,2)*R_ij(2,2),
R_ij(0,1)*tmp, -R_ij(2,2)*ch_ij*tmp, R_ij(1,2)*tmp,
R_ij(0,2)*R_ij(0,0), -R_ij(1,2)*ch_ij, -R_ij(2,2)
};
gsl_matrix_view Q = gsl_matrix_view_array (Q_data, 3, 3);
gsl_matrix_scale (&Q.matrix, 1./(tmp*tmp));
// J_minus
gsl_matrix_view J = gsl_matrix_view_array (J_minus, 6, 6);
gsl_matrix_set_zero (&J.matrix);
gsl_matrix_transpose (&R_ij.matrix);
gsl_matrix_scale (&R_ij.matrix, -1.0);
gslu_matrix_set_submatrix (&J.matrix, 0, 0, &R_ij.matrix); // -R_ij'
gslu_matrix_set_submatrix (&J.matrix, 0, 3, &N.matrix);
gslu_matrix_set_submatrix (&J.matrix, 3, 3, &Q.matrix);
}
return GSL_SUCCESS;
}
int
ssc_head2tail (double X_ik[6], double J_plus[6*12], const double X_ij[6], const double X_jk[6])
{
// R_ij, R_jk, R_ik
GSLU_MATRIX_VIEW (R_ij, 3, 3);
GSLU_MATRIX_VIEW (R_jk, 3, 3);
GSLU_MATRIX_VIEW (R_ik, 3, 3);
so3_rotxyz (R_ij.data, SSC_RPH (X_ij));
so3_rotxyz (R_jk.data, SSC_RPH (X_jk));
gslu_blas_mm (&R_ik.matrix, &R_ij.matrix, &R_jk.matrix);
// X_ij
const double x_ij = SSC_X(X_ij), y_ij = SSC_Y(X_ij), z_ij = SSC_Z(X_ij);
const double r_ij = SSC_R(X_ij), p_ij = SSC_P(X_ij), h_ij = SSC_H(X_ij);
// X_jk
const double x_jk = SSC_X(X_jk), y_jk = SSC_Y(X_jk), z_jk = SSC_Z(X_jk);
const double r_jk = SSC_R(X_jk), p_jk = SSC_P(X_jk); //, h_jk = SSC_H(X_jk);
// X_ik
const double x_ik = R_ij(0,0)*x_jk + R_ij(0,1)*y_jk + R_ij(0,2)*z_jk + x_ij;
const double y_ik = R_ij(1,0)*x_jk + R_ij(1,1)*y_jk + R_ij(1,2)*z_jk + y_ij;
const double z_ik = R_ij(2,0)*x_jk + R_ij(2,1)*y_jk + R_ij(2,2)*z_jk + z_ij;
const double h_ik = atan2 (R_ik(1,0), R_ik(0,0));
#ifdef _GNU_SOURCE
double sh_ik, ch_ik;
sincos (h_ik, &sh_ik, &ch_ik);
#else
double sh_ik = sin (h_ik), ch_ik = cos (h_ik);
#endif
const double r_ik = atan2 (R_ik(0,2)*sh_ik - R_ik(1,2)*ch_ik, -R_ik(0,1)*sh_ik + R_ik(1,1)*ch_ik);
const double p_ik = atan2 (-R_ik(2,0), R_ik(0,0)*ch_ik + R_ik(1,0)*sh_ik);
X_ik(SSC_DOF_X) = x_ik;
X_ik(SSC_DOF_Y) = y_ik;
X_ik(SSC_DOF_Z) = z_ik;
X_ik(SSC_DOF_R) = r_ik;
X_ik(SSC_DOF_P) = p_ik;
X_ik(SSC_DOF_H) = h_ik;
if (J_plus != NULL) {
#ifdef _GNU_SOURCE
double sr_ij, cr_ij;
sincos (r_ij, &sr_ij, &cr_ij);
double sp_ij, cp_ij;
sincos (p_ij, &sp_ij, &cp_ij);
double sh_ij, ch_ij;
sincos (h_ij, &sh_ij, &ch_ij);
double sp_jk, cp_jk;
sincos (p_jk, &sp_jk, &cp_jk);
double sr_ik, cr_ik;
sincos (r_ik, &sr_ik, &cr_ik);
double sp_ik, cp_ik;
sincos (p_ik, &sp_ik, &cp_ik);
double sh_dif, ch_dif;
sincos (h_ik-h_ij, &sh_dif, &ch_dif);
double sr_dif, cr_dif;
sincos (r_ik-r_jk, &sr_dif, &cr_dif);
#else
double sr_ij = sin (r_ij), cr_ij = cos (r_ij);
double sp_ij = sin (p_ij), cp_ij = cos (p_ij);
double sh_ij = sin (h_ij), ch_ij = cos (h_ij);
double sp_jk = sin (p_jk), cp_jk = cos (p_jk);
double sr_ik = sin (r_ik), cr_ik = cos (r_ik);
double sp_ik = sin (p_ik), cp_ik = cos (p_ik);
double sh_dif = sin (h_ik-h_ij), ch_dif = cos (h_ik-h_ij);
double sr_dif = sin (r_ik-r_jk), cr_dif = cos (r_ik-r_jk);
#endif
const double tp_ik = tan (p_ik);
const double x_dif = x_ik - x_ij, y_dif = y_ik - y_ij, z_dif = z_ik - z_ij;
if (fabs (cp_ik) < 1e-10)
GSL_ERROR ("divide by zero, cp_ik", GSL_EZERODIV);
double secp_ik = 1/cp_ik;
// M
const double M_data[9] =
{ R_ij(0,2)*y_jk - R_ij(0,1)*z_jk, z_dif*ch_ij, -y_dif,
R_ij(1,2)*y_jk - R_ij(1,1)*z_jk, z_dif*sh_ij, x_dif,
R_ij(2,2)*y_jk - R_ij(2,1)*z_jk, -x_jk*cp_ij - (y_jk*sr_ij + z_jk*cr_ij)*sp_ij, 0 };
gsl_matrix_const_view M = gsl_matrix_const_view_array (M_data, 3, 3);
// K1
const double K1_data[9] =
{ cp_ij*ch_dif*secp_ik, sh_dif*secp_ik, 0,
-cp_ij*sh_dif, ch_dif, 0,
(R_jk(0,1)*sr_ik + R_jk(0,2)*cr_ik)*secp_ik, sh_dif*tp_ik, 1 };
gsl_matrix_const_view K1 = gsl_matrix_const_view_array (K1_data, 3, 3);
// K2
const double K2_data[9] =
{ 1, sr_dif*tp_ik, (R_ij(0,2)*ch_ik + R_ij(1,2)*sh_ik)*secp_ik,
0, cr_dif, -cp_jk*sr_dif,
0, sr_dif*secp_ik, cp_jk*cr_dif*secp_ik };
gsl_matrix_const_view K2 = gsl_matrix_const_view_array (K2_data, 3, 3);
// I_3x3
double I_data[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
gsl_matrix_view I = gsl_matrix_view_array (I_data, 3, 3);
// J_plus
gsl_matrix_view J = gsl_matrix_view_array (J_plus, 6, 12);
gsl_matrix_set_zero (&J.matrix);
gslu_matrix_set_submatrix (&J.matrix, 0, 0, &I.matrix);
gslu_matrix_set_submatrix (&J.matrix, 0, 3, &M.matrix);
gslu_matrix_set_submatrix (&J.matrix, 0, 6, &R_ij.matrix);
gslu_matrix_set_submatrix (&J.matrix, 3, 3, &K1.matrix);
gslu_matrix_set_submatrix (&J.matrix, 3, 9, &K2.matrix);
}
return GSL_SUCCESS;
}
