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;
}