C++ MatrixXd::llt方法代码示例

// [[Rcpp::depends(RcppEigen)]]
Eigen::MatrixXd random_multivariate_normal(const Eigen::MatrixXd mu, const Eigen::MatrixXd Sigma) 
{

  int P = mu.rows(), i = 0;
  Eigen::MatrixXd y(Eigen::MatrixXd(P, 1).setZero());
  Eigen::MatrixXd z(Eigen::MatrixXd(P, 1).setZero());
  
  for( i = 0 ; i < P ; i++ )
    z(i, 0) = Rf_rnorm( 0, 1 );
  
  y = mu + Sigma.llt().matrixL() * z;

  return y;
}

  Eigen::VectorXd multivariateGaussian(const Eigen::MatrixXd &covariance, Eigen::VectorXd *mean){
    int size = covariance.rows();
    Eigen::VectorXd sample(size);
    for (int i=0; i<size; i++)
    {
      sample(i) = gaussian();
    }

    Eigen::MatrixXd mL = covariance.llt().matrixL(); // Cholesky decomposition
    if (mean != NULL)
      return mL*sample + *mean;
    else
      return mL*sample;
  }

void myDifferentialEquation( double *x, double *f, void *user_data )
{
    for (i = 0 ; i<6 ; i++) {
        v[i] = x[i+6];
        a[i] = 0.;
        ei[i] = 0.;
    }

//    for (i=0 ; i<16 ; i++) {
//        std::cout << " x   : " << x[i] << std::endl;
//    }

    for (i=0 ; i<3 ; i++) {
        q[i] = x[i];
    }


    //RPY to quaternion
    r = x[3]; p = x[4]; y = x[5];
    Mrot(0,0) = cos(y)*cos(p);
    Mrot(0,1) = cos(y)*sin(p)*sin(r) - sin(y)*cos(r);
    Mrot(0,2) = cos(y)*sin(p)*cos(r) + sin(y)*sin(r);
    Mrot(1,0) = sin(y)*cos(p);
    Mrot(1,1) = sin(y)*sin(p)*sin(r) + cos(y)*cos(r);
    Mrot(1,2) = sin(y)*sin(p)*cos(r) - cos(y)*sin(r);
    Mrot(2,0) = -sin(p);
    Mrot(2,1) = cos(p)*sin(r);
    Mrot(2,2) = cos(p)*cos(r);

    Eigen::Quaternion<double> quat(Mrot);
    q[6] = quat.w(); q[3] = quat.x(); q[4] = quat.y(); q[5] = quat.z();

//    std::cout << " q   : \n" << q << std::endl;

    MomentRPY(0) = x[14];//100;
    MomentRPY(1) = x[15];//100;
    MomentRPY(2) = x[16];//100;

   // MomentRPY = Mrot*MomentRPY;

    u[0] = 0;//- (x[13])*sin(p);
    u[1] = 0;//(x[13])*sin(r)*cos(p);
    u[2] = x[13];//(x[13])*cos(r)*cos(p);
    u[3] = MomentRPY(0);
    u[4] = MomentRPY(1);
    u[5] = MomentRPY(2);


    b0 = rnea(robot, data, q, a, a);

//    std::cout << "b0 : \n" << b0 << std::endl;

    for (i=0 ; i<6 ; i++) {
        ei(i)=1;
        Mi = rnea(robot, data, q, a, ei);
        for (j=0 ; j<6 ; j++ ) {
            M(i,j) = Mi(j)-b0(j);
        }
        ei(i)=0;
    }

//    std::cout << " M rnea :\n" <<  M << std::endl << std::endl;
//    std::cout << "crba :\n" << crba(robot, data, q) << std::endl << std::endl;
//    self adjoint upper ???

    b = rnea(robot, data, q, v, a);

    a = M.llt().solve(u-b);
//    std::cout << "F tot moteurs : \n" << x[12]+x[13]+x[14]+x[15] << std::endl << std::endl;
//    std::cout << "Forces : \n" << u << std::endl << std::endl;
//    std::cout << "Acceleration : \n" << a << std::endl << std::endl;

    // --------------------   COMPUTE OUTPUT -----------------------  //
    // vitesses lineaires Freeflyer
    for (i=0 ; i<3 ; i++ ) {
        f[i] = v[i];
    }

    // vitesses angulaires Freeflyer (transformation RPY)
    f[3] = v[3] + sin(p)*v[5];
    f[4] = cos(r)*v[4] - sin(r)*cos(p)*v[5];
    f[5] = sin(r)*v[4] + cos(r)*cos(p)*v[5];

    // accelerations lineaires
    for (i=0 ; i<3 ; i++) {
        al[i] = a[i];
        aa[i] = a[i+3];
    }

    al2 = Mrot*al;
    for (i=0 ; i<3 ;i++) {
        f[i+6] = al2[i];
    }

    //accelerations angulaires
    for (i=0 ; i<3 ;i++) {
        f[i+9] = aa[i];
    }


//.........这里部分代码省略.........

 void GaussianCovProposal::update(uint id, const Eigen::MatrixXd &cov)
 {
   sigL_[id] = cov.llt().matrixL();
 }

 Cholesky_LLT (const Eigen::MatrixXd &A){
     U= A.llt().matrixU();
 }