Ejemplo n.º 1
0
 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;
 }
Ejemplo n.º 2
0
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;
}
Ejemplo n.º 3
0
    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;
    }
Ejemplo n.º 4
0
 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));
}
Ejemplo n.º 6
0
    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;
    }
Ejemplo n.º 7
0
    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;
    }
Ejemplo n.º 8
0
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());
}
Ejemplo n.º 9
0
    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;
    }
Ejemplo n.º 10
0
    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);
	}
Ejemplo n.º 12
0
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);
}
Ejemplo n.º 13
0
    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;
    }
Ejemplo n.º 14
0
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;
}