C++ MatrixXf::inverse方法代码示例

MatrixXf Arm::pseudo_inverse() {
    MatrixXf Jacovian = compute_Jacobian();
    MatrixXf jjtInv = (Jacovian * Jacovian.transpose());
    jjtInv = jjtInv.inverse();
    
    return (Jacovian.transpose() * jjtInv);
}

VectorXf param_sensitivity_widget::computeSensitivity(
    MatrixXf &parameterMatrix, VectorXf &responseVector)
{
    MatrixXf Ctemp = parameterMatrix.transpose()*parameterMatrix;
    MatrixXf C;
    C = Ctemp.inverse();

    VectorXf b = C*parameterMatrix.transpose()*responseVector;

    VectorXf Y_hat = parameterMatrix*b;

    int p = b.rows();

    VectorXf sigma2Vec = responseVector-Y_hat;

    float sigma2 = sigma2Vec.squaredNorm();
    sigma2= sigma2/(parameterMatrix.rows() - p);

    Ctemp = C*sigma2;

    MatrixXf denominator = Ctemp.diagonal();

    // Do element-wise division
    VectorXf t = b;
    for (int i = 0; i < b.rows(); i++)
    {
        t(i) = abs(b(i)/sqrt(denominator(i)));
    }

    return t;
}

void KF_joseph_update(VectorXf &x, MatrixXf &P,float v,float R, MatrixXf H)
{
    VectorXf PHt = P*H.transpose();
    MatrixXf S = H*PHt;
    S(0,0) += R;
    MatrixXf Si = S.inverse();
    Si = make_symmetric(Si);
    MatrixXf PSD_check = Si.llt().matrixL(); //chol of scalar is sqrt
    PSD_check.transpose();
    PSD_check.conjugate();

    VectorXf W = PHt*Si;
    x = x+W*v;
    
    //Joseph-form covariance update
    MatrixXf eye(P.rows(), P.cols());
    eye.setIdentity();
    MatrixXf C = eye - W*H;
    P = C*P*C.transpose() + W*R*W.transpose();  

    float eps = 2.2204*pow(10.0,-16); //numerical safety 
    P = P+eye*eps;

    PSD_check = P.llt().matrixL();
    PSD_check.transpose();
    PSD_check.conjugate(); //for upper tri
}

MatrixXf LinkedStructure::pseudoInverse()
{
  // Simple math that represents the mathematics
  // explained on the website to computing the
  // pseudo inverse. this is exactly the math
  // discussed in the tutorial!!!
    MatrixXf j = jacobian();
    MatrixXf jjtInv = (j * j.transpose());
    jjtInv = jjtInv.inverse();

    return (j.transpose() * jjtInv);
}

//compute proposal distribution, then sample from it, and compute new particle weight
void sample_proposal(Particle &particle, vector<VectorXf> z, vector<int> idf, MatrixXf R)
{
    assert(isfinite(particle.w()));
    VectorXf xv = VectorXf(particle.xv()); //robot position
    MatrixXf Pv = MatrixXf(particle.Pv()); //controls (motion command)

    VectorXf xv0 = VectorXf(xv);
    MatrixXf Pv0 = MatrixXf(Pv);	

    vector<MatrixXf> Hv;
    vector<MatrixXf> Hf;
    vector<MatrixXf> Sf;
    vector<VectorXf> zp;

    VectorXf zpi;
    MatrixXf Hvi;
    MatrixXf Hfi;
    MatrixXf Sfi;

    //process each feature, incrementally refine proposal distribution
    unsigned i,r;
    vector<int> j;
    for (i =0; i<idf.size(); i++) {
        j.clear();
        j.push_back(idf[i]);

        Hv.clear();
        Hf.clear();
        Sf.clear();
        zp.clear();

        compute_jacobians(particle,j,R,zp,&Hv,&Hf,&Sf);

        zpi = zp[0];
        Hvi = Hv[0];
        Hfi = Hf[0];
        Sfi = Sf[0];

        VectorXf vi = z[i] - zpi;
        vi[1] = pi_to_pi(vi[1]);

        //proposal covariance
        Pv = Hvi.transpose() * Sfi * Hvi + Pv.inverse();
        Pv = Pv.inverse();

        //proposal mean
        xv = xv + Pv * Hvi.transpose() * Sfi * vi;
        particle.setXv(xv);
        particle.setPv(Pv); 
    }

    //sample from proposal distribution
    VectorXf xvs = multivariate_gauss(xv,Pv,1); 
    particle.setXv(xvs);
    MatrixXf zeros(3,3);
    zeros.setZero();
    particle.setPv(zeros);

    //compute sample weight: w = w* p(z|xk) p(xk|xk-1) / proposal
    float like = likelihood_given_xv(particle, z, idf, R);
    float prior = gauss_evaluate(delta_xv(xv0,xvs), Pv0,0);
    float prop = gauss_evaluate(delta_xv(xv,xvs),Pv,0);
    assert(isfinite(particle.w()));

    float a = prior/prop;
    float b = particle.w() * a;
    float newW = like * b;
    //float newW = particle.w() * like * prior / prop;
    #if 0
    if (!isfinite(newW)) {
	cout<<"LIKELIHOOD GIVEN XV INPUTS"<<endl;   
	cout<<"particle.w()"<<endl;
	cout<<particle.w()<<endl;
	cout<<"particle.xv()"<<endl;
	cout<<particle.xv()<<endl;
	
	cout<<"particle.Pv()"<<endl;
	cout<<particle.Pv()<<endl;
	cout<<"particle.xf"<<endl;
	for (int i =0; i<particle.xf().size(); i++) {
	    cout<<particle.xf()[i]<<endl;
	}
	cout<<endl;
	cout<<"particle.Pf()"<<endl;
	for (int i =0; i< particle.Pf().size(); i++) {
	    cout<<particle.Pf()[i]<<endl;
	}
	cout<<endl;
	
	cout<<"z"<<endl;
	for (int i=0; i<z.size(); i++) {
	    cout<<z[i]<<endl;
	}   
	cout<<endl;
	
	cout<<"idf"<<endl;
	for (int i =0; i<idf.size(); i++){
	    cout<<idf[i]<<" ";
	}		
//.........这里部分代码省略.........

int main(int argc, char **argv){
   int n = atof(argv[1]);
	string method = argv[2];
   double h = 1.0/(n+1.0);
	double max_error = pow(10,-6);
	double error =1;
   time_t t_start,t_end;
	double  dif_iterative, dif_matrix;
   VectorXf  x (n,1);
   VectorXf  xk (n,1);
   VectorXf  xkplus1 (n,1);
   VectorXf  f (n,1);
   MatrixXf A = MatrixXf::Zero(n,n);
   MatrixXf I = MatrixXf::Zero(n,n);
   VectorXf  b (n,1);
   VectorXf xc (n,1);

   for (int i=0; i<n;i++){
      x(i) = h*(i+1);
		f(i) = func(x(i));
   }

	for (int i=0;i<n;i++){
		A(i,i) = 2;
		if (i!=n-1){
			A(i,i+1) = -1;
			A(i+1,i) = -1;
		} 
	}
	b = f*pow(h,2);

	//Solving the Equation
   t_start = time(0);
   xc = A.colPivHouseholderQr().solve(b);
  	t_end=time(0);
  	dif_matrix = difftime (t_end,t_start);

/*
   cout << "Here is the matrix A:\n" << A << endl;
   cout << "Here is the vector x:\n" << x << endl;
   cout << "Here is the vector b:\n" << b << endl;
   cout << "Here is the vector uc:\n" << xc << endl;
*/
	//Jacobin part
	MatrixXf P = MatrixXf::Zero(n,n);
	MatrixXf Pinverse = MatrixXf::Zero(n,n);

	if (method == "Jacobian"){
		for (int i=0; i<n;i++){
      	P(i,i) = A(i,i);
   	}
	} else if (method=="Gauss-Seidel"){
		for (int i=0; i<n;i++){
      	P(i,i) = A(i,i);
			for (int j=0; j<i;j++){
				P(i,j) = A(i,j); 
			}
   	}
	} else if (method=="SOR"){
		double omega = atof(argv[3]);
		for (int i=0; i<n;i++){
      	P(i,i) = A(i,i);
			for (int j=0; j<i;j++){
				P(i,j) = omega*A(i,j); 
			}
   	}

	} else {
		return 1;
	}

	Pinverse = P.inverse();

   for (int i=0; i<n;i++){
      xk(i) = 0;
		I(i,i) = 1;
   }


   t_start = time(0);
	double repeat_error=100*n;
	int loop=1;
	while(error >=max_error){
		xkplus1 = (I-Pinverse*A)*xk+Pinverse*b;
		xk = xkplus1;
		error = maximum_array(xc,xkplus1,n);
		//cout << loop<<endl;
		//cout << error<<endl;

		if (repeat_error == error){
			break;
		}
		repeat_error = error;
		loop +=1;
	}
  t_end=time(0);
	dif_iterative = difftime (t_end,t_start);
	cout << dif_iterative <<"," <<loop<<"," << dif_matrix <<","<<maximum_array(xc,xkplus1,n);

   return 0; 
//.........这里部分代码省略.........