typename boost::math::tools::promote_args<T_y,T_loc,T_scale,T_shape>::type lkj_cov_log(const Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>& y, const Eigen::Matrix<T_loc,Eigen::Dynamic,1>& mu, const Eigen::Matrix<T_scale,Eigen::Dynamic,1>& sigma, const T_shape& eta, const Policy&) { static const char* function = "stan::prob::lkj_cov_log(%1%)"; using stan::math::check_size_match; using stan::math::check_finite; using stan::math::check_positive; using boost::math::tools::promote_args; typename promote_args<T_y,T_loc,T_scale,T_shape>::type lp(0.0); if (!check_size_match(function, mu.rows(), "Rows of location parameter", sigma.rows(), "columns of scale parameter", &lp, Policy())) return lp; if (!check_size_match(function, y.rows(), "Rows of random variable", y.cols(), "columns of random variable", &lp, Policy())) return lp; if (!check_size_match(function, y.rows(), "Rows of random variable", mu.rows(), "rows of location parameter", &lp, Policy())) return lp; if (!check_positive(function, eta, "Shape parameter", &lp, Policy())) return lp; if (!check_finite(function, mu, "Location parameter", &lp, Policy())) return lp; if (!check_finite(function, sigma, "Scale parameter", &lp, Policy())) return lp; // FIXME: build vectorized versions for (int m = 0; m < y.rows(); ++m) for (int n = 0; n < y.cols(); ++n) if (!check_finite(function, y(m,n), "Covariance matrix", &lp, Policy())) return lp; const unsigned int K = y.rows(); const Eigen::Array<T_y,Eigen::Dynamic,1> sds = y.diagonal().array().sqrt(); for (unsigned int k = 0; k < K; k++) { lp += lognormal_log<propto>(sds(k), mu(k), sigma(k), Policy()); } if (stan::is_constant<typename stan::scalar_type<T_shape> >::value && eta == 1.0) { // no need to rescale y into a correlation matrix lp += lkj_corr_log<propto,T_y,T_shape,Policy>(y, eta, Policy()); return lp; } Eigen::DiagonalMatrix<T_y,Eigen::Dynamic> D(K); D.diagonal() = sds.inverse(); lp += lkj_corr_log<propto,T_y,T_shape,Policy>(D * y * D, eta, Policy()); return lp; }
Scalar condition(Eigen::Matrix<Scalar, Rows, Cols>& matrix, Scalar maxWanted) { // TODO Separate case for self-adjoint matrices, which would be the case for // TODO covariance matrices. // TODO matrix.selfadjointView<Lower>().eigenvalues(); auto values = matrix.eigenvalues().cwiseAbs(); Scalar max = values.maxCoeff(); Scalar min = values.minCoeff(); // TODO Should I be using the signed min and max eigenvalues? // TODO I'm not sure how to deal generally with complex values if so. Scalar condition = max / min; if (condition > maxWanted) { // TODO If maxWanted is (near?) 1, then just set to identity? Scalar bonus = (max - min * maxWanted) / (maxWanted - 1); matrix.diagonal() = matrix.diagonal().array() + bonus; } return condition; }
typename boost::math::tools::promote_args<T_y, T_covar, T_w>::type multi_gp_cholesky_log(const Eigen::Matrix <T_y, Eigen::Dynamic, Eigen::Dynamic>& y, const Eigen::Matrix <T_covar, Eigen::Dynamic, Eigen::Dynamic>& L, const Eigen::Matrix<T_w, Eigen::Dynamic, 1>& w) { static const char* function("multi_gp_cholesky_log"); typedef typename boost::math::tools::promote_args<T_y, T_covar, T_w>::type T_lp; T_lp lp(0.0); check_size_match(function, "Size of random variable (rows y)", y.rows(), "Size of kernel scales (w)", w.size()); check_size_match(function, "Size of random variable", y.cols(), "rows of covariance parameter", L.rows()); check_finite(function, "Kernel scales", w); check_positive(function, "Kernel scales", w); check_finite(function, "Random variable", y); if (y.rows() == 0) return lp; if (include_summand<propto>::value) { lp += NEG_LOG_SQRT_TWO_PI * y.rows() * y.cols(); } if (include_summand<propto, T_covar>::value) { lp -= L.diagonal().array().log().sum() * y.rows(); } if (include_summand<propto, T_w>::value) { lp += 0.5 * y.cols() * sum(log(w)); } if (include_summand<propto, T_y, T_w, T_covar>::value) { T_lp sum_lp_vec(0.0); for (int i = 0; i < y.rows(); i++) { Eigen::Matrix<T_y, Eigen::Dynamic, 1> y_row(y.row(i)); Eigen::Matrix<typename boost::math::tools::promote_args <T_y, T_covar>::type, Eigen::Dynamic, 1> half(mdivide_left_tri_low(L, y_row)); sum_lp_vec += w(i) * dot_self(half); } lp -= 0.5*sum_lp_vec; } return lp; }
void GaussianDiagCov<ScalarType>::setCovarianceMatrix(const Eigen::Matrix<ScalarType, Eigen::Dynamic, Eigen::Dynamic>& fullCov) { assert(fullCov.rows() == mean.size()); this->diagCov = fullCov.diagonal(); ldlt.compute(diagCov.asDiagonal()); llt.compute(diagCov.asDiagonal()); if (pseudoInverse) { delete pseudoInverse; pseudoInverse = NULL; } calculatePrefactor(); }
void check_is_diagonal() { Eigen::Matrix<fl::Real, Size, Size> m; m.resize(Dim, Dim); m.setRandom(); EXPECT_FALSE(fl::is_diagonal(m)); m = m.diagonal().asDiagonal(); EXPECT_TRUE(fl::is_diagonal(m)); m.setIdentity(); m *= 3.; EXPECT_TRUE(fl::is_diagonal(m)); m(0, Dim - 1) = 2; EXPECT_FALSE(fl::is_diagonal(m)); }
typename boost::math::tools::promote_args<T_covar, T_shape>::type lkj_corr_cholesky_log(const Eigen::Matrix <T_covar, Eigen::Dynamic, Eigen::Dynamic>& L, const T_shape& eta) { static const char* function("stan::math::lkj_corr_cholesky_log"); using boost::math::tools::promote_args; using stan::math::check_positive; using stan::math::check_lower_triangular; using stan::math::sum; typename promote_args<T_covar, T_shape>::type lp(0.0); check_positive(function, "Shape parameter", eta); check_lower_triangular(function, "Random variable", L); const unsigned int K = L.rows(); if (K == 0) return 0.0; if (include_summand<propto, T_shape>::value) lp += do_lkj_constant(eta, K); if (include_summand<propto, T_covar, T_shape>::value) { const int Km1 = K - 1; Eigen::Matrix<T_covar, Eigen::Dynamic, 1> log_diagonals = L.diagonal().tail(Km1).array().log(); Eigen::Matrix<T_covar, Eigen::Dynamic, 1> values(Km1); for (int k = 0; k < Km1; k++) values(k) = (Km1 - k - 1) * log_diagonals(k); if ( (eta == 1.0) && stan::is_constant<typename stan::scalar_type<T_shape> >::value) { lp += sum(values); return(lp); } values += (2.0 * eta - 2.0) * log_diagonals; lp += sum(values); } return lp; }
typename boost::math::tools::promote_args<T_y,T_loc,T_scale,T_shape>::type lkj_cov_log(const Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>& y, const T_loc& mu, const T_scale& sigma, const T_shape& eta, const Policy&) { static const char* function = "stan::prob::lkj_cov_log(%1%)"; using stan::math::check_finite; using stan::math::check_positive; using boost::math::tools::promote_args; typename promote_args<T_y,T_loc,T_scale,T_shape>::type lp(0.0); if (!check_positive(function, eta, "Shape parameter", &lp, Policy())) return lp; if (!check_finite(function, mu, "Location parameter", &lp, Policy())) return lp; if (!check_finite(function, sigma, "Scale parameter", &lp, Policy())) return lp; const unsigned int K = y.rows(); const Eigen::Array<T_y,Eigen::Dynamic,1> sds = y.diagonal().array().sqrt(); for (unsigned int k = 0; k < K; k++) { lp += lognormal_log<propto>(sds(k), mu, sigma, Policy()); } if (stan::is_constant<typename stan::scalar_type<T_shape> >::value && eta == 1.0) { // no need to rescale y into a correlation matrix lp += lkj_corr_log<propto>(y,eta,Policy()); return lp; } Eigen::DiagonalMatrix<T_y,Eigen::Dynamic> D(K); D.diagonal() = sds.inverse(); lp += lkj_corr_log<propto,T_y,T_shape,Policy>(D * y * D, eta, Policy()); return lp; }
template<int N> void calculateGaussPdf(Eigen::Matrix<double,Eigen::Dynamic,N> X, Eigen::Matrix<double,1,N> mu, Eigen::Matrix<double,N,N> C, double* result){ Eigen::Matrix<double,N,N> L = C.llt().matrixL().transpose(); // cholesky decomposition Eigen::Matrix<double,N,N> Linv = L.inverse(); double det = L.diagonal().prod(); //determinant of L is equal to square rooot of determinant of C double lognormconst = -log(2 * M_PI)*X.cols()/2 - log(fabs(det)); Eigen::Matrix<double,1,N> x = mu; Eigen::Matrix<double,1,N> tmp = x; for (int i=0; i<X.rows(); i++){ x.noalias() = X.row(i) - mu; tmp.noalias() = x*Linv; double exponent = -0.5 * (tmp.cwiseProduct(tmp)).sum(); result[i] = lognormconst+exponent; } /* Eigen::Matrix<double,Eigen::Dynamic,N> X0 = (X.rowwise() - mu)*Linv; Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,1> > resultMap(result, X.rows()); resultMap = (X0.rowwise().squaredNorm()).array() * (-0.5) + lognormconst; */ fmath::expd_v(result, X.rows()); }
typename boost::math::tools::promote_args<T_y,T_loc,T_covar>::type multi_normal_cholesky_log(const Eigen::Matrix<T_y,Eigen::Dynamic,1>& y, const Eigen::Matrix<T_loc,Eigen::Dynamic,1>& mu, const Eigen::Matrix<T_covar,Eigen::Dynamic,Eigen::Dynamic>& L) { static const char* function = "stan::prob::multi_normal_cholesky_log(%1%)"; using stan::math::mdivide_left_tri_low; using stan::math::dot_self; using stan::math::multiply; using stan::math::subtract; using stan::math::sum; using stan::math::check_size_match; using stan::math::check_finite; using stan::math::check_not_nan; using stan::math::check_cov_matrix; using boost::math::tools::promote_args; typename promote_args<T_y,T_loc,T_covar>::type lp(0.0); if (!check_size_match(function, y.size(), "Size of random variable", mu.size(), "size of location parameter", &lp)) return lp; if (!check_size_match(function, y.size(), "Size of random variable", L.rows(), "rows of covariance parameter", &lp)) return lp; if (!check_size_match(function, y.size(), "Size of random variable", L.cols(), "columns of covariance parameter", &lp)) return lp; if (!check_finite(function, mu, "Location parameter", &lp)) return lp; if (!check_not_nan(function, y, "Random variable", &lp)) return lp; if (y.rows() == 0) return lp; if (include_summand<propto>::value) lp += NEG_LOG_SQRT_TWO_PI * y.rows(); if (include_summand<propto,T_covar>::value) { Eigen::Matrix<T_covar,Eigen::Dynamic,1> L_log_diag = L.diagonal().array().log().matrix(); lp -= sum(L_log_diag); } if (include_summand<propto,T_y,T_loc,T_covar>::value) { Eigen::Matrix<typename boost::math::tools::promote_args<T_y,T_loc>::type, Eigen::Dynamic, 1> y_minus_mu(y.size()); for (int i = 0; i < y.size(); i++) y_minus_mu(i) = y(i)-mu(i); Eigen::Matrix<typename boost::math::tools::promote_args<T_covar,T_loc,T_y>::type, Eigen::Dynamic, 1> half(mdivide_left_tri_low(L,y_minus_mu)); // FIXME: this code does not compile. revert after fixing subtract() // Eigen::Matrix<typename // boost::math::tools::promote_args<T_covar,T_loc,T_y>::type, // Eigen::Dynamic, 1> // half(mdivide_left_tri_low(L,subtract(y,mu))); lp -= 0.5 * dot_self(half); } return lp; }
typename boost::math::tools::promote_args<T_y,T_loc,T_covar>::type multi_normal_cholesky_log(const Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>& y, const Eigen::Matrix<T_loc,Eigen::Dynamic,1>& mu, const Eigen::Matrix<T_covar,Eigen::Dynamic,Eigen::Dynamic>& L) { static const char* function = "stan::prob::multi_normal_cholesky_log(%1%)"; using stan::math::mdivide_left_tri_low; using stan::math::columns_dot_self; using stan::math::multiply; using stan::math::subtract; using stan::math::sum; using stan::math::log; using stan::math::check_size_match; using stan::math::check_finite; using stan::math::check_not_nan; using stan::math::check_cov_matrix; using boost::math::tools::promote_args; typename promote_args<T_y,T_loc,T_covar>::type lp(0.0); if (!check_size_match(function, y.cols(), "Columns of random variable", mu.rows(), "rows of location parameter", &lp)) return lp; if (!check_size_match(function, y.cols(), "Columns of random variable", L.rows(), "rows of covariance parameter", &lp)) return lp; if (!check_size_match(function, y.cols(), "Columns of random variable", L.cols(), "columns of covariance parameter", &lp)) return lp; if (!check_finite(function, mu, "Location parameter", &lp)) return lp; if (!check_not_nan(function, y, "Random variable", &lp)) return lp; if (y.cols() == 0) return lp; if (include_summand<propto>::value) lp += NEG_LOG_SQRT_TWO_PI * y.cols() * y.rows(); if (include_summand<propto,T_covar>::value) { Eigen::Matrix<T_covar,Eigen::Dynamic,1> L_log_diag = L.diagonal().array().log().matrix(); lp -= sum(L_log_diag) * y.rows(); } if (include_summand<propto,T_y,T_loc,T_covar>::value) { Eigen::Matrix<T_loc, Eigen::Dynamic, Eigen::Dynamic> MU(y.rows(),y.cols()); for (typename Eigen::Matrix<T_loc, Eigen::Dynamic, Eigen::Dynamic>::size_type i = 0; i < y.rows(); i++) MU.row(i) = mu; Eigen::Matrix<typename boost::math::tools::promote_args<T_loc,T_y>::type, Eigen::Dynamic,Eigen::Dynamic> y_minus_MU(y.rows(), y.cols()); for (int i = 0; i < y.size(); i++) y_minus_MU(i) = y(i)-MU(i); Eigen::Matrix<typename boost::math::tools::promote_args<T_loc,T_y>::type, Eigen::Dynamic,Eigen::Dynamic> z(y_minus_MU.transpose()); // was = // FIXME: revert this code when subtract() is fixed. // Eigen::Matrix<typename // boost::math::tools::promote_args<T_loc,T_y>::type, // Eigen::Dynamic,Eigen::Dynamic> // z(subtract(y,MU).transpose()); // was = Eigen::Matrix<typename boost::math::tools::promote_args<T_covar,T_loc,T_y>::type, Eigen::Dynamic,Eigen::Dynamic> half(mdivide_left_tri_low(L,z)); lp -= 0.5 * sum(columns_dot_self(half)); } return lp; }
void SetUp() override { using SurgSim::Math::getSubVector; using SurgSim::Math::getSubMatrix; using SurgSim::Math::addSubMatrix; m_nodeIds[0] = 3; m_nodeIds[1] = 1; m_nodeIds[2] = 14; m_nodeIds[3] = 9; std::vector<size_t> m_nodeIdsVectorForm; // Useful for assembly helper function m_nodeIdsVectorForm.push_back(m_nodeIds[0]); m_nodeIdsVectorForm.push_back(m_nodeIds[1]); m_nodeIdsVectorForm.push_back(m_nodeIds[2]); m_nodeIdsVectorForm.push_back(m_nodeIds[3]); m_restState.setNumDof(3, 15); Vector& x0 = m_restState.getPositions(); // Tet is aligned with the axis (X,Y,Z), centered on (0.1, 1.2, 2.3), embedded in a cube of size 1 getSubVector(m_expectedX0, 0, 3) = getSubVector(x0, m_nodeIds[0], 3) = Vector3d(0.1, 1.2, 2.3); getSubVector(m_expectedX0, 1, 3) = getSubVector(x0, m_nodeIds[1], 3) = Vector3d(1.1, 1.2, 2.3); getSubVector(m_expectedX0, 2, 3) = getSubVector(x0, m_nodeIds[2], 3) = Vector3d(0.1, 2.2, 2.3); getSubVector(m_expectedX0, 3, 3) = getSubVector(x0, m_nodeIds[3], 3) = Vector3d(0.1, 1.2, 3.3); // The tet is part of a cube of size 1x1x1 (it occupies 1/6 of the cube's volume) m_expectedVolume = 1.0 / 6.0; m_rho = 1000.0; m_E = 1e6; m_nu = 0.45; m_expectedMassMatrix.setZero(3*15, 3*15); m_expectedDampingMatrix.setZero(3*15, 3*15); m_expectedStiffnessMatrix.setZero(3*15, 3*15); m_expectedStiffnessMatrix2.setZero(3*15, 3*15); m_vectorOnes.setOnes(3*15); Eigen::Matrix<double, 12, 12> M = Eigen::Matrix<double, 12, 12>::Zero(); { M.diagonal().setConstant(2.0); M.block(0, 3, 9, 9).diagonal().setConstant(1.0); M.block(0, 6, 6, 6).diagonal().setConstant(1.0); M.block(0, 9, 3, 3).diagonal().setConstant(1.0); M.block(3, 0, 9, 9).diagonal().setConstant(1.0); M.block(6, 0, 6, 6).diagonal().setConstant(1.0); M.block(9, 0, 3, 3).diagonal().setConstant(1.0); } M *= m_rho * m_expectedVolume / 20.0; addSubMatrix(M, m_nodeIdsVectorForm, 3 , &m_expectedMassMatrix); Eigen::Matrix<double, 12, 12> K = Eigen::Matrix<double, 12, 12>::Zero(); { // Calculation done by hand from // http://www.colorado.edu/engineering/CAS/courses.d/AFEM.d/AFEM.Ch09.d/AFEM.Ch09.pdf // ai = {} // bi = {-1 1 0 0} // ci = {-1 0 1 0} // di = {-1 0 0 1} Eigen::Matrix<double, 6, 12> B = Eigen::Matrix<double, 6, 12>::Zero(); Eigen::Matrix<double, 6, 6> E = Eigen::Matrix<double, 6, 6>::Zero(); B(0, 0) = -1; B(0, 3) = 1; B(1, 1) = -1; B(1, 7) = 1; B(2, 2) = -1; B(2, 11) = 1; B(3, 0) = -1; B(3, 1) = -1; B(3, 4) = 1; B(3, 6) = 1; B(4, 1) = -1; B(4, 2) = -1; B(4, 8) = 1; B(4, 10) = 1; B(5, 0) = -1; B(5, 2) = -1; B(5, 5) = 1; B(5, 9) = 1; B *= 1.0 / (6.0 * m_expectedVolume); E.block(0, 0, 3, 3).setConstant(m_nu); E.block(0, 0, 3, 3).diagonal().setConstant(1.0 - m_nu); E.block(3, 3, 3, 3).diagonal().setConstant(0.5 - m_nu); E *= m_E / (( 1.0 + m_nu) * (1.0 - 2.0 * m_nu)); K = m_expectedVolume * B.transpose() * E * B; } addSubMatrix(K, m_nodeIdsVectorForm, 3 , &m_expectedStiffnessMatrix); // Expecte stiffness matrix given for our case in: // http://www.colorado.edu/engineering/CAS/courses.d/AFEM.d/AFEM.Ch09.d/AFEM.Ch09.pdf double E = m_E / (12.0*(1.0 - 2.0*m_nu)*(1.0 + m_nu)); double n0 = 1.0 - 2.0 * m_nu; double n1 = 1.0 - m_nu; K.setZero(); // Fill up the upper triangle part first (without diagonal elements) K(0, 1) = K(0, 2) = K(1, 2) = 1.0; K(0, 3) = -2.0 * n1; K(0, 4) = -n0; K(0, 5) = -n0; K(1, 3) = -2.0 * m_nu; K(1, 4) = -n0; K(2, 3) = -2.0 * m_nu; K(2, 5) = -n0; K(0, 6) = - n0; K(0, 7) = -2.0 * m_nu; K(1, 6) = - n0; K(1, 7) = -2.0 * n1; K(1, 8) = - n0; K(2, 7) = - 2.0 * m_nu; K(2, 8) = -n0; K(0, 9) = - n0; K(0, 11) = -2.0 * m_nu; K(1, 10) = - n0; K(1, 11) = -2.0 * m_nu; K(2, 9) = - n0; K(2, 10) = - n0; K(2, 11) = -2.0 * n1; K(3, 7) = K(3, 11) = 2.0 * m_nu; K(4, 6) = n0; K(5, 9) = n0; K(7, 11) = 2.0 * m_nu; K(8, 10) = n0; K += K.transpose().eval(); // symmetric part (do not forget the .eval() !) K.block(0,0,3,3).diagonal().setConstant(4.0 - 6.0 * m_nu); // diagonal elements K.block(3,3,9,9).diagonal().setConstant(n0); // diagonal elements K(3, 3) = K(7, 7) = K(11, 11) = 2.0 * n1; // diagonal elements K *= E; addSubMatrix(K, m_nodeIdsVectorForm, 3 , &m_expectedStiffnessMatrix2); }
void setDiagonal(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov, MTK::SubManifold<T, idx> Base::*, const typename Base::scalar &val) { cov.diagonal().template segment<T::DOF>(idx).setConstant(val); }
typename boost::math::tools::promote_args<typename scalar_type<T_y>::type, typename scalar_type<T_loc>::type, T_covar>::type multi_normal_cholesky_log(const T_y& y, const T_loc& mu, const Eigen::Matrix<T_covar,Eigen::Dynamic,Eigen::Dynamic>& L) { static const char* function = "stan::prob::multi_normal_cholesky_log(%1%)"; using stan::math::mdivide_left_tri_low; using stan::math::dot_self; using stan::math::multiply; using stan::math::subtract; using stan::math::sum; using stan::math::check_size_match; using stan::math::check_finite; using stan::math::check_not_nan; using stan::math::check_cov_matrix; using boost::math::tools::promote_args; typedef typename boost::math::tools::promote_args<typename scalar_type<T_y>::type, typename scalar_type<T_loc>::type, T_covar>::type lp_type; lp_type lp(0.0); VectorViewMvt<const T_y> y_vec(y); VectorViewMvt<const T_loc> mu_vec(mu); //size of std::vector of Eigen vectors size_t size_vec = max_size_mvt(y, mu); //Check if every vector of the array has the same size int size_y = y_vec[0].size(); int size_mu = mu_vec[0].size(); if (size_vec > 1) { int size_y_old = size_y; int size_y_new; for (size_t i = 1, size_ = length_mvt(y); i < size_; i++) { int size_y_new = y_vec[i].size(); check_size_match(function, size_y_new, "Size of one of the vectors of the random variable", size_y_old, "Size of another vector of the random variable", &lp); size_y_old = size_y_new; } int size_mu_old = size_mu; int size_mu_new; for (size_t i = 1, size_ = length_mvt(mu); i < size_; i++) { int size_mu_new = mu_vec[i].size(); check_size_match(function, size_mu_new, "Size of one of the vectors of the location variable", size_mu_old, "Size of another vector of the location variable", &lp); size_mu_old = size_mu_new; } (void) size_y_old; (void) size_y_new; (void) size_mu_old; (void) size_mu_new; } check_size_match(function, size_y, "Size of random variable", size_mu, "size of location parameter", &lp); check_size_match(function, size_y, "Size of random variable", L.rows(), "rows of covariance parameter", &lp); check_size_match(function, size_y, "Size of random variable", L.cols(), "columns of covariance parameter", &lp); for (size_t i = 0; i < size_vec; i++) { check_finite(function, mu_vec[i], "Location parameter", &lp); check_not_nan(function, y_vec[i], "Random variable", &lp); } if (size_y == 0) return lp; if (include_summand<propto>::value) lp += NEG_LOG_SQRT_TWO_PI * size_y * size_vec; if (include_summand<propto,T_covar>::value) { Eigen::Matrix<T_covar,Eigen::Dynamic,1> L_log_diag = L.diagonal().array().log().matrix(); lp -= sum(L_log_diag) * size_vec; } if (include_summand<propto,T_y,T_loc,T_covar>::value) { lp_type sum_lp_vec(0.0); for (size_t i = 0; i < size_vec; i++) { Eigen::Matrix<typename boost::math::tools::promote_args<typename scalar_type<T_y>::type,typename scalar_type<T_loc>::type>::type, Eigen::Dynamic, 1> y_minus_mu(size_y); for (int j = 0; j < size_y; j++) y_minus_mu(j) = y_vec[i](j)-mu_vec[i](j); Eigen::Matrix<typename boost::math::tools::promote_args<T_covar,typename scalar_type<T_loc>::type,typename scalar_type<T_y>::type>::type, Eigen::Dynamic, 1> half(mdivide_left_tri_low(L,y_minus_mu)); // FIXME: this code does not compile. revert after fixing subtract() // Eigen::Matrix<typename // boost::math::tools::promote_args<T_covar,typename scalar_type<T_loc>::type,typename scalar_type<T_y>::type>::type>::type, // Eigen::Dynamic, 1> // half(mdivide_left_tri_low(L,subtract(y,mu))); sum_lp_vec += dot_self(half); } lp -= 0.5*sum_lp_vec; } return lp; }
void initPrecs() { // set up some precision matrices n2prec << 260312.1329351594, 17615.81091248868, -11716.3738046826, -260221.3577238563, 3028947.570775249, 284048.6838048229, 17615.81091248783, 369156.349498884, -8122.584888439054, -4130281.103526653, 265383.1196958761, 523737.7444220608, -11716.3738046842, -8122.58488844048, 673.3469031685361, 93635.22686723019, -137533.0434459766, -22834.5012408561, -260221.3577238639, -4130281.103526646, 93635.22686720481, 52493931.52684124, -4078689.933502881, -9475682.025736494, 3028947.570775286, 265383.119695912, -137533.0434459558, -4078689.933502988, 39416288.19312727, 3894322.443643413, 284048.6838048277, 523737.7444220638, -22834.50124085596, -9475682.025736755, 3894322.443643621, 50690679.29036696; n2vprec << 624875.2423563644,-8.596260869004408e-11,10576.14746839753, -65704.86829639168,10646337.23355757,646569.8439109828, -1.045228848835824e-10,-2.955857780762017e-10,9.820269042393193e-10, 6.912159733474255e-09,-3.751665644813329e-09,-3.511559043545276e-08, 10576.14746839765,7.860307960072532e-10,224224.9112157905, -233966.3120641535,77714.35666432518,65704.86829639639, -65704.86829639156,8.021743269637227e-09,-233966.312064158, 7256072.962556601,-1242408.349188809,197719.0360238712, 10646337.23355758,-6.682398634438869e-09,77714.35666432098, -1242408.349188721,214456943.0273151,11161674.13523376, 646569.8439109783,-3.356490196892992e-08,65704.86829639817, 197719.0360238167,11161674.13523367, 19698666.98402661; n2aprec << 229528.3846633453, 886.7480854882738, -10039.08940223746, 62445.98594207098, 2715273.460194867, 106542.6230004076, 886.7480854885912, 319242.7032811134, -6397.916315207351, -3608430.146373766, -49269.13482550202, 582748.417531022, -10039.08940223649, -6397.916315208951, 565.7603057193751, 69152.18264815415, -117569.9760459389, -16259.89068069827, 62445.98594206382, -3608430.146373736, 69152.1826481162, 47244836.25653829, 1303537.745687656, -9808843.224988466, 2715273.46019485, -49269.13482549335, -117569.9760459207, 1303537.745687651, 35830355.245529, 709155.852370202, 106542.623000413, 582748.4175310251, -16259.89068069991, -9808843.224988459, 709155.8523703497, 48304469.04982638; n2bprec << 148324.039595044, 222.4623044457281, -19531.70697504873, -10192.06466578297, 1631677.485087357, 60190.82294241861, 222.4623044456828, 200041.4398061978, -4054.812572933995, -2258670.079144401, 29578.86052762273, 799843.0721628161, -19531.70697504886, -4054.812572933865, 2652.99484259674, 46794.05582115334, -215409.6450292048, -24019.87801347017, -10192.06466578462, -2258670.079144401, 46794.05582115659, 28945336.2353294, -434508.6610355716, -12934377.41525949, 1631677.485087361, 29578.86052762576, -215409.6450292043, -434508.6610355551, 20018126.98420228, 1153711.950184977, 60190.82294241752, 799843.0721628153, -24019.8780134693, -12934377.41525948, 1153711.950184968, 22955884.81085673; #if 0 // this has zeros for rot-trans interaction n2prec << 260312.1329351594, 17615.81091248868, -11716.3738046826, 0.0, 0.0, 0.0, 17615.81091248783, 369156.349498884, -8122.584888439054, 0.0, 0.0, 0.0, -11716.3738046842, -8122.58488844048, 673.3469031685361, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 52493931.52684124, -4078689.933502881, -9475682.025736494, 0.0, 0.0, 0.0, -4078689.933502988, 39416288.19312727, 3894322.443643413, 0.0, 0.0, 0.0, -9475682.025736755, 3894322.443643621, 50690679.29036696; n2vprec << 624875.2423563644,-8.596260869004408e-11,10576.14746839753,0,0,0, -1.045228848835824e-10,-2.955857780762017e-10,9.820269042393193e-10,0,0,0, 10576.14746839765,7.860307960072532e-10,224224.9112157905,0,0,0, 0,0,0, 7256072.962556601,-1242408.349188809,197719.0360238712, 0,0,0, -1242408.349188721,214456943.0273151,11161674.13523376, 0,0,0, 197719.0360238167,11161674.13523367, 19698666.98402661; n2aprec << 229528.3846633453, 886.7480854882738, -10039.08940223746, 0,0,0, 886.7480854885912, 319242.7032811134, -6397.916315207351, 0,0,0, -10039.08940223649, -6397.916315208951, 565.7603057193751, 0,0,0, 0,0,0, 47244836.25653829, 1303537.745687656, -9808843.224988466, 0,0,0, 1303537.745687651, 35830355.245529, 709155.852370202, 0,0,0, -9808843.224988459, 709155.8523703497, 48304469.04982638; n2bprec << 148324.039595044, 222.4623044457281, -19531.70697504873, 0,0,0, 222.4623044456828, 200041.4398061978, -4054.812572933995, 0,0,0, -19531.70697504886, -4054.812572933865, 2652.99484259674, 0,0,0, 0,0,0, 28945336.2353294, -434508.6610355716, -12934377.41525949, 0,0,0, -434508.6610355551, 20018126.98420228, 1153711.950184977, 0,0,0, -12934377.41525948, 1153711.950184968, 22955884.81085673; #endif #if 1 n2prec *= 1.0/100000.0; n2vprec *= 1.0/100000.0; n2aprec *= 1.0/100000.0; n2bprec *= 1.0/100000.0; #endif diagprec.setIdentity(); diagprec = diagprec*(1000); diagprec.diagonal().head(3) *= .0001; }