Exemplo n.º 1
0
float likelihood_given_xv(Particle particle, vector<VectorXf> z, vector<int>idf, MatrixXf R) 
{
    float w = 1;

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

    unsigned j,k;
    vector<int> idfi;
    
    for (j=0; j<idf.size(); j++){
	idfi.clear();
        idfi.push_back(idf[j]);
	Hv.clear();
	Hf.clear();
	Sf.clear();
	zp.clear();

	VectorXf v(z.size()); 
        compute_jacobians(particle,idfi,R,zp,&Hv,&Hf,&Sf);
	v = z[j] - zp[0];
        v(1) = pi_to_pi(v(1));
        w = w*gauss_evaluate(v,Sf[0],0);
    } 
    return w;
}
Exemplo n.º 2
0
//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]<<" ";
	}		
	cout<<endl;

	cout<<"R"<<endl;
	cout<<R<<endl;

	cout<<"GAUSS EVALUATE INPUTS"<<endl;
	cout<<"delta_xv(xv0,xvs)"<<endl;	
	cout<<delta_xv(xv0,xvs)<<endl;
	
	cout<<"Pv0"<<endl;
	cout<<Pv0<<endl;
	
	cout<<"delta_xv(xv,xvs)"<<endl;	
	cout<<delta_xv(xv,xvs)<<endl;
	
	cout<<"Pv"<<endl;
	cout<<Pv<<endl;
		
	
	cout<<"like"<<endl;
	cout<<like<<endl;
	cout<<"prior"<<endl;
	cout<<prior<<endl;
	cout<<"prop"<<endl;
	cout<<prop<<endl;
    }
    #endif
    particle.setW(newW);
} 
Exemplo n.º 3
0
void Mass_spring_viewer::time_integration(float dt)
{
    switch (integration_)
    {
        case Euler:
        {
            /** \todo (Part 1) Implement Euler integration scheme
             \li The Particle class has variables position_t and velocity_t to store current values
             \li Hint: compute_forces() computes all forces for the current positions and velocities.
             */
            compute_forces ();
            for (std::vector<Particle>::iterator p_it = body_.particles.begin (); p_it != body_.particles.end (); ++p_it)
              if (!p_it->locked)
              {
                /// 1)
                p_it->acceleration = p_it->force / p_it->mass;

                /// 2)
                p_it->position = p_it->position + dt * p_it->velocity;

                /// 3)
                p_it->velocity = p_it->velocity + dt * p_it->acceleration;
              }

            break;
        }

        case Midpoint:
        {
            /** \todo (Part 2) Implement the Midpoint time integration scheme
             \li The Particle class has variables position_t and velocity_t to store current values
             \li Hint: compute_forces() computes all forces for the current positions and velocities.
             */
             compute_forces();
            for (std::vector<Particle>::iterator p_it = body_.particles.begin (); p_it != body_.particles.end (); ++p_it)
              if (!p_it->locked)
              {
                p_it->position_t = p_it->position;
                p_it->velocity_t = p_it->velocity;
                p_it->acceleration = p_it->force/p_it->mass;

                p_it->position += 0.5*dt * p_it->velocity;
                p_it->velocity += 0.5*dt * p_it->acceleration;
              }
            compute_forces();
            for (std::vector<Particle>::iterator p_it = body_.particles.begin (); p_it != body_.particles.end (); ++p_it)
              if (!p_it->locked)
              {
                p_it->acceleration = p_it->force / p_it->mass;


                p_it->position = p_it->position_t + dt * p_it->velocity;
                p_it->velocity = p_it->velocity_t + dt * p_it->acceleration;
              }

            break;
        }


        case Verlet:
        {
            /** \todo (Part 2) Implement the Verlet time integration scheme
             \li The Particle class has a variable acceleration to remember the previous values
             \li Hint: compute_forces() computes all forces for the current positions and velocities.
             */            
            compute_forces();
            // x(t)  v(t-h)
            for (std::vector<Particle>::iterator p_it = body_.particles.begin (); p_it != body_.particles.end (); ++p_it)
              if (!p_it->locked)
              {
                vec2 a = p_it->force / p_it->mass;
                p_it->velocity += 0.5*dt*(p_it->acceleration + a);

                // x(t) v(t)
                p_it->position += dt*p_it->velocity + 0.5*dt*dt*a;
                p_it->acceleration = a;
              }

            break;
        }

        case Implicit:
        {
            /// The usual force computation method is called, and then the jacobian matrix dF/dx is calculated
            compute_forces ();
            compute_jacobians ();

            /// Finally the linear system is composed and solved
            solver_.solve (dt, particle_mass_, body_.particles);

            break;
        }
    }


    // impulse-based collision handling
    if (collisions_ == Impulse_based)
    {
        impulse_based_collisions();
    }


    // E_c = 1/2 m*v^2
    float total_E = 0.0f;
    for (std::vector<Particle>::iterator p_it = body_.particles.begin (); p_it != body_.particles.end (); ++p_it)
        total_E += 0.5 * p_it->mass * dot(p_it->velocity, p_it->velocity);


    log_file << total_E << "\n";
    glutPostRedisplay();
}