Skip to content
Browse files

start at sample_proposal. v is wrong there

  • Loading branch information...
1 parent 22b0640 commit a9a88a2319517b618d85bf81032f18693391f578 @yglee committed Aug 21, 2012
Showing with 83 additions and 22 deletions.
  1. +0 −1 cpp/KF_joseph_update.cpp
  2. +11 −1 cpp/README
  3. +29 −12 cpp/gauss_evaluate.cpp
  4. +0 −2 cpp/observe_heading.cpp
  5. +43 −6 cpp/sample_proposal.cpp
View
1 cpp/KF_joseph_update.cpp
@@ -53,7 +53,6 @@ void KF_joseph_update(VectorXf &x, MatrixXf &P,float v,float R, MatrixXf H)
PSD_check = P.llt().matrixL();
PSD_check.transpose();
PSD_check.conjugate(); //for upper tri
- //break
}
MatrixXf make_symmetric(MatrixXf P)
View
12 cpp/README
@@ -1,3 +1,13 @@
-todo:
+
+8/21/12
+even a little bit of numerical error results in exp(E) to go zero (when E<0) in gauss evaluate.
+The problem is the input to gauss evaluate, v. It's set in sample_proposal. line 172. v is wrong...
+
+Do a check:
+is likelhood_given_xv returning a 0 with the right inputs? Then, the function has a problem. If the inputs are wrong, then sample_particles might be wrong. Check this (add a break in likelihood_given_xv.
+
+
+w is zero in resample particles. w is set in sample_proposal. The likelihood given xv returns a 0 everytime, causing w to be 0. This results in nan's, which should throw a division by 0 error
+
7/25/12
fix the multivariate gauss's use of Random() (from eigen). It doesn't return diff. random numbers at each turn
View
41 cpp/gauss_evaluate.cpp
@@ -10,23 +10,30 @@ using namespace std;
float gauss_evaluate(VectorXf v, MatrixXf S, int logflag)
{
#if 0
- cout<<"in gauss_eval"<<endl;
- cout<<"v"<<endl;
- cout<<v<<endl;
- cout<<endl;
- cout<<"S"<<endl;
- cout<<S<<endl;
+ //cout<<"in gauss_eval"<<endl;
+ //cout<<"v"<<endl;
+ //cout<<v<<endl;
+ //cout<<endl;
+ //cout<<"S"<<endl;
+ //cout<<S<<endl;
#endif
int D = v.size();
MatrixXf Sc = S.llt().matrixL();
-
- //normalised innovations
+ //cout<<"Sc"<<endl;
+ //cout<<Sc<<endl;
+ //cout<<endl;
+ //cout<<"v"<<endl;
+ //cout<<v<<endl;
+
+ //normalised innovation
VectorXf nin = Sc.jacobiSvd(ComputeThinU | ComputeThinV).solve(v);
+ //cout<<"nin in gauss_eval"<<endl;
+ //cout<<nin<<endl;
#if 0
- cout<<"nin"<<endl;
- cout<<nin<<endl;
+ //cout<<"nin"<<endl;
+ //cout<<nin<<endl;
#endif
int s;
@@ -37,7 +44,7 @@ float gauss_evaluate(VectorXf v, MatrixXf S, int logflag)
E += nin(s);
}
E=-0.5*E;
- //Note: writing sum (x.*x,1) is a fast way to compute sets of inner-products
+ //Note: above is a fast way to compute sets of inner-products
unsigned i;
float C,w;
@@ -48,15 +55,25 @@ float gauss_evaluate(VectorXf v, MatrixXf S, int logflag)
for (i=0; i<m; i++) {
prod=prod*Sc(i,i); //multiply the diagonals
}
- C = pow((2*pi),(D/2))*prod; //normalizing term(makes Gaussian hyper volume =1)
+ C = pow((2*pi),(D/2))*prod; //normalizing term(makes Gaussian hyper volume =1
+ //cout<<"C in gauss_eval"<<endl;
+ //cout<<C<<endl;
+ //cout<<"E in gauss_eval"<<endl;
+ //cout<<E<<endl;
+ //cout<<"exp(E)"<<endl;
+ //cout<<exp(E)<<endl;
w = exp(E)/C; //likelihood
+ //cout<<"w in gauss eval"<<endl;
+ //cout<<w<<endl;
} else {
float sum=0;
for (i=0; i<m; i++) {
sum+=log(Sc(i,i));
}
C = 0.5*D*log(2*pi) + sum; //log of normalising term
w = E-C; //log-likelihood
+ //cout<<"w in gauss eval 2"<<endl;
+ //cout<<w<<endl;
}
return w;
}
View
2 cpp/observe_heading.cpp
@@ -36,6 +36,4 @@ void observe_heading(Particle &particle, float phi, int useheading)
particle.setXv(xv);
particle.setPv(Pv);
- //break here
- cout<<particle.Pf()[1000]<<endl;
}
View
49 cpp/sample_proposal.cpp
@@ -93,7 +93,9 @@ void sample_proposal(Particle &particle, MatrixXf z, vector<int> idf, MatrixXf R
//sample from proposal distribution
VectorXf xvs = multivariate_gauss(xv,Pv,1);
particle.setXv(xvs);
- particle.setPv(MatrixXf(3,3));
+ 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);
@@ -127,22 +129,57 @@ float likelihood_given_xv(Particle particle, MatrixXf z, vector<int>idf, MatrixX
MatrixXf Sfi;
VectorXf v(z.rows());
+ cout<<"xv"<<endl;
+ cout<<particle.xv()<<endl;
+ cout<<endl;
+ cout<<"Pv"<<endl;
+ cout<<particle.Pv()<<endl;
+ cout<<endl;
+ cout<<"xf"<<endl;
+ cout<<particle.xf()<<endl;
+ cout<<endl;
+ cout<<"Pf"<<endl;
+ cout<<particle.Pf()[0]<<endl;
+ cout<<endl;
+ cout<<particle.Pf()[1]<<endl;
+ cout<<endl;
+ cout<<"w"<<endl;
+ cout<<particle.w()<<endl;
+ cout<<endl;
+ cout<<"z"<<endl;
+ cout<<z<<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;
+
unsigned i,k;
//cout<<"idf.size() "<<idf.size()<<endl;
for (i=0; i<idf.size(); i++){
- idfi.push_back(i);
+ idfi.push_back(idf[i]);
compute_jacobians(particle,idfi,R,zp,&Hv,&Hf,&Sf);
- Hvi = Hv[0];
- Hfi = Hf[0];
- Sfi = Sf[0];
+ Hvi = Hv[i];//Hv[0];
+ Hfi = Hf[i];//Hf[0];
+ Sfi = Sf[i];//Sf[0];
for (k=0; k<z.rows(); k++) {
v(k) = z(k,i)-zp(k,0); //TODO: this returns wrong values
}
v(1) = pi_to_pi(v(1));
- w = w*gauss_evaluate(v,Sf[i],0);
+ w = w*gauss_evaluate(v,Sfi,0);//Sf[i],0);
+ cout<<"new w: "<<w<<endl;
}
+ cout<<"w returned "<<w<<endl;
+
+ //break
+ cout<<particle.Pf()[100]<<endl;
return w;
}

0 comments on commit a9a88a2

Please sign in to comment.
Something went wrong with that request. Please try again.