diff --git a/CMakeCache.txt b/CMakeCache.txt
new file mode 100644
index 0000000..751e737
--- /dev/null
+++ b/CMakeCache.txt
@@ -0,0 +1,40 @@
+# This is the CMakeCache file.
+# For build in directory: /Users/ylee8/FastSLAM
+# It was generated by CMake: /opt/local/bin/cmake
+# You can edit this file to change values found and used by cmake.
+# If you do not want to change any of the values, simply exit the editor.
+# If you do want to change a value, simply edit, save, and exit the editor.
+# The syntax for the file is as follows:
+# KEY:TYPE=VALUE
+# KEY is the name of a variable in the cache.
+# TYPE is a hint to GUI's for the type of VALUE, DO NOT EDIT TYPE!.
+# VALUE is the current value for the KEY.
+
+########################
+# EXTERNAL cache entries
+########################
+
+
+########################
+# INTERNAL cache entries
+########################
+
+//This is the directory where this CMakeCache.txt was created
+CMAKE_CACHEFILE_DIR:INTERNAL=/Users/ylee8/FastSLAM
+//Major version of cmake used to create the current loaded cache
+CMAKE_CACHE_MAJOR_VERSION:INTERNAL=2
+//Minor version of cmake used to create the current loaded cache
+CMAKE_CACHE_MINOR_VERSION:INTERNAL=8
+//Patch version of cmake used to create the current loaded cache
+CMAKE_CACHE_PATCH_VERSION:INTERNAL=9
+//Path to CMake executable.
+CMAKE_COMMAND:INTERNAL=/opt/local/bin/cmake
+//Path to cpack program executable.
+CMAKE_CPACK_COMMAND:INTERNAL=/opt/local/bin/cpack
+//Path to ctest program executable.
+CMAKE_CTEST_COMMAND:INTERNAL=/opt/local/bin/ctest
+//Path to cache edit program executable.
+CMAKE_EDIT_COMMAND:INTERNAL=/opt/local/bin/ccmake
+//Path to CMake installation.
+CMAKE_ROOT:INTERNAL=/opt/local/share/cmake-2.8
+
diff --git a/CMakeFiles/cmake.check_cache b/CMakeFiles/cmake.check_cache
new file mode 100644
index 0000000..3dccd73
--- /dev/null
+++ b/CMakeFiles/cmake.check_cache
@@ -0,0 +1 @@
+# This file is generated by cmake for dependency checking of the CMakeCache.txt file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7e4994f..eba6a02 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -47,6 +47,28 @@ find_package(Eigen3 REQUIRED)
 find_package(GooglePerfTools)
 find_package(OpenCL REQUIRED)
 
+#Find Boost
+#FIND_PACKAGE(Boost)
+#IF (Boost_FOUND)
+#    INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR})
+#    ADD_DEFINITIONS( "-DHAS_BOOST" )
+#ENDIF()
+find_package(Boost 1.44.0)
+
+# uncomment the following line to let the env variable BOOST_DIR set the Boost_DIR path
+# set(Boost_DIR $ENV{BOOST_ROOT})
+
+# just print out the variables to see their values
+message("Boost_DIR : " ${Boost_DIR})
+message("BOOST_ROOT: " $ENV{BOOST_ROOT})
+
+# now see if boost was actually found
+if (Boost_FOUND)
+  message("Boost WAS found!")
+endif (Boost_FOUND)
+
+
+
 # Finish up local custom options based on what was found above
 fslam_enable_testing()
 if (ENABLE_PROFILING AND GOOGLE_PERFTOOLS_FOUND)
diff --git a/TODO b/TODO
new file mode 100644
index 0000000..0e838ba
--- /dev/null
+++ b/TODO
@@ -0,0 +1 @@
+multivariate_gauss and other files that use instance of "nRandMat" are wrong. Replace with boost library's normally distributed random function?
diff --git a/cpp/core/add_control_noise.h b/cpp/core/add_control_noise.h
index b694161..3896214 100644
--- a/cpp/core/add_control_noise.h
+++ b/cpp/core/add_control_noise.h
@@ -3,9 +3,13 @@
 
 #include <Eigen/Dense>
 #include "multivariate_gauss.h"
+ # include <cstdlib>
+# include <cmath>
 
 using namespace Eigen;
 
+//MatrixXf randn(int m, int n); 
+//MatrixXf rand(int m, int n);
 void add_control_noise(float V, float G, Matrix2f Q, int addnoise,float* VnGn);
 
 #endif //ADD_CONTROL_NOISE
diff --git a/cpp/core/add_observation_noise.cpp b/cpp/core/add_observation_noise.cpp
index cad096e..772795a 100644
--- a/cpp/core/add_observation_noise.cpp
+++ b/cpp/core/add_observation_noise.cpp
@@ -1,8 +1,7 @@
 #include "add_observation_noise.h"
-
+#if 0
 //http://moby.ihme.washington.edu/bradbell/mat2cpp/randn.cpp.xml
-
-MatrixXf nRandMat::randn(int m, int n) 
+MatrixXf randn(int m, int n) 
 {	
 	// use formula 30.3 of Statistical Distributions (3rd ed)
 	// Merran Evans, Nicholas Hastings, and Brian Peacock
@@ -40,7 +39,7 @@ MatrixXf nRandMat::randn(int m, int n)
 	return x;
 }
 
-MatrixXf nRandMat::rand(int m, int n) 
+MatrixXf rand(int m, int n) 
 {	
 	MatrixXf x(m,n);	
 	int i, j;
@@ -52,15 +51,30 @@ MatrixXf nRandMat::rand(int m, int n)
 	} 
 	return x;
 }
-
+#endif
 //add random measurement noise. We assume R is diagnoal matrix
 void add_observation_noise(vector<VectorXf> &z, MatrixXf R, int addnoise)
 {
+    float LO = -1.0f;
+    float HI = 1.0f;
+
 	if (addnoise == 1){
 		int len = z.size();	
 		if (len > 0) {
-			MatrixXf randM1 = nRandMat::randn(1,len);
-			MatrixXf randM2 = nRandMat::randn(1,len);
+			//MatrixXf randM1 = nRandMat::randn(1,len);
+            MatrixXf randM1(1,len);
+            for (int i=0; i< len; i++) {
+                float r3 = LO + (float)rand()/((float)RAND_MAX/(HI-LO));
+                randM1(0,i) = r3; 
+            }
+			//MatrixXf randM2 = nRandMat::randn(1,len);
+			MatrixXf randM2(1,len);
+            for (int j=0; j< len; j++) {
+                float r4 = LO + (float)rand()/((float)RAND_MAX/(HI-LO));
+                randM2(0,j) = r4; 
+            }
+            //cout<<"randM1"<<endl;
+            //cout<<randM1<<endl;
 
 			for (int c=0; c<len; c++) {
 				z[c][0] = z[c][0] + randM1(0,c)*sqrt(R(0,0));
diff --git a/cpp/core/multivariate_gauss.cpp b/cpp/core/multivariate_gauss.cpp
index b9c56af..9916a81 100644
--- a/cpp/core/multivariate_gauss.cpp
+++ b/cpp/core/multivariate_gauss.cpp
@@ -15,10 +15,22 @@ VectorXf multivariate_gauss(VectorXf x, MatrixXf P, int n)
 	int len = x.size();
 	//choleksy decomposition
 	MatrixXf S = P.llt().matrixL();
-    MatrixXf X;
-	X = nRandMat::randn(len,n);	
-    	
-	//VectorXf ones = //VectorXf::Ones(n).transpose();
+	MatrixXf X(len,n);
+	
+
+    float LO = -1.0f;
+    float HI = 1.0f;
+
+    for (int i = 0; i < len; i++) {
+        for (int j=0; j< n; j++) {
+            float r3 = LO + (float)rand()/((float)RAND_MAX/(HI-LO));
+            X(i,j) = r3;
+        }
+    }
+
+    //TODO: this does not work. Also fixed other instances of nRandMat	
+	//X = nRandMat::randn(len,n);	
+	
 	MatrixXf ones = MatrixXf::Ones(1,n);	
 	return S*X + x*ones;
 }
diff --git a/cpp/core/particle.cpp b/cpp/core/particle.cpp
index 55297f3..71fa5c7 100644
--- a/cpp/core/particle.cpp
+++ b/cpp/core/particle.cpp
@@ -16,7 +16,7 @@ Particle::Particle()
 	_da = NULL;
 }
 
-Particle::Particle(float &w, VectorXf &xv, MatrixXf &Pv, vector<VectorXf> &xf, vector<MatrixXf> &Pf, float* da)
+Particle::Particle(float w, VectorXf &xv, MatrixXf &Pv, vector<VectorXf> &xf, vector<MatrixXf> &Pf, float* da)
 {
 	_w = w;
 	_xv = xv;
@@ -62,7 +62,7 @@ float* Particle::da() const
 }
 
 //setters
-void Particle::setW(float &w)
+void Particle::setW(float w)
 {
 	_w = w;
 }
diff --git a/cpp/core/particle.h b/cpp/core/particle.h
index 7c68a30..86c8107 100644
--- a/cpp/core/particle.h
+++ b/cpp/core/particle.h
@@ -10,7 +10,7 @@ using namespace std;
 class Particle{
 public:
 	Particle();
-	Particle(float &w, VectorXf &xv, MatrixXf &Pv, vector<VectorXf> &xf, vector<MatrixXf> &Pf, float* da);
+	Particle(float w, VectorXf &xv, MatrixXf &Pv, vector<VectorXf> &xf, vector<MatrixXf> &Pf, float* da);
 	~Particle();
         
 	//getters	
@@ -22,7 +22,7 @@ class Particle{
 	float* da() const; //
 
 	//setters
-	void setW(float &w);
+	void setW(float w);
 	void setXv(VectorXf &xv);
 	void setPv(MatrixXf &Pv);
 	void setXf(vector<VectorXf> &xf);
diff --git a/cpp/core/resample_particles.cpp b/cpp/core/resample_particles.cpp
index 5d2765c..d85adc4 100644
--- a/cpp/core/resample_particles.cpp
+++ b/cpp/core/resample_particles.cpp
@@ -1,25 +1,31 @@
+#include <math.h>
 #include "resample_particles.h"
 #include "configfile.h"
 #include "stratified_resample.h"
 #include <iostream>
+#include <assert.h>
 
 using namespace std;
 
 void resample_particles(vector<Particle> &particles, int Nmin, int doresample) 
 {
-    unsigned int N = particles.size();
+    int N = particles.size();
     VectorXf w(N);
     w.setZero();
 
-    int i;    
-    for (i=0; i<N; i++) {
-	w(i) = particles[i].w();
+    for (int i=0; i<N; i++) {
+        w(i) = particles[i].w();
     }
 
     float ws = w.sum();
+    assert(ws != 0);
 
-    for (i=0; i<N; i++) {
-	w(i) = w(i)/ws;
+    for (int i=0; i<N; i++) {
+        w(i) = w(i)/ws;
+    }
+
+    for (int i=0; i<N; i++) {
+        particles[i].setW(particles[i].w()/ws);
     }
 
     float Neff=0;
@@ -28,16 +34,18 @@ void resample_particles(vector<Particle> &particles, int Nmin, int doresample)
 
     vector<Particle> old_particles = vector<Particle>(particles);
     particles.resize(keep.size());	
+    //do I need to clear this? 
+
 
     if ((Neff < Nmin) && (doresample == 1)) {
-	for(i=0; i< keep.size(); i++) {
-	    particles[i] = old_particles[keep[i]]; 	
-	}	
-	for (i=0; i<N; i++) {
-	    cout<<"N"<<endl;
-	    cout<<N<<endl;
-	    float new_w = 1.0f/(float)N;
-	    particles[i].setW(new_w);
-	}
+        for(int i=0; i< keep.size(); i++) {
+            particles[i] = old_particles[keep[i]]; 	
+        }	
+        for (int i=0; i<N; i++) {
+            float new_w = 1.0f/(float)N;
+            assert(isfinite(new_w));
+            assert(N == 100);
+            particles[i].setW(new_w);
+        }
     }		
 }
diff --git a/cpp/fastslam2/fastslam2_sim.cpp b/cpp/fastslam2/fastslam2_sim.cpp
index b1731b4..9019266 100644
--- a/cpp/fastslam2/fastslam2_sim.cpp
+++ b/cpp/fastslam2/fastslam2_sim.cpp
@@ -1,6 +1,7 @@
 #include <iostream>
 #include <math.h>
 #include <vector>
+#include <assert.h>
 
 #include "fastslam2_sim.h"
 #include "core/add_control_noise.h"
@@ -22,10 +23,10 @@ using namespace std;
 vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp) 
 {
     if (SWITCH_PREDICT_NOISE) {
-        printf("Sampling from predict noise usually OFF for FastSLAM 2.0\n");	
+	printf("Sampling from predict noise usually OFF for FastSLAM 2.0\n");	
     }
     if (SWITCH_SAMPLE_PROPOSAL == 0) {
-        printf("Sampling from optimal proposal is usually ON for FastSLAM 2.0\n");
+	printf("Sampling from optimal proposal is usually ON for FastSLAM 2.0\n");
     }
     //normally initialized configfile.h
     Q << pow(sigmaV,2), 0,
@@ -39,13 +40,13 @@ vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp)
     //vector of particles (their count will change)
     vector<Particle> particles(NPARTICLES);
     for (int i=0; i<particles.size(); i++){
-        particles[i] = Particle();
+	particles[i] = Particle();
     }
-   
+
     //initialize particle weights as uniform
     float uniformw = 1.0/(float)NPARTICLES;    
     for (unsigned int p = 0; p < NPARTICLES; p++) {
-	    particles[p].setW(uniformw);
+	particles[p].setW(uniformw);
     }
 
     VectorXf xtrue(3);
@@ -56,112 +57,121 @@ vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp)
 
     vector<int> ftag; //identifier for each landmark
     for (int i=0; i< lm.cols(); i++) {
-        ftag.push_back(i); 
+	ftag.push_back(i); 
     }
 
-    //data ssociation table
+    //data association table
     VectorXf da_table(lm.cols());
     for (int s=0; s<da_table.size(); s++) {
-        da_table[s] = -1;
+	da_table[s] = -1;
     }
 
     int iwp = 0; //index to first waypoint
     float G = 0; //initial steer angle
-    //MatrixXf plines; //will later change to list of points
 
-    if (SWITCH_SEED_RANDOM !=0) {
-        srand(SWITCH_SEED_RANDOM);
-    } 		
+    //if (SWITCH_SEED_RANDOM !=0) {
+	//srand(SWITCH_SEED_RANDOM);
+    //} 		
 
     MatrixXf Qe = MatrixXf(Q);
     MatrixXf Re = MatrixXf(R);
 
     if (SWITCH_INFLATE_NOISE ==1) {
-        Qe = 2*Q;
-        Re = 2*R;
+	Qe = 2*Q;
+	Re = 2*R;
     }
 
     vector<int> ftag_visible;
-   	vector<VectorXf> z;
- 
+    vector<VectorXf> z;
+
     //Main loop
     while (iwp !=-1) {
-        compute_steering(xtrue, wp, iwp, AT_WAYPOINT, G, RATEG, MAXG, dt);
-        if (iwp ==-1 && NUMBER_LOOPS > 1) {
-            iwp = 0;
-            NUMBER_LOOPS = NUMBER_LOOPS-1;
-        }
-        predict_true(xtrue,V,G,WHEELBASE,dt);
-
-        //add process noise
-        float* VnGn = new float[2];
-        add_control_noise(V,G,Q,SWITCH_CONTROL_NOISE,VnGn);
-        float Vn = VnGn[0];
-        float Gn = VnGn[1];
-
-        //Predict step	
-        for (unsigned int i=0; i< NPARTICLES; i++) {
-            predict(particles[i],Vn,Gn,Qe,WHEELBASE,dt,SWITCH_PREDICT_NOISE);
-            observe_heading(particles[i], xtrue(2), SWITCH_HEADING_KNOWN); //if heading known, observe heading
-        }
-
-        //Observe step
-        dtsum = dtsum+dt;
-        if (dtsum >= DT_OBSERVE) {
-            dtsum=0;
-
-            //Compute true data, then add noise
-            ftag_visible = vector<int>(ftag); //modify the copy, not the ftag	
-            
-            //z is the range and bearing of the observed landmark
-            z = get_observations(xtrue,lm,ftag_visible,MAX_RANGE);
-            add_observation_noise(z,R,SWITCH_SENSOR_NOISE);
-            
-	    if(!z.empty()){
-                //plines = make_laser_lines(z,xtrue);
-            }
-
-            //Compute (known) data associations
-            int Nf = particles[0].xf().size(); //(particles[0].xf()).cols();
-            vector<int> idf;
-            vector<VectorXf> zf;
+	compute_steering(xtrue, wp, iwp, AT_WAYPOINT, G, RATEG, MAXG, dt);
+	if (iwp ==-1 && NUMBER_LOOPS > 1) {
+	    iwp = 0;
+	    NUMBER_LOOPS = NUMBER_LOOPS-1;
+	}
+	predict_true(xtrue,V,G,WHEELBASE,dt);
+
+	//add process noise
+	float* VnGn = new float[2];
+	add_control_noise(V,G,Q,SWITCH_CONTROL_NOISE,VnGn);
+	float Vn = VnGn[0];
+	float Gn = VnGn[1];
+
+	//Predict step	
+	for (unsigned int i=0; i< NPARTICLES; i++) {
+	    predict(particles[i],Vn,Gn,Qe,WHEELBASE,dt,SWITCH_PREDICT_NOISE);
+	    assert(isfinite(particles[i].w()));
+	    observe_heading(particles[i], xtrue(2), SWITCH_HEADING_KNOWN); //if heading known, observe heading
+	    assert(isfinite(particles[i].w()));
+	}
+
+	//Observe step
+	dtsum = dtsum+dt;
+	if (dtsum >= DT_OBSERVE) {
+	    dtsum=0;
+
+	    //Compute true data, then add noise
+	    ftag_visible = vector<int>(ftag); //modify the copy, not the ftag	
+
+	    //z is the range and bearing of the observed landmark
+	    z = get_observations(xtrue,lm,ftag_visible,MAX_RANGE);
+	    add_observation_noise(z,R,SWITCH_SENSOR_NOISE);
+
+	    //Compute (known) data associations
+	    int Nf = particles[0].xf().size();
+	    vector<int> idf;
+	    vector<VectorXf> zf;
 	    vector<VectorXf> zn;
 
-            bool testflag= false;
-            data_associate_known(z,ftag_visible,da_table,Nf,zf,idf,zn);
-            
+	    bool testflag= false;
+	    data_associate_known(z,ftag_visible,da_table,Nf,zf,idf,zn);
+
 	    //observe map features
-            if (!zf.empty()) {
-                //isample from 'optimal' proposal distribution, then update map
-                for (unsigned i=0; i<NPARTICLES; i++) {
-                    sample_proposal(particles[i], zf, idf, Re);
-                    feature_update(particles[i], zf, idf, Re);
-                }
-                //resample
-                resample_particles(particles,NEFFECTIVE,SWITCH_RESAMPLE);
-            }
-
-            //Observe new features, augment map
-            if (!zn.empty()) {//!zn.isZero()) {
-                for (unsigned i=0; i<NPARTICLES; i++) {
-                    if (zf.empty()) {//zf.isZero()) {//sample from proposal distribution if we have not already done so above
-                        VectorXf xv = multivariate_gauss(particles[i].xv(),
-                                                        particles[i].Pv(),1);
-                        particles[i].setXv(xv);
-                        MatrixXf pv(3,3); 
-                        pv.setZero();
-                        particles[i].setPv(pv); 
-                    }
-                    add_feature(particles[i], zn, Re);
-                }
-            }	
-            
-			if (VnGn) { 
-                delete[] VnGn;
-            }
-        }
-    }
-    cout<<"done with all functions and will return particles"<<endl<<flush;
-    return particles;
-}
+	    if (!zf.empty()) {
+		//isample from 'optimal' proposal distribution, then update map
+		for (unsigned i=0; i<NPARTICLES; i++) {
+		    assert(isfinite(particles[i].w()));
+		    sample_proposal(particles[i], zf, idf, Re);
+		    
+		    assert(isfinite(particles[i].w()));
+		    feature_update(particles[i], zf, idf, Re);
+		    assert(isfinite(particles[i].w()));
+		}
+		//resample
+		resample_particles(particles,NEFFECTIVE,SWITCH_RESAMPLE);
+
+		//testing
+		for (unsigned i=0; i<NPARTICLES; i++) {
+		    assert(isfinite(particles[i].w()));
+		}   
+	    }
+
+	    //Observe new features, augment map
+	    if (!zn.empty()) {
+		for (unsigned i=0; i<NPARTICLES; i++) {
+		    if (zf.empty()) { //sample from proposal distribution if we have not already done so above
+			VectorXf xv = multivariate_gauss(particles[i].xv(),
+				particles[i].Pv(),1);
+
+			assert(isfinite(particles[i].w()));
+			particles[i].setXv(xv);
+			MatrixXf pv(3,3); 
+			pv.setZero();
+			particles[i].setPv(pv); 
+		    }
+		    add_feature(particles[i], zn, Re);
+		    assert(isfinite(particles[i].w()));
+		    }
+		}	
+
+		if (VnGn) { 
+		    delete[] VnGn;
+		}
+	    }
+	    }
+	    cout<<"done with all functions and will return particles"<<endl<<flush;
+	    return particles;
+	}
 
diff --git a/cpp/fastslam2/gauss_evaluate.cpp b/cpp/fastslam2/gauss_evaluate.cpp
index 563255d..ca38fa7 100644
--- a/cpp/fastslam2/gauss_evaluate.cpp
+++ b/cpp/fastslam2/gauss_evaluate.cpp
@@ -18,7 +18,7 @@ float gauss_evaluate(VectorXf v, MatrixXf S, int logflag)
     float E=0.f;
     VectorXf nin(ni.size());
     for (int s=0; s< ni.size(); s++) {
-	nin[s] = ni[s]*ni[s];
+        nin[s] = ni[s]*ni[s];
     }
     E = -0.5f * nin.sum();
 
@@ -27,24 +27,24 @@ float gauss_evaluate(VectorXf v, MatrixXf S, int logflag)
     unsigned  m = min(Sc.rows(), Sc.cols());
 
     if (logflag !=1) {
-	float prod = 1;
-	for (i=0; i<Sc.rows(); i++) {
-	    for (j=0; j<Sc.cols(); j++) {
-		if (i==j) {
-		    prod=prod*Sc(i,j); //multiply the diagonals 
-		}
-	    }
-	}
-	C = pow((2*pi),(D/2))*prod; //normalizing term(makes Gaussian hyper volume =1
-	w = exp(E)/C; //likelihood (exp(E) is targe distribution).
-			//C is the proposal distribution
+        float prod = 1;
+        for (i=0; i<Sc.rows(); i++) {
+            for (j=0; j<Sc.cols(); j++) {
+                if (i==j) {
+                    prod=prod*Sc(i,j); //multiply the diagonals 
+                }
+            }
+        }
+        C = pow((2*pi),(D/2))*prod; //normalizing term(makes Gaussian hyper volume =1
+        w = exp(E)/C; //likelihood (exp(E) is targe distribution).
+        //C is the proposal distribution
     } 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
+        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
     }    
     return w;
 }
diff --git a/cpp/fastslam2/sample_proposal.cpp b/cpp/fastslam2/sample_proposal.cpp
index ffbf2d8..64437ee 100644
--- a/cpp/fastslam2/sample_proposal.cpp
+++ b/cpp/fastslam2/sample_proposal.cpp
@@ -2,55 +2,54 @@
 #include <iostream>
 #include <Eigen/SVD>
 #include <iomanip>
+#include <math.h>
 #include "assert.h"
 
 //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);	
 
-    //cout<<"xv"<<endl;
-    //cout<<xv<<endl;
-    //cout<<"Pv"<<endl;
-    //cout<<Pv<<endl;
-
     vector<MatrixXf> Hv;
     vector<MatrixXf> Hf;
     vector<MatrixXf> Sf;
     vector<VectorXf> zp;
-    
+
     VectorXf zpi;
     MatrixXf Hvi;
     MatrixXf Hfi;
     MatrixXf Sfi;
 
-    //VectorXf vi(z.rows());
-    vector<VectorXf> vi;
-
     //process each feature, incrementally refine proposal distribution
     unsigned i,r;
+    vector<int> j;
     for (i =0; i<idf.size(); i++) {
-        vector<int> j;
+        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[i];
-        Hvi = Hv[i];
-        Hfi = Hf[i];
-        Sfi = Sf[i].inverse();
+        zpi = zp[0];
+        Hvi = Hv[0];
+        Hfi = Hf[0];
+        Sfi = Sf[0];
+
+        VectorXf vi = z[i] - zpi;
+        vi[1] = pi_to_pi(vi[1]);
 
-	VectorXf vi = z[i] - zpi;
-	vi[1] = pi_to_pi(vi[1]);
-	
         //proposal covariance
-        MatrixXf Pv_inv = Pv.llt().solve(MatrixXf::Identity(Pv.rows(), Pv.cols())); 
-        Pv = Hvi.transpose() * Sfi * Hvi + Pv_inv;
-        Pv = Pv.llt().solve(MatrixXf::Identity(Pv.rows(), Pv.cols()));//Pv.inverse();
+        Pv = Hvi.transpose() * Sfi * Hvi + Pv.inverse();
+        Pv = Pv.inverse();
 
         //proposal mean
         xv = xv + Pv * Hvi.transpose() * Sfi * vi;
@@ -60,52 +59,79 @@ void sample_proposal(Particle &particle, vector<VectorXf> z, vector<int> idf, Ma
 
     //sample from proposal distribution
     VectorXf xvs = multivariate_gauss(xv,Pv,1); 
-    //cout<<"xvs"<<endl;
-    //cout<<xvs<<endl;
     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
-    //cout<<"likelihood given xv input"<<endl;
-    //cout<<"particle.w() "<<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<<"idf"<<endl;
-    for (int i =0; i< idf.size(); i++) {
-	//cout<<idf[i]<<" "<<endl;
-    } 
-    //cout<<endl;
-    //cout<<"R"<<endl;
-    //cout<<R<<endl;
-
-
     float like = likelihood_given_xv(particle, z, idf, R);
-    //cout<<"like "<<like<<endl;
     float prior = gauss_evaluate(delta_xv(xv0,xvs), Pv0,0);
-    //cout<<"prior "<<prior<<endl;
     float prop = gauss_evaluate(delta_xv(xv,xvs),Pv,0);
-    //cout<<"prop "<<prop<<endl;
-
-    float newW = particle.w() * like * prior / prop;
-
+    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);
 } 
 
@@ -116,7 +142,6 @@ float likelihood_given_xv(Particle particle, vector<VectorXf> z, vector<int>idf,
     vector<MatrixXf> Hv;
     vector<MatrixXf> Hf;
     vector<MatrixXf> Sf;
-    
     vector<VectorXf> zp;
 
     unsigned j,k;
@@ -132,34 +157,9 @@ float likelihood_given_xv(Particle particle, vector<VectorXf> z, vector<int>idf,
 
 	VectorXf v(z.size()); 
         compute_jacobians(particle,idfi,R,zp,&Hv,&Hf,&Sf);
-	
-	cout<<"zp"<<endl;
-	for (int i =0; i< zp.size(); i++) {
-	    cout<<zp[i]<<" "<<endl;
-	}
-
-	cout<<"Hv"<<endl;
-	for (int i =0; i< Hv.size(); i++) {
-	    cout<<Hv[i]<<" "<<endl;
-	}
-
-	cout<<"Hf"<<endl;
-	for (int i =0; i< Hf.size(); i++) {
-	    cout<<Hf[i]<<" "<<endl;
-	}
-
-	cout<<"Sf"<<endl;
-	for (int i =0; i< Sf.size(); i++) {
-	    cout<<Sf[i]<<" "<<endl;
-	} 
-
-	cout<<"z["<<j<<"] "<<z[j]<<endl;
 	v = z[j] - zp[0];
-	cout<<"v "<<v<<endl;
         v(1) = pi_to_pi(v(1));
-	cout<<"v "<<v<<endl;
         w = w*gauss_evaluate(v,Sf[0],0);
-	cout<<"w "<<w<<endl;
     } 
     return w;
 }
diff --git a/cpp/gtest/TestFS1AgainstMatlab.cpp b/cpp/gtest/TestFS1AgainstMatlab.cpp
index 1ea0f71..496ce92 100644
--- a/cpp/gtest/TestFS1AgainstMatlab.cpp
+++ b/cpp/gtest/TestFS1AgainstMatlab.cpp
@@ -15,8 +15,11 @@
 #include "core/stratified_resample.h"
 #include "core/stratified_random.h"
 #include "fastslam2/sample_proposal.h"
+#include "core/multivariate_gauss.h"
 
 #include <iostream>
+//#include <boost/random.hpp>
+//#include <boost/random/normal_distribution.hpp>
 
 #define ZERO 1e-10
 #define SMALL 1e-4
@@ -490,7 +493,6 @@ TEST(FASTSLAM_TEST,feature_update_test)
     zf.push_back(e);
     zf.push_back(f);   
 
-
     //idf
     vector<int> idf;
     idf.push_back(0);
@@ -572,7 +574,6 @@ TEST(FASTSLAM_TEST,add_feature_test)
     R<< 0.0100, 0,
 	0, 0.0003;
 
-
     //add_feature
     add_feature(particle,zn,R);    
 
@@ -664,6 +665,7 @@ TEST(FASTSLAM_TEST, stratified_resample_test)
     keep_gt.push_back(9); 
 
     int i;
+    /*
     cout<<"keep"<<endl;
     for (i=0; i<keep.size(); i++) {
 	cout<<keep[i]<<" ";
@@ -675,7 +677,7 @@ TEST(FASTSLAM_TEST, stratified_resample_test)
 	cout<<keep_gt[i]<<" ";
     }
     cout<<endl;
-
+    */
     EXPECT_TRUE(EqualVectors(keep_gt, keep));
     EXPECT_NE(Neff, 8.0764);     
 }
@@ -801,7 +803,7 @@ TEST(FASTSLAM_TEST, likelihood_given_xv_test)
 
     //xv
     VectorXf xv(3);
-    xv << 1.3502, -0.1073, -0.0237;
+    xv << 1.2919, -0.1874,-0.0423;
 
     //Pv
     MatrixXf Pv(3,3);
@@ -810,20 +812,20 @@ TEST(FASTSLAM_TEST, likelihood_given_xv_test)
     //xf
     vector<VectorXf> xf;
     VectorXf a(2);
-    a<<2.2237,-25.8443;
+    a<<2.4261,-25.8041;
     VectorXf b(2);
-    b<<25.6484,3.6701;
+    b<<25.7095,3.2392;
     xf.push_back(a);
     xf.push_back(b);
 
     //Pf
     vector<MatrixXf> Pf;
     MatrixXf c(2,2);
-    c<< 0.2029, 0.0117,
-    0.0117, 0.0107; 
+    c<< 0.2019, 0.0133,
+	0.0133, 0.0109; 
     MatrixXf d(2,2);
-    d<< 0.0140, -0.0268,
-    -0.0268, 0.1904;
+    d<< 0.0131, -0.0239,
+	-0.0239,0.1915;
     Pf.push_back(c);
     Pf.push_back(d);
 
@@ -837,9 +839,9 @@ TEST(FASTSLAM_TEST, likelihood_given_xv_test)
 
     vector<VectorXf> zf;
     VectorXf i(2);
-    i<<25.6029,-1.4864;
+    i<<25.7432,-1.4901;
     VectorXf j(2);
-    j<<24.7119, 0.1680;
+    j<<24.4729, 0.1353;
     zf.push_back(i);
     zf.push_back(j);
      
@@ -853,8 +855,190 @@ TEST(FASTSLAM_TEST, likelihood_given_xv_test)
     R<< 0.0100, 0,
 	0, 0.0003;
    
+    float like = likelihood_given_xv(particle, zf,idf, R);
+    EXPECT_NE(like, 122.7224);
+}
+
+TEST(FASTSLAM_TEST, gauss_evaluate_test)
+{
+   
+    VectorXf v(3);
+    v<<-0.0480,-0.0286,-0.0069;
+
+    MatrixXf Pv0(3,3);
+    Pv0<<0.0004977,-0.0000494, -0.0000091,
+   -0.0000494,0.0001815,0.0000418,
+   -0.0000091,0.0000418,0.0000097;
+
+    float prior_gt = 6378.3;
+
+    float prior = gauss_evaluate(v,Pv0,0);
+    #if 0
+    VectorXf v(3);
+    v <<-0.0306, 0.0077, 0.0017;
+
+    MatrixXf Sf(3,3);
+    Sf << 0.0004983, -0.0000430, -0.0000089, 
+	-0.0000430, 0.0001697, 0.0000393,
+	-0.0000089, 0.0000393, 0.0000091;
+    float prior_gt = 603610;
+    float prior  = gauss_evaluate(v,Sf,0);
+    #endif
+    #if 0
+    VectorXf v(2);
+    v<< -1.18453, 1.62279;
+    
+    MatrixXf Sf(2,2);
+    Sf<<0.020094, -0.000184015,
+    -0.000184015,  0.000607922;
+
+    float prior = gauss_evaluate(v, Sf, 0);
+    float prior_gt = 0; 
+    #endif
+    EXPECT_EQ(prior, prior_gt);
+}
+
+TEST(FASTSLAM_TEST, multivariate_gauss_test)
+{
+    VectorXf x(2);
+    x<< 3.0000, -0.0087;
+
+    MatrixXf P(2,2);
+    P<< 0.09, 0,
+         0, 0.0027;
+
+    int n = 1;
+    VectorXf mg = multivariate_gauss(x,P,n);
+    VectorXf mg_gt(2);
+    mg_gt << 0.1977, 0.2207;
+    EXPECT_EQ(mg, mg_gt);
+}
+
+TEST(FASTSLAM_TEST, delta_xv_test) 
+{
 
-    likelihood_given_xv(particle, zf,idf, R);
 
-    EXPECT_NE(particle.w(), 0.01);
 }
+
+
+TEST(FASTSLAM_TEST, resample_particles_test)
+{
+    vector<Particle> particles(10);
+    for (int i = 0; i< particles.size(); i++) {
+        particles[i] = Particle();
+    }
+   
+    particles[0].setW(5.9056);
+    particles[1].setW(14.0648);
+    particles[2].setW(9.5197);
+    particles[3].setW(21.3748);
+    particles[4].setW(6.5495);
+    particles[5].setW(4.1826);
+    particles[6].setW(17.4620);
+    particles[7].setW(12.8126);
+    particles[8].setW(7.9298);
+    particles[9].setW(20.8908);
+
+    int Nmin = 7; 
+    int doresample = 1; 
+    resample_particles(particles,Nmin,doresample);
+
+    vector<float> w_gt(10);
+    w_gt[0] = 0.0489;
+    w_gt[1] = 0.1165;
+    w_gt[2] = 0.0789;
+    w_gt[3] = 0.1771;
+    w_gt[4] = 0.0543;
+    w_gt[5] = 0.0347;
+    w_gt[6] = 0.1447;
+    w_gt[7] = 0.1062;
+    w_gt[8] = 0.0657;
+    w_gt[9] = 0.1731;
+
+    for (int i =0; i< 10; i++) {
+        EXPECT_EQ(particles[i].w(), w_gt[i]);
+    }
+}
+
+TEST(FASTSLAM_TEST, misc_test)
+{
+    float like = 4.2596*pow(10.f,3.f);
+    float prior = 5.4764*pow(10.f,5.f);
+    float prop = 6.5415* pow(10.f,5.f);
+    float w_gt = 35.6746;
+
+
+    float a = prior/prop;
+    float b = 0.01 * a;
+    float newW = like * b;
+    EXPECT_EQ(newW, w_gt);
+
+    #if 0
+    //MatrixXf P;
+    float total =0;
+    float particle_w= 1.28907*pow(10.0f,30.0f);
+    float like = 3135.09;
+    float prop = 3.07849*pow(10.0f,6.0f);
+    float prior = 3.1012*pow(10.0f,6.0f);
+    
+    cout<<"particle_w "<<particle_w<<endl;
+    cout<<"like "<<like<<endl;
+    cout<<"prop "<<prop<<endl;
+    cout<<"prior "<<prior<<endl;
+    cout<<"prop/prior "<<prop/prior<<endl;
+    float b = prop/prior;
+    cout<<"particle_w * prop/prior "<<particle_w * b<<endl;
+    float c = particle_w * b;
+    float d = c* like;
+    cout<<"total "<< d<<endl;
+    
+    //P = nRandMat::randn(3,10);
+    //cout<<"normalized random matrix"<<endl;    
+    //cout<<P<<endl;
+    #endif
+    #if 0
+    boost::mt19937 rng; // I don't seed it on purpouse (it's not relevant)
+
+    boost::normal_distribution<> nd(0.0, 1.0);
+
+    boost::variate_generator<boost::mt19937&, 
+                           boost::normal_distribution<> > var_nor(rng, nd);
+
+    int i = 0; for (; i < 10; ++i)
+    {
+        double d = var_nor();
+        std::cout << d << std::endl;
+    }
+
+    #endif
+//    boost::minstd_rand random_gen(42u);
+//    boost::normal_distribution<double> normal(0,20);
+    
+/*
+    EXPECT_EQ(exp(2),7.3891);
+    
+    MatrixXf S(3,3);
+    S<< 0.0004960, -0.0000521, -0.0000098,
+    -0.0000521, 0.0001869, 0.0000430,
+    -0.0000098, 0.0000430, 0.0000099;
+
+    MatrixXf cholMat = S.llt().matrixL();
+
+    MatrixXf Sc(3,3);
+    Sc<< 0.0223, 0, 0,
+    -0.0023, 0.0121, 0,
+    -0.0005, 0.0028, 0.0001;
+    
+    VectorXf v(3);
+    v<< -0.0081,-0.0135,-0.0033;
+    
+    VectorXf nin = Sc.jacobiSvd(ComputeThinU | ComputeThinV).solve(v);
+
+    VectorXf nin_gt(3);
+    nin_gt<<-0.3651,-1.1870,-0.6920;
+    EXPECT_EQ(nin, nin_gt);
+   
+    EXPECT_EQ(cholMat, Sc); 
+*/
+}
+