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;
}
//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);
}
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();
}
