Skip to content

Commit

Permalink
working on bug in feature_update
Browse files Browse the repository at this point in the history
  • Loading branch information
@yglee
yglee committed Aug 17, 2012
1 parent 9c204f0 commit 7e39036
Show file tree
Hide file tree
Showing 5 changed files with 48 additions and 37 deletions.
7 changes: 5 additions & 2 deletions cpp/add_feature.cpp
View file
Expand Up @@ -51,9 +51,10 @@ void add_feature(Particle &particle, MatrixXf z, MatrixXf R)


for (unsigned c=0; c<ii.size(); c++) {
cout<<"ii"<<endl;
#if 0
cout<<"ii"<<endl;
cout<<ii[c]<<endl;

#endif

if (xfCopy.isZero()) {
xfCopy = xf;
Expand All @@ -69,6 +70,7 @@ void add_feature(Particle &particle, MatrixXf z, MatrixXf R)
}
}

#if 0
cout<<"xf in add_feature"<<endl;
cout<<xf<<endl;
cout<<"should be"<<endl;
Expand All @@ -79,6 +81,7 @@ void add_feature(Particle &particle, MatrixXf z, MatrixXf R)
cout<<"xfCopy"<<endl;
cout<<xfCopy<<endl;
cout<<endl;
#endif

particle.setXf(xfCopy);
particle.setPf(pfCopy);
Expand Down
8 changes: 5 additions & 3 deletions cpp/compute_jacobians.cpp
View file
Expand Up @@ -94,12 +94,14 @@ void compute_jacobians(Particle particle,

//Jacobian wrt vehicle states
HvMat<< -dx/d, -dy/d, 0,
dy/d2, -dx/d2,-1;
dy/d2, -dx/d2,-1;

//Jacobian wrt feature states
HfMat<< dx/d, dy/d,
-dy/d2, dx/d2;
Hv->push_back(HvMat);
-dy/d2, dx/d2;
cout<<"HvMat"<<endl;
cout<<HvMat<<endl;
Hv->push_back(HvMat);
Hf->push_back(HfMat);

//innovation covariance of 'feature observation given the vehicle'
Expand Down
14 changes: 4 additions & 10 deletions cpp/feature_update.cpp
View file
Expand Up @@ -11,29 +11,22 @@ void feature_update(Particle &particle, MatrixXf z, vector<int>idf, MatrixXf R)
MatrixXf xf(rows,idf.size());
vector<MatrixXf> Pf;

cout<<"particles.Pf()[0]"<<endl;
cout<<(particle.Pf())[0]<<endl;

cout<<"idf in feature update"<<endl;
for (int i=0; i< idf.size(); i++) {
cout<<"idf["<<i<<"]: "<<idf[i]<<endl;
}

unsigned i,r;
for (i=0; i<idf.size(); i++) {
for (r=0; r<(particle.xf()).rows(); r++) {
xf(r,i) = (particle.xf())(r,(idf[i]));
}
Pf[i] = (particle.Pf())[idf[i]]; //particle.Pf is a array of matrices
Pf.push_back((particle.Pf())[idf[i]]); //particle.Pf is a array of matrices
}

MatrixXf zp;
vector<MatrixXf> *Hv;
vector<MatrixXf> *Hf;
vector<MatrixXf> *Sf;
compute_jacobians(particle,idf,R,zp,Hv,Hf,Sf);
cout<<"after compute_jacobians"<<endl;

#if 0
cout<<"after compute_jacobians"<<endl;
cout<<endl;
cout<<"in feature_update"<<endl;
cout<<"zp is "<<endl;
Expand All @@ -42,6 +35,7 @@ void feature_update(Particle &particle, MatrixXf z, vector<int>idf, MatrixXf R)
cout<<"25.6675 24.5294"<<endl;
cout<<"-1.4925 0.1547"<<endl;
cout<<endl;
#endif

MatrixXf v = z-zp; //TODO: need to fix: idf.size() is zero. which makes this break.

Expand Down
2 changes: 1 addition & 1 deletion cpp/makefile
View file
Expand Up @@ -21,7 +21,7 @@ else
INCLUDE := -I./include/ -I/usr/X11R6/include -I/sw/include \
-I/usr/sww/include -I/usr/sww/pkg/Mesa/include -I/usr/include/eigen3
LIBRARY := -L./lib/nix -L/usr/X11R6/lib -L/sw/lib -L/usr/sww/lib \
-L/usr/sww/bin -L/usr/sww/pkg/Mesa/lib -lX11
-L/usr/sww/bin -L/usr/sww/pkg/Mesa/lib
FRAMEWORK :=
MACROS :=
PLATFORM := *Nix
Expand Down
54 changes: 33 additions & 21 deletions cpp/sample_proposal.cpp
View file
Expand Up @@ -22,7 +22,7 @@ void sample_proposal(Particle &particle, MatrixXf z, vector<int> idf, MatrixXf R
MatrixXf Hfi;
MatrixXf Sfi;

VectorXf vi;//(z.rows(),1);
VectorXf vi(z.rows(),1);
#if 0
cout<<"Pv in sample_proposal"<<endl;
cout<<Pv<<endl;
Expand All @@ -45,16 +45,31 @@ void sample_proposal(Particle &particle, MatrixXf z, vector<int> idf, MatrixXf R
Hfi = Hf[i];
Sfi = Sf[i].inverse();

#if 0
cout<<"i in SAMPLE_PROPOSAL"<<endl;
cout<<i<<endl;
cout<<"should be 1"<<endl;
for (r=0; r<z.rows(); r++) {
vi[r] = z(r,i) - zpi(r,i);
cout<<endl;

cout<<"vi"<<endl;
cout<<vi<<endl;
cout<<endl;
cout<<"z"<<endl;
cout<<z<<endl;
cout<<endl;
cout<<"zpi"<<endl;
cout<<zpi<<endl;
#endif
for (r=0; r<z.rows(); r++) {
vi[r] = z(r,i) - zpi(r,0);
}
#if 0
cout<<"vi"<<endl;
cout<<vi<<endl;
cout<<"should be [0.1355; -0.0333]"<<endl;
cout<<endl;
#endif
vi[1] = pi_to_pi(vi[1]);
//vi<<1,2;
//vi = z.conservativeResize(z.rows(),1) - zpi;
//vi(1,0) = pi_to_pi(vi(1,0));

#if 0
cout<<"Hfi"<<endl;
Expand All @@ -69,8 +84,10 @@ void sample_proposal(Particle &particle, MatrixXf z, vector<int> idf, MatrixXf R
cout<<vi<<endl;
#endif
//add a little bias so I won't get NaNs when I do an inverse
cout<<"Pv"<<endl;
#if 0
cout<<"Pv"<<endl;
cout<<Pv<<endl;
#endif

for (r=0; r<Pv.rows(); r++) {
for (c=0; c<Pv.cols(); c++) {
Expand All @@ -80,10 +97,10 @@ void sample_proposal(Particle &particle, MatrixXf z, vector<int> idf, MatrixXf R
}
}

#if 0
#if 0
cout<<"Pv.inverse()"<<endl;
cout<<Pv.inverse()<<endl;
#endif
#endif

//proposal covariance
Pv = Hvi.transpose() * Sfi * Hvi + Pv.inverse();
Expand Down Expand Up @@ -123,7 +140,7 @@ void sample_proposal(Particle &particle, MatrixXf z, vector<int> idf, MatrixXf R
float likelihood_given_xv(Particle &particle, MatrixXf z, vector<int>idf, MatrixXf R)
{
float w = 1;
vector<int> temp;
vector<int> idfi;

vector<MatrixXf> Hv;
vector<MatrixXf> Hf;
Expand All @@ -133,26 +150,21 @@ float likelihood_given_xv(Particle &particle, MatrixXf z, vector<int>idf, Matrix
MatrixXf Hvi;
MatrixXf Hfi;
MatrixXf Sfi;
MatrixXf zi(z.rows(),1);
MatrixXf v;
VectorXf v(z.rows());

unsigned i,k;
for (i=0; i<idf.size(); i++){
temp.push_back(i);
compute_jacobians(particle,temp,R,zp,&Hv,&Hf,&Sf);
idfi.push_back(i);
compute_jacobians(particle,idfi,R,zp,&Hv,&Hf,&Sf);
Hvi = Hv[0];
Hfi = Hf[0];
Sfi = Sf[0];
cout<<"in LIKELIHOOD GIVEN XV"<<endl;
cout<<"zi"<<endl;
cout<<zi<<endl;
cout<<"can we access row: "<<z.rows()<<" and cols: "<<idf.size()<<endl;

for (k=0; k<z.rows(); k++) {
zi(k,i);
v(k) = z(k,i)-zp(k,0);
}
v = zi-zp;
v(1,0) = pi_to_pi(v(1,0));
v(1) = pi_to_pi(v(1));
w = w*gauss_evaluate(v,Sf[i],0);
}
return w;
Expand Down

0 comments on commit 7e39036

Please sign in to comment.