Skip to content
Browse files

lot of bugs fixed from debugging the files individually. THere's stil…

…l a bug in data_assoc and get_obs. looking into it.
  • Loading branch information...
1 parent 2070534 commit fcccc5983d4e63089357b4e1346af20b43db6e6d @yglee committed Aug 23, 2012
View
2 cpp/compute_jacobians.cpp
@@ -99,7 +99,7 @@ void compute_jacobians(Particle particle,
HfMat<< dx/d, dy/d,
-dy/d2, dx/d2;
- Hv->push_back(HvMat);
+ Hv->push_back(HvMat);
Hf->push_back(HfMat);
//innovation covariance of 'feature observation given the vehicle'
View
19 cpp/get_observations.cpp
@@ -45,7 +45,12 @@ void get_visible_landmarks(VectorXf x, MatrixXf &lm, vector<int> &idf, float rma
}
}
lm = MatrixXf(lm_new);
- idf = vector<int>(ii);
+ //idf = vector<int>(ii);
+ vector<int>idf_backup(idf);
+ idf.clear();
+ for(int i=0; i<ii.size(); i++) {
+ idf.push_back(idf_backup[ii[i]]);
+ }
}
MatrixXf compute_range_bearing(VectorXf x, MatrixXf lm)
@@ -71,12 +76,14 @@ vector<int> find2(MatrixXf dx, MatrixXf dy, float phi, float rmax)
{
vector<int> index;
//incremental tests for bounding semi-circle
- for (int i =0; i<dx.cols(); i++) {
- if ((abs(dx(0,i)) < rmax) && (abs(dy(0,i)) < rmax)
- && ((dx(0,i)* cos(phi) + dy(0,i)* sin(phi)) > 0)
- && ((pow(dx(0,i),2) + pow(dy(0,i),2)) < pow(rmax,2))) {
+ for (int j=0; j<dx.rows(); j++) {
+ for (int i =0; i<dx.cols(); i++) {
+ if ((abs(dx(j,i)) < rmax) && (abs(dy(j,i)) < rmax)
+ && ((dx(j,i)* cos(phi) + dy(j,i)* sin(phi)) > 0)
+ && ((pow(dx(j,i),2) + pow(dy(j,i),2)) < pow(rmax,2))) {
index.push_back(i);
}
- }
+ }
+ }
return index;
}
View
8 cpp/multivariate_gauss.cpp
@@ -9,9 +9,11 @@ VectorXf multivariate_gauss(VectorXf x, MatrixXf P, int n)
int len = x.size();
//choleksy decomposition
MatrixXf S = P.llt().matrixL();
- //TODO: why is random doesn't change values?
- //MatrixXf X = MatrixXf::Random(len,n);
- MatrixXf X = MatrixXf::Ones(len,n);
+ cout<<"S"<<endl;
+ cout<<S<<endl;
+ cout<<endl;
+ MatrixXf X(len,n); //TODO this needs to be randomized
+ X<<-0.8809,0.8571,0.2638;
VectorXf ones = VectorXf::Ones(n).transpose();
return S*X + x*ones;
}
View
64 cpp/pi_to_pi.cpp
@@ -1,49 +1,42 @@
#include "pi_to_pi.h"
+#include <iostream>
-void pi_to_pi(VectorXf &angle)
-{
- vector<int> index;
- index = find1(angle);
- vector<int>::iterator iter;
-
- if (!index.empty()){
- for (iter = index.begin(); iter != index.end(); iter++) {
- //remember that iterators are pointers
- int n = floor(angle[*iter] / 2*pi);
- angle[*iter] = angle[*iter] - n*(2*pi);//angle[*iter] % (2*pi);
- }
- }
-
- for (int i = 0; i< angle.size(); i++) {
- if (angle[i] > pi) {
- angle[i] = angle[i]-2*pi;
- }
- }
-
- for (int j=0; j< angle.size(); j++) {
- if(angle[j] < -1*pi){
- angle[j] = angle[j]+2*pi;
+void pi_to_pi(VectorXf &angle)
+{
+ int n;
+ for (int i=0; i<angle.size(); i++) {
+ if ((angle[i] < (-2*pi)) || (angle[i] > (2*pi))) {
+ n=floor(angle[i]/(2*pi));
+ angle[i] = angle[i]-n*(2*pi);
+
+ if (angle[i] > pi) {
+ angle[i] = angle[i] - (2*pi);
+ }
+ if (angle[i] < -pi) {
+ angle[i] = angle[i] + (2*pi);
+ }
}
}
}
float pi_to_pi(float ang)
{
- if ((ang<-2*pi) || (ang>2*pi)) {
- int n = floor(ang / 2*pi);
- ang = ang-n*2*pi;
- }
+ int n;
+ if ((ang < (-2*pi)) || (ang > (2*pi))) {
+ n=floor(ang/(2*pi));
+ ang = ang-n*(2*pi);
- if(ang > pi) {
- ang = ang-2*pi;
- }
-
- if (ang < -pi) {
- ang = ang+2*pi;
- }
- return ang;
+ if (ang > pi) {
+ ang = ang - (2*pi);
+ }
+ if (ang < -pi) {
+ ang = ang + (2*pi);
+ }
+ }
+ return ang;
}
+#if 0
vector<int> find1(VectorXf input)
{
vector<int> index;
@@ -54,3 +47,4 @@ vector<int> find1(VectorXf input)
}
return index;
}
+#endif
View
16 cpp/predict.cpp
@@ -2,6 +2,8 @@
#include <math.h>
#include <iostream>
+
+#define pi 3.1416
//checked all values. everything works
using namespace std;
@@ -55,6 +57,18 @@ void predict(Particle &particle,float V,float G,Matrix2f Q, float WB,float dt, i
VectorXf xv_temp(3);
xv_temp << xv(0) + V*dt*cos(G+xv(2)),
xv(1) + V*dt*sin(G+xv(2)),
- pi_to_pi(xv(2) + V*dt*sin(G/WB));
+ pi_to_pi2(xv(2) + V*dt*sin(G/WB));
particle.setXv(xv_temp);
}
+
+float pi_to_pi2(float ang)
+{
+ if (ang > pi) {
+ ang = ang - (2*pi);
+ }
+ if (ang < -pi) {
+ ang = ang + (2*pi);
+ }
+ return ang;
+}
+
View
3 cpp/predict.h
@@ -4,10 +4,11 @@
#include <Eigen/Dense>
#include "particle.h"
#include "multivariate_gauss.h"
-#include "pi_to_pi.h"
using namespace Eigen;
void predict(Particle &particle,float V,float G,Matrix2f Q, float WB,float dt, int addrandom);
+float pi_to_pi2(float ang);
+
#endif //PREDICT_H
View
10 cpp/resample_particles.cpp
@@ -12,21 +12,19 @@ void resample_particles(vector<Particle> &particles, int Nmin, int doresample)
VectorXf w(N);
w.setZero();
- unsigned i;
+ int i;
for (i=0; i<N; i++) {
w(i) = particles[i].w();
//cout<<"w at "<<i<<" "<<w(i)<<endl;
}
float ws = w.sum();
- //TODO: ws is zero!
- cout<<"ws is "<<ws<<endl;
for (i=0; i<N; i++) {
w(i) = w(i)/ws;
}
- float Neff;
+ float Neff=0;
vector<int> keep;
stratified_resample(w,keep,Neff);
@@ -38,7 +36,9 @@ void resample_particles(vector<Particle> &particles, int Nmin, int doresample)
particles[i] = old_particles[keep[i]];
}
for (i=0; i<N; i++) {
- particles[i].setW(1/N);
+ cout<<"N"<<endl;
+ cout<<N<<endl;
+ particles[i].setW(1.0/(float)N);
}
}
}
View
42 cpp/sample_proposal.cpp
@@ -55,7 +55,7 @@ void sample_proposal(Particle &particle, MatrixXf z, vector<int> idf, MatrixXf R
//cout<<"vi["<<r<<"] "<<vi[r]<<endl;
}
-#if 1
+#if 0
cout<<"in sample proposal"<<endl;
cout<<"z"<<endl;
cout<<z<<endl;
@@ -134,36 +134,6 @@ 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++){
@@ -179,19 +149,23 @@ float likelihood_given_xv(Particle particle, MatrixXf z, vector<int>idf, MatrixX
cout<<endl;
for (k=0; k<z.rows(); k++) {
- v(k) = z(k,i)-zp(k,0); //TODO: this returns wrong values
+ v(k) = z(k,i)-zp(k,i); //TODO: this returns wrong values
}
v(1) = pi_to_pi(v(1));
+ cout<<"GAUSS EVAL INPUTS"<<endl;
cout<<"v"<<endl;
cout<<v<<endl;
cout<<endl;
+ cout<<"sf"<<endl;
+ cout<<Sf[0]<<endl;
+ cout<<"gauss_eval "<<gauss_evaluate(v,Sf[0],0)<<endl;
w = w*gauss_evaluate(v,Sf[0],0);
- cout<<"new w: "<<w<<endl;
+ //cout<<"new w: "<<w<<endl;
}
- cout<<"w returned "<<w<<endl;
+ //cout<<"w returned "<<w<<endl;
return w;
}
View
14 cpp/stratified_resample.cpp
@@ -12,16 +12,22 @@ void stratified_resample(VectorXf w, vector<int> &keep, float &Neff)
wsqrd(i) = pow(w(i),2);
}
Neff = 1/wsqrd.sum();
+
int len = w.size();
+ keep.resize(len);
+ for (int i=0; i<len; i++) {
+ keep[i] = -1;
+ }
+
vector<float> select;
- cout<<"len in stratified resample "<<len<<endl;
stratified_random(len,select);
cumsum(w);
int ctr=0;
for (int i=0; i<len; i++) {
- while ((ctr<len) && (select[ctr] < w(i))) {
- keep.push_back(i);
+ while ((ctr<len) && (select[ctr]<w(i))) {
+ keep[ctr] = i;
+ ctr++;
}
}
}
@@ -34,7 +40,7 @@ void cumsum(VectorXf &w)
VectorXf csumVec(w.size());
for (int i=0; i< w.size(); i++) {
float sum =0;
- for (int j=0; j<i; j++) {
+ for (int j=0; j<=i; j++) {
sum+=w(j);
}
csumVec(i) = sum;

0 comments on commit fcccc59

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