Permalink
Browse files

Merge branch 'master' of https://github.com/yglee/FastSLAM

Conflicts:
	cpp/compute_steering.cpp
	cpp/fastslam2_sim.cpp
  • Loading branch information...
2 parents 8493db1 + 45997fa commit 2070534ca25c2bb6d506392db36ab78c5de1fa63 Yeon Jin Lee committed Aug 22, 2012
View
@@ -8,27 +8,48 @@ using namespace std;
void KF_joseph_update(VectorXf &x, MatrixXf &P,float v,float R, MatrixXf H)
{
VectorXf PHt = P*H.transpose();
-
+ cout<<"KF_Joseph: PHt"<<endl;
+ cout<<PHt<<endl;
MatrixXf S = H*PHt;
S(0,0) += R;
- MatrixXf Si = S.inverse();
- Si = make_symmetric(Si);
+ cout<<"S"<<endl;
+ cout<<S<<endl;
+ MatrixXf Si = S.inverse();
+ cout<<"Si before symmetric"<<endl;
+ cout<<Si<<endl;
+ Si = make_symmetric(Si);
+ cout<<"Si after symmetric"<<endl;
+ cout<<Si<<endl;
MatrixXf PSD_check = Si.llt().matrixL(); //chol of scalar is sqrt
PSD_check.transpose();
PSD_check.conjugate();
+ cout<<"PSD_check"<<endl;
+ cout<<PSD_check<<endl;
VectorXf W = PHt*Si;
+ cout<<"W"<<endl;
+ cout<<W<<endl;
x = x+W*v;
+ cout<<"x"<<endl;
+ cout<<x<<endl;
//Joseph-form covariance update
MatrixXf eye(P.rows(), P.cols());
eye.setIdentity();
MatrixXf C = eye - W*H;
+ cout<<"C"<<endl;
+ cout<<C<<endl;
P = C*P*C.transpose() + W*R*W.transpose();
-
+ cout<<"P"<<endl;
+ cout<<P<<endl;
+
float eps = 2.2204*pow(10.0,-16); //numerical safety
+ cout<<"eps"<<endl;
+ cout<<eps<<endl;
P = P+eye*eps;
-
+ cout<<"P"<<endl;
+ cout<<P<<endl;
+
PSD_check = P.llt().matrixL();
PSD_check.transpose();
PSD_check.conjugate(); //for upper tri
View
@@ -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
@@ -3,7 +3,7 @@
using namespace std;
-void add_control_noise(float &V, float &G, Matrix2f Q, int addnoise, float* VnGn)
+void add_control_noise(float V, float G, Matrix2f Q, int addnoise, float* VnGn)
{
if (addnoise ==1) {
VectorXf A(2);
View
@@ -6,6 +6,6 @@
using namespace Eigen;
-void add_control_noise(float &V, float &G, Matrix2f Q, int addnoise,float* VnGn);
+void add_control_noise(float V, float G, Matrix2f Q, int addnoise,float* VnGn);
#endif //ADD_CONTROL_NOISE
@@ -104,6 +104,6 @@ void compute_jacobians(Particle particle,
//innovation covariance of 'feature observation given the vehicle'
MatrixXf SfMat = HfMat*Pf[i]*HfMat.transpose() + R;
- Sf->push_back(SfMat);
+ Sf->push_back(SfMat);
}
}
View
@@ -30,10 +30,10 @@ void compute_steering(VectorXf x, MatrixXf wp, int& iwp, float minD,
float d2 = pow((cwp[0] - x[0]),2) + pow((cwp[1]-x[1]),2);
if (d2 < minD*minD) {
- iwp = iwp+1; //switch to next
- if (iwp > wp.cols()) {
- iwp =-1;
- return;
+ iwp++; //switch to next
+ if (iwp >= wp.cols()) {
+ iwp =-1;
+ return;
}
cwp[0] = wp(0,iwp); //-1 since indexed from 0
Oops, something went wrong.

0 comments on commit 2070534

Please sign in to comment.