void predict(Particle &particle,float V,float G,Matrix2f Q, float WB,float dt, int addrandom) { VectorXf xv = particle.xv(); MatrixXf Pv = particle.Pv(); #if 0 cout<<"particle.xv() in predict"<<endl; cout<<particle.xv()<<endl; cout<<"particle.Pv() in predict"<<endl; cout<<particle.Pv()<<endl; #endif //Jacobians float phi = xv(2); MatrixXf Gv(3,3); Gv << 1,0,-V*dt*sin(G+phi), 0,1,V*dt*cos(G+phi), 0,0,1; MatrixXf Gu(3,2); Gu << dt*cos(G+phi), -V*dt*sin(G+phi), dt*sin(G+phi), V*dt*cos(G+phi), dt*sin(G)/WB, V*dt*cos(G)/WB; //predict covariance MatrixXf newPv;//(Pv.rows(),Pv.cols()); //TODO: Pv here is somehow corrupted. Probably in sample_proposal #if 0 cout<<"Pv in predict"<<endl; cout<<Pv<<endl; #endif newPv = Gv*Pv*Gv.transpose() + Gu*Q*Gu.transpose(); particle.setPv(newPv); //optional: add random noise to predicted state if (addrandom ==1) { VectorXf A(2); A(0) = V; A(1) = G; VectorXf VG(2); VG = multivariate_gauss(A,Q,1); V = VG(0); G = VG(1); } //predict state VectorXf xv_temp(3); xv_temp << xv(0) + V*dt*cos(G+xv(2)), xv(1) + V*dt*sin(G+xv(2)), pi_to_pi2(xv(2) + V*dt*sin(G/WB)); particle.setXv(xv_temp); }
//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); }