Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

testcode dir removed

  • Loading branch information...
commit 43303ceb677957d5cefcf3abadfa407cbc1fb757 1 parent c8987a4
@yglee authored
Showing with 0 additions and 28,382 deletions.
  1. +0 −5 testcode/.gitignore
  2. +0 −20 testcode/KF_cholesky_update.cpp
  3. +0 −10 testcode/KF_cholesky_update.h
  4. +0 −61 testcode/KF_joseph_update.cpp
  5. +0 −11 testcode/KF_joseph_update.h
  6. +0 −13 testcode/README
  7. +0 −30 testcode/TransformToGlobal.cpp
  8. +0 −11 testcode/TransformToGlobal.h
  9. +0 −17 testcode/add_control_noise.cpp
  10. +0 −11 testcode/add_control_noise.h
  11. +0 −122 testcode/add_feature.cpp
  12. +0 −13 testcode/add_feature.h
  13. +0 −20 testcode/add_observation_noise.cpp
  14. +0 −11 testcode/add_observation_noise.h
  15. +0 −61 testcode/compute_jacobians.cpp
  16. +0 −19 testcode/compute_jacobians.h
  17. +0 −60 testcode/compute_steering.cpp
  18. +0 −11 testcode/compute_steering.h
  19. +0 −50 testcode/configfile.cpp
  20. +0 −47 testcode/configfile.h
  21. +0 −56 testcode/data_associate_known.cpp
  22. +0 −13 testcode/data_associate_known.h
  23. +0 −56 testcode/example_webmap.mat
  24. +0 −277 testcode/fastslam2_sim.cpp
  25. +0 −22 testcode/fastslam2_sim.h
  26. +0 −76 testcode/feature_update.cpp
  27. +0 −17 testcode/feature_update.h
  28. +0 −79 testcode/gauss_evaluate.cpp
  29. +0 −26 testcode/gauss_evaluate.h
  30. +0 −88 testcode/get_observations.cpp
  31. +0 −14 testcode/get_observations.h
  32. +0 −36 testcode/line_plot_conversion.cpp
  33. +0 −10 testcode/line_plot_conversion.h
  34. +0 −136 testcode/main.cpp
  35. +0 −56 testcode/makefile
  36. +0 −20 testcode/multivariate_gauss.cpp
  37. +0 −11 testcode/multivariate_gauss.h
  38. +0 −39 testcode/observe_heading.cpp
  39. +0 −9 testcode/observe_heading.h
  40. +0 −26,003 testcode/output_old
  41. +0 −101 testcode/particle.cpp
  42. +0 −41 testcode/particle.h
  43. +0 −38 testcode/pi_to_pi.cpp
  44. +0 −16 testcode/pi_to_pi.h
  45. +0 −74 testcode/predict.cpp
  46. +0 −14 testcode/predict.h
  47. +0 −9 testcode/predict_true.cpp
  48. +0 −13 testcode/predict_true.h
  49. +0 −15 testcode/printMat.h
  50. +0 −44 testcode/resample_particles.cpp
  51. +0 −14 testcode/resample_particles.h
  52. +0 −164 testcode/sample_proposal.cpp
  53. +0 −27 testcode/sample_proposal.h
  54. +0 −33 testcode/stratified_random.cpp
  55. +0 −13 testcode/stratified_random.h
  56. +0 −49 testcode/stratified_resample.cpp
  57. +0 −13 testcode/stratified_resample.h
  58. +0 −57 testcode/test_main.cc
View
5 testcode/.gitignore
@@ -1,5 +0,0 @@
-*.swp
-test/**/*
-*~
-*.o
-.DS_Store
View
20 testcode/KF_cholesky_update.cpp
@@ -1,20 +0,0 @@
-#include "KF_cholesky_update.h"
-
-void KF_cholesky_update(VectorXf &x, MatrixXf &P,VectorXf v,MatrixXf R,MatrixXf H)
-{
- MatrixXf PHt = P*H.transpose();
- MatrixXf S = H*PHt + R;
-
- S = (S+S.transpose()) * 0.5; //make symmetric
- MatrixXf SChol = S.llt().matrixL();
- SChol.transpose();
- SChol.conjugate();
-
- MatrixXf SCholInv = SChol.inverse(); //tri matrix
- MatrixXf W1 = PHt * SCholInv;
- MatrixXf W = W1 * SCholInv.transpose();
-
- //TODO: last three lines
- x = x + W*v;
- P = P - W1*W1.transpose();
-}
View
10 testcode/KF_cholesky_update.h
@@ -1,10 +0,0 @@
-#ifndef KF_CHOLESKY_UPDATE_H
-#define KF_CHOLESKY_UPDATE_H
-
-#include <Eigen/Dense>
-
-using namespace Eigen;
-
-void KF_cholesky_update(VectorXf &x,MatrixXf &P,VectorXf v,MatrixXf R,MatrixXf H);
-
-#endif
View
61 testcode/KF_joseph_update.cpp
@@ -1,61 +0,0 @@
-#include "KF_joseph_update.h"
-#include <iostream>
-#include <math.h>
-
-using namespace std;
-
-//TODO: check this!
-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;
- 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
-}
-
-MatrixXf make_symmetric(MatrixXf P)
-{
- return (P + P.transpose())*0.5;
-}
View
11 testcode/KF_joseph_update.h
@@ -1,11 +0,0 @@
-#ifndef KF_JOSEPH_UPDATE_H
-#define KF_JOSEPH_UPDATE_H
-
-#include <Eigen/Dense>
-
-using namespace Eigen;
-
-void KF_joseph_update(VectorXf &x,MatrixXf &P,float v,float R, MatrixXf H);
-MatrixXf make_symmetric(MatrixXf P);
-
-#endif
View
13 testcode/README
@@ -1,13 +0,0 @@
-
-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
30 testcode/TransformToGlobal.cpp
@@ -1,30 +0,0 @@
-#include "TransformToGlobal.h"
-#include "pi_to_pi.h"
-
-void TransformToGlobal(MatrixXf &p, VectorXf b)
-{
- //rotate
- MatrixXf rot(2,2);
- rot<<cos(b(2)), -sin(b(2)), sin(b(2)), cos(b(2));
-
- MatrixXf p_resized;
- p_resized = MatrixXf(p);
- p_resized.conservativeResize(2,p_resized.cols());
- p_resized = rot*p_resized;
-
- //translate
- int c;
- for (c=0;c<p_resized.cols();c++) {
- p(0,c) = p_resized(0,c)+b(0);
- p(1,c) = p_resized(1,c)+b(1);
- }
-
- float input;
- //if p is a pose and not a point
- if (p.rows() ==3){
- for (int k=0; k<p_resized.cols();k++) {
- input = p(2,k) +b(2);
- p(2,k) = pi_to_pi(input);
- }
- }
-}
View
11 testcode/TransformToGlobal.h
@@ -1,11 +0,0 @@
-#ifndef TRANSFORMGLOBAL_H
-#define TRANSFORMGLOBAL_H
-
-#include <Eigen/Dense>
-#include <iostream>
-
-using namespace Eigen;
-
-void TransformToGlobal(MatrixXf &p, VectorXf b);
-
-#endif //TRANSFORMGLOBAL_H
View
17 testcode/add_control_noise.cpp
@@ -1,17 +0,0 @@
-#include "add_control_noise.h"
-#include <iostream>
-
-using namespace std;
-
-void add_control_noise(float V, float G, Matrix2f Q, int addnoise, float* VnGn)
-{
- if (addnoise ==1) {
- VectorXf A(2);
- A(0) = V;
- A(1) = G;
- VectorXf C(2);
- C = multivariate_gauss(A,Q,1);
- VnGn[0] = C(0);
- VnGn[1] = C(1);
- }
-}
View
11 testcode/add_control_noise.h
@@ -1,11 +0,0 @@
-#ifndef ADD_CONTROL_NOISE
-#define ADD_CONTROL_NOISE
-
-#include <Eigen/Dense>
-#include "multivariate_gauss.h"
-
-using namespace Eigen;
-
-void add_control_noise(float V, float G, Matrix2f Q, int addnoise,float* VnGn);
-
-#endif //ADD_CONTROL_NOISE
View
122 testcode/add_feature.cpp
@@ -1,122 +0,0 @@
-#include "add_feature.h"
-#include <math.h>
-#include <vector>
-#include <iostream>
-
-using namespace std;
-
-//
-// add new features
-//
-void add_feature(Particle &particle, MatrixXf z, MatrixXf R)
-{
- cout<<"ADD_FEATURE"<<endl;
- cout<<"z"<<endl;
- cout<<z<<endl;
- cout<<"R"<<endl;
- cout<<R<<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;
- cout<<particle.xf()<<endl;
- cout<<"particle.Pf"<<endl;
- for (int i=0; i<(particle.Pf()).size();i++) {
- cout<<particle.Pf()[i]<<endl;
- }
-
- int lenz = z.cols();
- MatrixXf xf(2,lenz);
- vector<MatrixXf> Pf;
- VectorXf xv = particle.xv();
-
- float r,b,s,c;
- MatrixXf Gz(2,2);
-
- for (int i=0; i<lenz; i++) {
- r = z(0,i);
- b = z(1,i);
- s = sin(xv(2)+b);
- c = cos(xv(2)+b);
-
- xf(0,i) = xv(0) + r*c;
- xf(1,i) = xv(1) + r*s;
- Gz <<c,-r*s,s,r*c;
-
- Pf.push_back(Gz*R*Gz.transpose());
- }
-
- int lenx = particle.xf().cols();
- vector<int> ii;
- for (int i=lenx; i<lenx+lenz; i++) {
- ii.push_back(i);
- }
-
- //MatrixXf xfCopy;// = particle.xf();
- //vector<MatrixXf> pfCopy(particle.Pf());
-
- cout<<"xf"<<endl;
- cout<<xf<<endl;
-
-//TODO: there is a bug here. xfCopy (which is particle.xf) should grow in size
-//based on length of ii
-//new xfCopy columns = max(ii.size(),xfCopy.cols())
-//initialize to 0
-
-
- //stupid dynamically sized matlab matrices :((((
- int prows = particle.xf().rows();
- int pcols = particle.xf().cols();
- int iisize = ii.size();
-
- //max value in ii
- vector<int>::iterator maxelem;
- maxelem = std::max_element(ii.begin(),ii.end());
- int xfc = std::max((*maxelem)+1,pcols);
-
- cout<<"particle.xf().cols()"<<endl;
- cout<<pcols<<endl;
- cout<<"max"<<endl;
- cout<<xfc<<endl;
- MatrixXf xfCopy(prows,xfc);
- xfCopy.setZero();
- for (int i =0; i<prows; i++) {
- for (int j=0; j<pcols; j++) {
- xfCopy(i,j) =particle.xf()(i,j);
- }
- }
-
-
- //stupid matlab... I need to dynamically
- //pfCopy
- int pfcols = particle.Pf().size();
- int Pfc = std::max((*maxelem)+1,pfcols);
- vector<MatrixXf> pfCopy(Pfc);
- for (int i=0; i<pfcols; i++) {
- pfCopy[i] = particle.Pf()[i];
- }
-
- for (unsigned l=0; l<ii.size(); l++) {
- if (ii.size() == 0) {
- break;
- }
- if (xfCopy.isZero()) {
- xfCopy = xf;
- } else {
- for (unsigned k=0; k<xf.rows(); k++) {
- xfCopy(k,ii[l]) = xf(k,l);
- }
- }
- if (pfCopy.empty()) {
- pfCopy = Pf;
- } else {
- pfCopy[ii[l]] = Pf[l];
- }
- }
-
- particle.setXf(xfCopy);
- particle.setPf(pfCopy);
-}
View
13 testcode/add_feature.h
@@ -1,13 +0,0 @@
-#ifndef ADD_FEATURE_H
-#define ADD_FEATURE_H
-
-#include <Eigen/Dense>
-
-#include "particle.h"
-
-using namespace Eigen;
-
-void add_feature(Particle &particle, MatrixXf z, MatrixXf R);
-
-#endif //ADD_FEATURE_H
-
View
20 testcode/add_observation_noise.cpp
@@ -1,20 +0,0 @@
-#include "add_observation_noise.h"
-
-//add random measurement noise. We assume R is diagnoal matrix
-void add_observation_noise(MatrixXf &z, MatrixXf R, int addnoise)
-{
- if (addnoise == 1){
- int len = z.cols();
- if (len > 0) {
- MatrixXf randM1(1,len);
- MatrixXf randM2(1,len);
- randM1.setRandom();
- randM2.setRandom();
-
- for(int c=0; c<len; c++) {
- z(0,c) = z(0,c) + randM1(0,c)*sqrt(R(0,0));
- z(1,c) = z(1,c) + randM2(0,c)*sqrt(R(1,1));
- }
- }
- }
-}
View
11 testcode/add_observation_noise.h
@@ -1,11 +0,0 @@
-#ifndef ADD_OBSERVATION_NOISE_H
-#define ADD_OBSERVATION_NOISE_H
-
-#include <Eigen/Dense>
-#include <iostream>
-
-using namespace Eigen;
-
-void add_observation_noise(MatrixXf &z, MatrixXf R, int addnoise);
-
-#endif //ADD_OBSERVATION_NOISE_H
View
61 testcode/compute_jacobians.cpp
@@ -1,61 +0,0 @@
-#include <iostream>
-#include "compute_jacobians.h"
-#include "math.h"
-#include "pi_to_pi.h"
-
-void compute_jacobians(Particle particle,
- vector<int> idf,
- MatrixXf R,
- MatrixXf &zp,
- vector<MatrixXf> *Hv,
- vector<MatrixXf> *Hf,
- vector<MatrixXf> *Sf)
-{
- VectorXf xv = particle.xv();
-
- int rows = (particle.xf()).rows();
- MatrixXf xf(rows,idf.size());
- vector<MatrixXf> Pf;
-
- unsigned i;
- int r;
- for (unsigned i=0; i<idf.size(); i++) {
- for (r=0; r<(particle.xf()).rows(); r++) {
- xf(r,i) = (particle.xf())(r,(idf[i]));
- }
- if (particle.Pf().size() != 0) {
- Pf.push_back((particle.Pf())[idf[i]]); //particle.Pf is a array of matrices
- }
- }
-
- float dx,dy,d2,d;
- MatrixXf HvMat(2,3);
- MatrixXf HfMat (2,2);
-
- for (i=0; i<idf.size(); i++) {
- dx = xf(0,i) - xv(0);
- dy = xf(1,i) - xv(1);
- d2 = pow(dx,2) + pow(dy,2);
- d = sqrt(d2);
-
- //predicted observation
- zp(0,i) = d;
- zp(1,i) = atan2(dy,dx) - xv(2);
- zp(1,i) = pi_to_pi(zp(1,i));
-
- //Jacobian wrt vehicle states
- HvMat<< -dx/d, -dy/d, 0,
- dy/d2, -dx/d2,-1;
-
- //Jacobian wrt feature states
- HfMat<< dx/d, dy/d,
- -dy/d2, dx/d2;
-
- Hv->push_back(HvMat);
- Hf->push_back(HfMat);
-
- //innovation covariance of 'feature observation given the vehicle'
- MatrixXf SfMat = HfMat*Pf[i]*HfMat.transpose() + R;
- Sf->push_back(SfMat);
- }
-}
View
19 testcode/compute_jacobians.h
@@ -1,19 +0,0 @@
-#ifndef COMPUTE_JACOBIANS_H
-#define COMPUTE_JACOBIANS_H
-
-#include <Eigen/Dense>
-#include "particle.h"
-#include <vector>
-
-using namespace std;
-using namespace Eigen;
-
-void compute_jacobians(Particle particle,
- vector<int> idf,
- MatrixXf R,
- MatrixXf &zp,
- vector<MatrixXf> *Hv,
- vector<MatrixXf> *Hf,
- vector<MatrixXf> *Sf);
-
-#endif //COMPUTE_JACOBIANS_H
View
60 testcode/compute_steering.cpp
@@ -1,60 +0,0 @@
-#include "compute_steering.h"
-#include "pi_to_pi.h"
-
-#include <math.h>
-#include <vector>
-#include <iostream>
-
-using namespace std;
-
-void compute_steering(VectorXf x, MatrixXf wp, int& iwp, float minD,
- float& G, float rateG, float maxG, float dt)
-{
- /*
- % INPUTS:
- % x - true position
- % wp - waypoints
- % iwp - index to current waypoint
- % minD - minimum distance to current waypoint before switching to next
- % G - current steering angle
- % rateG - max steering rate (rad/s)
- % maxG - max steering angle (rad)
- % dt - timestep
- */
-
- //determine if current waypoint reached
- Vector2d cwp;
- cwp[0] = wp(0,iwp); //-1 since indexed from 0
- cwp[1] = wp(1,iwp);
-
- float d2 = pow((cwp[0] - x[0]),2) + pow((cwp[1]-x[1]),2);
-
- if (d2 < minD*minD) {
- iwp++; //switch to next
- if (iwp >= wp.cols()) {
- iwp =-1;
- return;
- }
-
- cwp[0] = wp(0,iwp); //-1 since indexed from 0
- cwp[1] = wp(1,iwp);
- }
-
- //compute change in G to point towards current waypoint
- float deltaG = atan2(cwp[1]-x[1], cwp[0]-x[0]) - x[2] - G;
- deltaG = pi_to_pi(deltaG);
-
- //limit rate
- float maxDelta = rateG*dt;
- if (abs(deltaG) > maxDelta) {
- int sign = (deltaG > 0) ? 1 : ((deltaG < 0) ? -1 : 0);
- deltaG = sign*maxDelta;
- }
-
- //limit angle
- G = G+deltaG;
- if (abs(G) > maxG) {
- int sign2 = (G > 0) ? 1: ((G < 0) ? -1 : 0);
- G = sign2*maxG;
- }
-}
View
11 testcode/compute_steering.h
@@ -1,11 +0,0 @@
-#ifndef COMPUTE_STEERING_H
-#define COMPUTE_STEERING_H
-
-#include <Eigen/Dense>
-
-using namespace Eigen;
-
-void compute_steering(VectorXf x, MatrixXf wp, int& iwp, float minD,
- float& G, float rateG, float maxG, float dt);
-
-#endif //COMPUTE_STEERING_H
View
50 testcode/configfile.cpp
@@ -1,50 +0,0 @@
-#include "configfile.h"
-
-#define pi 3.14159265
-
-// Configuration file
-//Permits various adjustments to parameters of the FastSLAM algorithm.
-// See fastslam_sim.h for more information
-
-// control parameters
-float config::V= 3.0; // m/s
-float config::MAXG= 30*pi/180; // radians, maximum steering angle (-MAXG < g < MAXG)
-float config::RATEG= 20*pi/180; // rad/s, maximum rate of change in steer angle
-int config::WHEELBASE= 4; // metres, vehicle wheel-base
-float config::DT_CONTROLS= 0.025; // seconds, time interval between control signals
-
-// control noises
-float config::sigmaV= 0.3; // m/s
-float config::sigmaG= (3.0*pi/180); // radians
-
-Eigen::MatrixXf config::Q(2,2);
-
-// observation parameters
-float config::MAX_RANGE= 30.0; // metres
-float config::DT_OBSERVE= 8* config::DT_CONTROLS; // seconds, time interval between observations
-
-// observation noises
-float config::sigmaR= 0.1; // metres
-float config::sigmaB= (1.0*pi/180); // radians
-
-Eigen::MatrixXf config::R(2,2);
-
-// waypoint proximity
-float config::AT_WAYPOINT= 1.0; // metres, distance from current waypoint at which to switch to next waypoint
-int config::NUMBER_LOOPS= 2; // number of loops through the waypoint list
-
-// resampling
-unsigned int config::NPARTICLES= 100;
-float config::NEFFECTIVE= 0.75* config::NPARTICLES; // minimum number of effective particles before resampling
-
-// switches
-int config::SWITCH_CONTROL_NOISE= 1;
-int config::SWITCH_SENSOR_NOISE = 1;
-int config::SWITCH_INFLATE_NOISE= 0;
-int config::SWITCH_PREDICT_NOISE = 0; // sample noise from predict (usually 1 for fastslam1.0 and 0 for fastslam2.0)
-int config::SWITCH_SAMPLE_PROPOSAL = 1; // sample from proposal (no effect on fastslam1.0 and usually 1 for fastslam2.0)
-int config::SWITCH_HEADING_KNOWN= 0;
-int config::SWITCH_RESAMPLE= 1;
-int config::SWITCH_PROFILE= 1;
-int config::SWITCH_SEED_RANDOM= 0; // if not 0, seed the randn() with its value at beginning of simulation (for repeatability)
-
View
47 testcode/configfile.h
@@ -1,47 +0,0 @@
-#ifndef CONFIGFILE_H
-#define CONFIGFILE_H
-
-#include <Eigen/Dense>
-
-//******************
-// Global Variables
-//******************
-
-namespace config {
- extern float V;
- extern float MAXG;
- extern float RATEG;
- extern int WHEELBASE;
- extern float DT_CONTROLS;
-
- extern float sigmaV;
- extern float sigmaG;
-
- extern Eigen::MatrixXf Q;
-
- extern float MAX_RANGE;
- extern float DT_OBSERVE;
-
- extern float sigmaR;
- extern float sigmaB;
-
- extern Eigen::MatrixXf R;
-
- extern float AT_WAYPOINT;
- extern int NUMBER_LOOPS;
-
- extern unsigned int NPARTICLES;
- extern float NEFFECTIVE;
-
- extern int SWITCH_CONTROL_NOISE;
- extern int SWITCH_SENSOR_NOISE;
- extern int SWITCH_INFLATE_NOISE;
- extern int SWITCH_PREDICT_NOISE;
-
- extern int SWITCH_SAMPLE_PROPOSAL;
- extern int SWITCH_HEADING_KNOWN;
- extern int SWITCH_RESAMPLE;
- extern int SWITCH_PROFILE;
- extern int SWITCH_SEED_RANDOM;
-}
-#endif //CONFIGFILE_H
View
56 testcode/data_associate_known.cpp
@@ -1,56 +0,0 @@
-#include "data_associate_known.h"
-#include <iostream>
-
-void data_associate_known(MatrixXf z, vector<int> idz, VectorXf &table, int Nf, \
- MatrixXf &zf, vector<int> &idf, MatrixXf &zn)
-{
- idf.clear();
- vector<int> idn;
-
- unsigned i,ii,r;
-
- //this extra loopis required to set the dimension of zn and zf
- int zncols = 0;
- int zfcols = 0;
- for (i =0; i< idz.size(); i++){
- ii = idz[i];
- if (table(ii) ==-1) { //new feature
- zncols++;
- }
- else {
- zfcols++;
- }
- }
-
- //resize
- zn.resize(z.rows(),zncols);
- zn.setZero();
- zf.resize(z.rows(),zfcols);
- zf.setZero();
-
- int znc,zfc;
- znc =0;
- zfc =0;
- for (i =0; i< idz.size(); i++){
- ii = idz[i];
- if (table(ii) ==-1) { //new feature
- for (r=0; r<z.rows();r++) {
- zn(r,znc) = z(r,i);
- }
- znc++;
- idn.push_back(ii);
- }
- else {
- for (r=0; r<z.rows(); r++) {
- zf(r,zfc) = z(r,i);
- }
- zfc++;
- idf.push_back(table(ii));
- }
- }
-
- for (int i=0; i<idn.size(); i++) {
- table(idn[i]) = Nf+i;
- }
-}
-
View
13 testcode/data_associate_known.h
@@ -1,13 +0,0 @@
-#ifndef DATA_ASSOCIATE_KNOWN_H
-#define DATA_ASSOCIATE_KNOWN_H
-
-#include <Eigen/Dense>
-#include <vector>
-
-using namespace std;
-using namespace Eigen;
-
-void data_associate_known(MatrixXf z, vector<int> idz, VectorXf &table, int Nf, \
- MatrixXf &zf, vector<int> &idf, MatrixXf &zn);
-
-#endif //DATA_ASSOCIATE_KNOWN_H
View
56 testcode/example_webmap.mat
@@ -1,56 +0,0 @@
-#type columns rows
-lm 2 35
-2.9922 -25.7009
-32.8988 -33.1776
-24.7991 -68.3801
-75.2664 -65.5763
-73.7087 -35.6698
-98.6308 3.8941
-49.4097 26.9470
-80.2508 59.9688
-54.7056 88.6293
-13.2726 80.8411
--16.3224 49.0654
--65.8551 83.6449
--96.3847 60.5919
--76.1355 36.2928
--87.3505 -21.3396
--103.5498 -32.2430
--92.9579 -77.7259
--55.8863 -55.9190
--35.3255 -19.1589
--56.1978 16.3551
--7.2882 19.7819
-25.7336 3.2710
--19.5610 80.1444
--41.7506 46.2325
-25.4021 26.8543
-91.3870 28.2385
--13.7216 -79.0338
--52.8454 -92.1833
--86.1298 15.0890
--127.0053 22.7018
--25.9843 4.7078
-56.3508 -17.4388
-51.6793 -83.1863
-21.3146 -96.3358
-48.1757 57.9979
-
-wp 2 17
-12.6495 -41.5888
-44.7368 -54.9844
-85.5467 -45.0156
-93.6464 -17.2897
-64.9860 5.7632
-71.8396 31.6199
-71.2165 70.5607
-33.5218 76.4798
-12.9611 51.5576
--32.5218 67.4455
--74.8894 69.0031
--97.3193 41.9003
--107.5997 6.3863
--86.4159 -25.7009
--83.9237 -64.0187
--39.3754 -81.4642
--17.5685 -51.5576
View
277 testcode/fastslam2_sim.cpp
@@ -1,277 +0,0 @@
-#include <iostream>
-#include <math.h>
-#include <vector>
-
-#include "fastslam2_sim.h"
-#include "add_control_noise.h"
-#include "predict.h"
-#include "observe_heading.h"
-#include "get_observations.h"
-#include "add_observation_noise.h"
-#include "TransformToGlobal.h"
-#include "line_plot_conversion.h"
-#include "data_associate_known.h"
-#include "sample_proposal.h"
-#include "feature_update.h"
-#include "resample_particles.h"
-#include "add_feature.h"
-
-using namespace config;
-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");
- }
- if (SWITCH_SAMPLE_PROPOSAL == 0) {
- printf("Sampling from optimal proposal is usually ON for FastSLAM 2.0\n");
- }
- //normally initialized configfile.h
- Q << pow(sigmaV,2), 0,
- 0 , pow(sigmaG,2);
-
- R << sigmaR*sigmaR, 0,
- 0, sigmaB*sigmaB;
-
- float veh[2][3] = {{0,-WHEELBASE,-WHEELBASE},{0,-1,1}};
-
- //vector of particles (their count will change)
- vector<Particle> particles(NPARTICLES);
- for (int i=0; i<particles.size(); i++){
- 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);
- }
-
- VectorXf xtrue(3);
- xtrue.setZero();
-
- float dt = DT_CONTROLS; //change in time btw predicts
- float dtsum = 0; //change in time since last observation
-
- vector<int> ftag; //identifier for each landmark
- for (int i=0; i< lm.cols(); i++) {
- ftag.push_back(i);
- }
-
- //data ssociation table
- VectorXf da_table(lm.cols());
- for (int s=0; s<da_table.size(); s++) {
- 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);
- }
-
- MatrixXf Qe = MatrixXf(Q);
- MatrixXf Re = MatrixXf(R);
-
- if (SWITCH_INFLATE_NOISE ==1) {
- Qe = 2*Q;
- Re = 2*R;
- }
-
- if (SWITCH_PROFILE) {
- //TODO:
- }
-
- vector<int> ftag_visible;
- MatrixXf z;
- int count =0;
-
-
- //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
- //TODO: need to truly randomize function in multivariate_gauss
- float* VnGn = new float[2];
- //add_control_noise(V,G,Q,SWITCH_CONTROL_NOISE,VnGn);
- float Vn = V;//VnGn[0];
- float Gn = G;//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 = get_observations(xtrue,lm,ftag_visible,MAX_RANGE);
- //TODO
- //add_observation_noise(z,R,SWITCH_SENSOR_NOISE);
- if(!z.isZero()){
- plines = make_laser_lines(z,xtrue);
- }
-
- //Compute (known) data associations
- int Nf = (particles[0].xf()).cols();
- vector<int> idf;
- if (ftag_visible.size() ==2) {
- cout<<"INPUT GET OBSERVE"<<endl;
- cout<<"xtrue"<<endl;
- cout<<xtrue<<endl;
- cout<<"lm"<<endl;
- cout<<lm<<endl;
- cout<<"ftag_visible"<<endl;
- for (int i=0; i<ftag.size(); i++) {
- cout<<ftag[i]<<" ";
- }
- cout<<endl;
- cout<<"max range: "<<MAX_RANGE<<endl;
- cout<<"OUTPUT GET_OBSERVE"<<endl;
- cout<<"ftag_visible"<<endl;
- for (int i=0; i<ftag_visible.size(); i++) {
- cout<<ftag_visible[i]<<" ";
- }
- cout<<"z"<<endl;
- cout<<z<<endl;
- cout<<endl;
- }
-
- MatrixXf zf(z.rows(),ftag_visible.size());
- zf.setZero();
- MatrixXf zn(z.rows(),ftag_visible.size());
- zn.setZero();
-
- bool testflag= false;
- if(ftag_visible.size() ==2) {
- testflag = true;
- cout<<"INPUT TO DATA_ASSOC"<<endl;
- cout<<"z"<<endl;
- cout<<z<<endl;
- cout<<"ftag_visible"<<endl;
- for (int i=0; i<ftag_visible.size(); i++) {
- cout<<ftag_visible[i]<<" ";
- }
- cout<<endl;
- cout<<"da_table"<<endl;
- for (int i=0; i<da_table.size(); i++) {
- cout<<da_table[i]<<" ";
- }
- cout<<endl;
- cout<<"Nf"<<endl;
- cout<<Nf<<endl;
- cout<<"zf"<<endl;
- cout<<zf<<endl;
- for (int i=0; i<idf.size(); i++) {
- cout<<idf[i]<<" ";
- }
- cout<<endl;
- cout<<"zn"<<endl;
- cout<<zn<<endl;
- cout<<"idf"<<endl;
- for (int i=0; i<idf.size(); i++) {
- cout<<idf[i]<<" ";
- }
- }
- data_associate_known(z,ftag_visible,da_table,Nf,zf,idf,zn);
- if(testflag==true) {
- cout<<"OUTPUT DATA_ASSOC"<<endl;
- cout<<"da_table"<<endl;
- for (int i=0; i<da_table.size(); i++) {
- cout<<da_table[i]<<" ";
- }
- cout<<endl;
- cout<<"zf"<<endl;
- cout<<zf<<endl;
- for (int i=0; i<idf.size(); i++) {
- cout<<idf[i]<<" ";
- }
- cout<<endl;
- cout<<"zn"<<endl;
- cout<<zn<<endl;
- cout<<"idf"<<endl;
- for (int i=0; i<idf.size(); i++) {
- cout<<idf[i]<<" ";
- }
- cout<<endl;
- }
- //observe map features
- if (!zf.isZero()) {
- //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
- //TODO: xv gets depreicated here
- if (!zn.isZero()) {
- for (unsigned i=0; i<NPARTICLES; i++) {
- if (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);
- //TODO: xv[0] seems to have an approximation error from chol.
- particles[i].setXv(xv);
- MatrixXf pv(3,3);
- pv.setZero();
- particles[i].setPv(pv);
- }
- add_feature(particles[i], zn, Re);
- }
- }
- count++;
- if (VnGn) {
- delete[] VnGn;
- }
- }
- }
- cout<<"done with all functions and will return particles"<<endl<<flush;
- return particles;
-}
-
-MatrixXf make_laser_lines(MatrixXf rb, VectorXf xv)
-{
- if (rb.isZero()) {
- return MatrixXf(0,0);
- }
-
- int len = rb.cols();
- MatrixXf lnes(4,len);
-
- MatrixXf globalMat(2,rb.cols());
- int j;
- for (j=0; j<globalMat.cols();j++) {
- globalMat(0,j) = rb(0,j)*cos(rb(1,j));
- globalMat(1,j) = rb(0,j)*sin(rb(1,j));
- }
-
- TransformToGlobal(globalMat,xv);
-
- for (int c=0; c<lnes.cols();c++) {
- lnes(0,c) = xv(0);
- lnes(1,c) = xv(1);
- lnes(2,c) = globalMat(0,c);
- lnes(3,c) = globalMat(1,c);
- }
-
- return line_plot_conversion(lnes);
-}
View
22 testcode/fastslam2_sim.h
@@ -1,22 +0,0 @@
-#ifndef FASTSLAM2_SIM_H
-#define FASTSLAM2_SIM_H
-
-#include <iostream>
-#include <fstream>
-#include <sstream>
-#include <stdlib.h>
-#include <vector>
-#include <Eigen/Dense>
-
-#include "configfile.h"
-#include "compute_steering.h"
-#include "predict_true.h"
-#include "particle.h"
-
-using namespace std;
-using namespace Eigen;
-
-vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp);
-MatrixXf make_laser_lines(MatrixXf rb, VectorXf xv);
-
-#endif //FASTSLAM2_SIM_H
View
76 testcode/feature_update.cpp
@@ -1,76 +0,0 @@
-#include "feature_update.h"
-#include <iostream>
-
-using namespace std;
-
-void feature_update(Particle &particle, MatrixXf z, vector<int>idf, MatrixXf R)
-{
- //Having selected a new pose from the proposal distribution, this pose is assumed perfect and each feature update maybe computed independently and without pose uncertainty
-
- int rows = (particle.xf()).rows();
- MatrixXf xf(rows,idf.size());
- vector<MatrixXf> Pf;
-
- 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.push_back((particle.Pf())[idf[i]]); //particle.Pf is a array of matrices
- }
-
- MatrixXf zp(z.rows(), idf.size());
- zp.setZero();
- vector<MatrixXf> Hv;
- vector<MatrixXf> Hf;
- vector<MatrixXf> Sf;
- compute_jacobians(particle,idf,R,zp,&Hv,&Hf,&Sf);
-
- #if 0
- cout<<"after compute_jacobians"<<endl;
- cout<<endl;
- cout<<"in feature_update"<<endl;
- cout<<"zp is "<<endl;
- cout<<zp<<endl;
- cout<<"zp should be"<<endl;
- cout<<"25.6675 24.5294"<<endl;
- cout<<"-1.4925 0.1547"<<endl;
- cout<<endl;
- #endif
-
- cout<<"z"<<endl;
- cout<<z.rows()<<" "<<z.cols()<<endl;
- cout<<"zp"<<endl;
- cout<<zp.rows()<<" "<<zp.cols()<<endl;
- MatrixXf v = z-zp; //TODO: need to fix: idf.size() is zero. which makes this break.
-
- unsigned c;
- for (c=0; c<v.cols();c++) {
- v(1,c) = pi_to_pi(v(1,c));
- }
-
- VectorXf vi(v.rows());
- MatrixXf Hfi;
- MatrixXf Pfi;
- VectorXf xfi(xf.rows());
-
- for (i=0; i< idf.size(); i++) {
- for (r=0; r<v.rows(); r++) {
- vi(r) = v(r,i); //v.column(i);
- }
- Hfi = Hf[i];
- Pfi = Pf[i];
- for (r=0; r<xf.rows(); r++) {
- xfi(r) = xf(r,i); //xfi = xf.column(i);
- }
- KF_cholesky_update(xfi,Pfi,vi,R,Hfi);
-
- for (r=0; r<xf.rows(); r++) {
- xf(r,i) = xfi(r);
- }
- Pf[i] = Pfi;
- }
-
- particle.setXf(xf);
- particle.setPf(Pf);
-}
View
17 testcode/feature_update.h
@@ -1,17 +0,0 @@
-#ifndef FEATURE_UPDATE_H
-#define FEATURE_UPDATE_H
-
-#include <Eigen/Dense>
-#include <vector>
-
-#include "particle.h"
-#include "compute_jacobians.h"
-#include "pi_to_pi.h"
-#include "KF_cholesky_update.h"
-
-using namespace Eigen;
-using namespace std;
-
-void feature_update(Particle &particle, MatrixXf z, vector<int>idf, MatrixXf R);
-
-#endif
View
79 testcode/gauss_evaluate.cpp
@@ -1,79 +0,0 @@
-#include "gauss_evaluate.h"
-#include <algorithm>
-#include <iostream>
-
-#define pi 3.14159
-
-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;
- #endif
-
- int D = v.size();
- MatrixXf Sc = S.llt().matrixL();
- //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;
- #endif
-
- int s;
- //VectorXf E(nin.cols());
- float E=0;
- for (s=0; s<nin.size(); s++) {
- nin(s) = pow(nin(s),2);
- E += nin(s);
- }
- E=-0.5*E;
- //Note: above is a fast way to compute sets of inner-products
-
- unsigned i;
- float C,w;
- unsigned m = min(Sc.rows(), Sc.cols());
-
- if (logflag !=1) {
- float prod = 1;
- 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
- //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
26 testcode/gauss_evaluate.h
@@ -1,26 +0,0 @@
-#ifndef GAUSS_EVALUATE_H
-#define GAUSS_EVALUATE_H
-
-#include <Eigen/Dense>
-
-using namespace Eigen;
-
-float gauss_evaluate(VectorXf v, MatrixXf S, int logflag);
-
-/*
-**INPUTS:
-** v - a set of innovation vectors
-** S - the covariance matrix for the innovations
-** logflat - <optional> - if 1 computes the log-likelihood, otherwise computes
-** the likelihood.
-**
-**OUTPUT:
-** w- set of Gaussian likelihoods or log-likelihoods for ech v(:i).
-**
-**This implementaiton uses the Cholesky factor of S to compute the likelihoods
-**and so is more numerically stable than a simple full covariance form.
-**This function is identical to gauss_likelihood().
-*/
-
-#endif
-
View
88 testcode/get_observations.cpp
@@ -1,88 +0,0 @@
-#include "get_observations.h"
-#include <iostream>
-#include <math.h>
-
-MatrixXf get_observations(VectorXf x, MatrixXf lm, vector<int> &idf, float rmax)
-{
- get_visible_landmarks(x,lm,idf,rmax);
- #if 0
- cout<<"lm: "<<lm<<endl;
- cout<<"lm should be [2.9922,25.7336;-25.7009, 3.27100]"<<endl;
- cout<<"x: "<<x<<endl;
- cout<<"x should be [0.6741;-0.0309;-0.0074]"<<endl;
- cout<<"compute_range_bearing returns: "<<endl;
- MatrixXf temp = compute_range_bearing(x,lm);
- cout<<temp<<endl;
- cout<<"compute range beearing should be [25.7745,25.2762;-1.4734,0.1384]"<<endl;
- #endif
- return compute_range_bearing(x,lm);
-}
-
-void get_visible_landmarks(VectorXf x, MatrixXf &lm, vector<int> &idf, float rmax)
-{
- //select set of landmarks that are visible within vehicle's
- //semi-circular field of view
- MatrixXf dx(1,lm.cols());
- MatrixXf dy(1,lm.cols());
- for (int c=0; c<lm.cols(); c++) {
- dx(0,c) = lm(0,c) - x(0);
- dy(0,c) = lm(1,c) - x(1);
- }
-
- float phi = x(2);
- vector<int> ii = find2(dx,dy,phi,rmax);
-
- //lm(:[2 3]) return matrix of column 2 and colum 3 of lm
-
- MatrixXf lm_new (lm.rows(), ii.size());
- vector<int>::iterator iter;
-
- unsigned j,k;
- for (j=0; j<lm.rows(); j++){
- for(k=0; k< ii.size(); k++){
- lm_new(j,k) = lm(j,ii[k]);
- }
- }
- lm = MatrixXf(lm_new);
- //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)
-{
- MatrixXf dx(1,lm.cols());
- MatrixXf dy(1,lm.cols());
- for (int c=0; c<lm.cols(); c++) {
- dx(0,c) = lm(0,c) - x(0);
- dy(0,c) = lm(1,c) - x(1);
- }
-
- float phi = x(2);
- MatrixXf z(2,lm.cols());
- for (int i =0; i<lm.cols(); i++) {
- z(0,i) = sqrt(pow(dx(0,i),2) + pow(dy(0,i),2));
- z(1,i) = atan2(dy(0,i),dx(0,i)) - phi;
- }
-
- return z;
-}
-
-vector<int> find2(MatrixXf dx, MatrixXf dy, float phi, float rmax)
-{
- vector<int> index;
- //incremental tests for bounding semi-circle
- 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
14 testcode/get_observations.h
@@ -1,14 +0,0 @@
-#ifndef GET_OBSERVATIONS_H
-#define GET_OBSERVATIONS_H
-
-#include <Eigen/Dense>
-#include <vector>
-
-using namespace Eigen;
-using namespace std;
-
-MatrixXf get_observations(VectorXf x,MatrixXf lm,vector<int> &idf,float rmax);
-void get_visible_landmarks(VectorXf x, MatrixXf &lm,vector<int> &idf, float rmax);
-MatrixXf compute_range_bearing(VectorXf x, MatrixXf lm);
-vector<int> find2(MatrixXf dx, MatrixXf dy, float phi, float rmax);
-#endif
View
36 testcode/line_plot_conversion.cpp
@@ -1,36 +0,0 @@
-#include "line_plot_conversion.h"
-#include <iostream>
-
-using namespace std;
-
-MatrixXf line_plot_conversion(MatrixXf lnes)
-{
- int len = lnes.cols()*3 -1;
- MatrixXf p(2,len);
-
-#if 0
- int i;
- p(0,0) = lne(0,0);
- p(1,0) = lne(1,0);
- p(0,1) = lne(2,0);
- p(1,1) = lne(3,0);
-
- p(0,3) = lne(0,1);
- p(1,3) = lne(1,1);
- p(0,4) = lne(2,1);
- p(1,4) = lne(3,1);
-#endif
-
- for (int j=0; j<len; j+=3) {
- int k = floor((j+1)/3); //reverse of len calculation
- p(0,j) = lnes(0,k);
- p(1,j) = lnes(1,k);
- p(0,j+1) = lnes(2,k);
- p(1,j+1) = lnes(3,k);
- if (j+2 <len) {
- p(0,j+2) = NULL;
- p(1,j+2) = NULL;
- }
- }
- return p;
-}
View
10 testcode/line_plot_conversion.h
@@ -1,10 +0,0 @@
-#ifndef LINE_PLOT_CONVERSION_H
-#define LINE_PLOT_CONVERSION_H
-
-#include <Eigen/Dense>
-
-using namespace Eigen;
-
-MatrixXf line_plot_conversion(MatrixXf lnes);
-
-#endif //LINE_PLOT_CONVERSION_H
View
136 testcode/main.cpp
@@ -1,136 +0,0 @@
-#include <stdio.h>
-#include <algorithm>
-#include <iterator>
-#include <unistd.h>
-#include <errno.h>
-#include <string>
-#include <vector>
-#include <Eigen/Dense>
-
-#include "fastslam2_sim.h"
-#include "particle.h"
-
-using namespace Eigen;
-using namespace std;
-
-void read_input_file(const string s, MatrixXf *lm, MatrixXf *wp)
-{
- using std::ifstream;
- using std::istringstream;
-
- if(access(s.c_str(),R_OK) == -1) {
- std::cerr << "Unable to read input file" << s << std::endl;
- exit(EXIT_FAILURE);
- }
- ifstream in(s.c_str());
-
- int lineno = 0;
- int lm_rows =0;
- int lm_cols =0;
- int wp_rows =0;
- int wp_cols =0;
-
- while(in) {
- lineno++;
- string str;
- getline(in,str);
- istringstream line(str);
-
- vector<string> tokens;
- copy(istream_iterator<string>(line),
- istream_iterator<string>(),
- back_inserter<vector<string> > (tokens));
-
- if(tokens.size() ==0) {
- continue;
- }
- else if (tokens[0][0] =='#') {
- continue;
- }
- else if (tokens[0] == "lm") {
- if(tokens.size() != 3) {
- std::cerr<<"Wrong args for lm!"<<std::endl;
- std::cerr<<"Error occuredon line"<<lineno<<std::endl;
- std::cerr<<"line:"<<str<<std::endl;
- exit(EXIT_FAILURE);
- }
- lm_rows = strtof(tokens[1].c_str(),NULL);
- lm_cols = strtof(tokens[2].c_str(),NULL);
-
- lm->resize(lm_rows,lm_cols);
- for (int c =0; c<lm_cols; c++) {
- lineno++;
- if (!in) {
- std::cerr<<"EOF after reading" << std::endl;
- exit(EXIT_FAILURE);
- }
- getline(in,str);
- istringstream line(str);
- vector<string> tokens;
- copy(istream_iterator<string>(line),
- istream_iterator<string>(),
- back_inserter<vector<string> > (tokens));
- if(tokens.size() < lm_rows) {
- std::cerr<<"invalid line for lm coordinate!"<<std::endl;
- std::cerr<<"Error occured on line "<<lineno<<std::endl;
- std::cerr<<"line: "<<str<<std::endl;
- exit(EXIT_FAILURE);
- }
-
- for (unsigned r=0; r< lm_rows; r++) {
- (*lm)(r,c) = strtof(tokens[r].c_str(),NULL);
- }
- }
- }
- else if (tokens[0] == "wp") {
- if(tokens.size() != 3) {
- std::cerr<<"Wrong args for wp!"<<std::endl;
- std::cerr<<"Error occured on line"<<lineno<<std::endl;
- std::cerr<<"line:"<<str<<std::endl;
- exit(EXIT_FAILURE);
- }
- wp_rows = strtof(tokens[1].c_str(),NULL);
- wp_cols = strtof(tokens[2].c_str(),NULL);
- wp->resize(wp_rows, wp_cols);
- for (int c =0; c<wp_cols; c++) {
- lineno++;
- if (!in) {
- std::cerr<<"EOF after reading" << std::endl;
- exit(EXIT_FAILURE);
- }
- getline(in,str);
- istringstream line(str);
- std::vector<string> tokens;
- copy(istream_iterator<string>(line),
- istream_iterator<string>(),
- back_inserter<std::vector<string> > (tokens));
- if(tokens.size() < wp_rows) {
- std::cerr<<"invalid line for wp coordinate!"<<std::endl;
- std::cerr<<"Error occured on line "<<lineno<<std::endl;
- std::cerr<<"line: "<<str<<std::endl;
- exit(EXIT_FAILURE);
- }
-
- for (int r=0; r< lm_rows; r++) {
- (*wp)(r,c) = strtof(tokens[r].c_str(),NULL);
- }
- }
- }
- else {
- std::cerr<<"Unkwown command"<<tokens[0] <<std::endl;
- std::cerr<<"Error occured on line"<<lineno<<std::endl;
- std::cerr<<"line: "<<str<<std::endl;
- exit(EXIT_FAILURE);
- }
- }
-}
-
-
-int main (int argc, char *argv[])
-{
- MatrixXf lm;
- MatrixXf wp;
-
- read_input_file("example_webmap.mat", &lm, &wp);
- vector<Particle> data = fastslam2_sim(lm,wp);
-}
View
56 testcode/makefile
@@ -1,56 +0,0 @@
-#Code -------------------------------
-
-### YOU CAN CHANGE THESE VARIABLES AS YOU ADD CODE:
-TARGET := fastslam
-SOURCES := $(wildcard ./*.cpp)
-
-#Libraries -------------------------------
-
-#Check for OSX. Works on 10.5 (and 10.4 but untested)
-#This line still kicks out an annoying error on Solaris.
-ifeq ($(shell sw_vers 2>/dev/null | grep Mac | awk '{ print $$2}'),Mac)
- #Assume Mac
- INCLUDE := -I./include/ -I/usr/X11/include -I/opt/local/include -I/opt/local/include/eigen3
- LIBRARY := -L"/System/Library/Frameworks/OpenGL.framework/Libraries" \
- -lm -lstdc++
- FRAMEWORK :=
- MACROS := -DOSX
- PLATFORM := Mac
-else
- #Assume X11
- 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
- FRAMEWORK :=
- MACROS :=
- PLATFORM := *Nix
-endif
-
-#Basic Stuff -----------------------------
-
-CC := gcc
-CXX := g++
-CXXFLAGS := -g -Wall -O0 -fmessage-length=0 -DGL_GLEXT_PROTOTYPES $(INCLUDE) $(MACROS)
-LDFLAGS := $(FRAMEWORK) $(LIBRARY)
-#-----------------------------------------
-%.o : %.cpp
- @echo "Compiling .cpp to .o for $(PLATFORM) Platform: $<"
- @$(CXX) -c -Wall $(CXXFLAGS) $< -o $@
-
-OBJECTS = $(SOURCES:.cpp=.o)
-
-$(TARGET): $(OBJECTS)
- @echo "Linking .o files into: $(TARGET)"
- @$(CXX) $(LDFLAGS) $(OBJECTS) -o $(TARGET)
-
-default: $(TARGET)
-
-all: default
-
-clean:
- @echo "Removing compiled files: $<"
- /bin/rm -f $(OBJECTS) $(TARGET)
-
-
-
View
20 testcode/multivariate_gauss.cpp
@@ -1,20 +0,0 @@
-#include "multivariate_gauss.h"
-#include <Eigen/Cholesky>
-#include <iostream>
-
-using namespace std;
-
-VectorXf multivariate_gauss(VectorXf x, MatrixXf P, int n)
-{
- int len = x.size();
- //choleksy decomposition
- MatrixXf S = P.llt().matrixL();
- 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
11 testcode/multivariate_gauss.h
@@ -1,11 +0,0 @@
-#ifndef MULTIVARIATE_GAUSS_H
-#define MULTIVARIATE_GAUSS_H
-
-#include <Eigen/Dense>
-
-using namespace Eigen;
-
-VectorXf multivariate_gauss(VectorXf x, MatrixXf P, int n);
-
-#endif //MULTIVARIATE_GAUSS_H
-
View
39 testcode/observe_heading.cpp
@@ -1,39 +0,0 @@
-#include "observe_heading.h"
-#include "pi_to_pi.h"
-#include "KF_joseph_update.h"
-
-#include <iostream>
-#include <vector>
-
-//observe_heading doesn't get called unless I change SWITCH_HEADING KNOWN
-
-using namespace std;
-
-void observe_heading(Particle &particle, float phi, int useheading)
-{
- if (useheading ==0){
- return;
- }
- float sigmaPhi = 0.01*pi/180.0;
- cout<<"sigmaPhi "<<sigmaPhi<<endl;
- VectorXf xv = particle.xv();
- cout<<"xv"<<endl;
- cout<<xv<<endl;
- MatrixXf Pv = particle.Pv();
- cout<<"Pv"<<endl;
- cout<<Pv<<endl;
- MatrixXf H(1,3);
- H<<0,0,1;
-
- float v = pi_to_pi(phi-xv(2));
- cout<<"v"<<endl;
- cout<<v<<endl;
- KF_joseph_update(xv,Pv,v,pow(sigmaPhi,2),H);
- cout<<"KF_Joseph = xv"<<endl;
- cout<<xv<<endl;
- cout<<"KF_Joseph = Pv"<<endl;
- cout<<Pv<<endl;
-
- particle.setXv(xv);
- particle.setPv(Pv);
-}
View
9 testcode/observe_heading.h
@@ -1,9 +0,0 @@
-#ifndef OBSERVE_HEADING_H
-#define OBSERVE_HEADING_H
-
-#include<Eigen/Dense>
-#include "particle.h"
-
-void observe_heading(Particle &particle, float phi, int useheading);
-
-#endif
View
26,003 testcode/output_old
0 additions, 26,003 deletions not shown
View
101 testcode/particle.cpp
@@ -1,101 +0,0 @@
-#include <iostream>
-#include "particle.h"
-#include <algorithm>
-
-/*************
-** Particle
-*************/
-
-Particle::Particle()
-{
- _w = 1.0;
- _xv = VectorXf(3);
- _xv.setZero();
- _Pv = MatrixXf(3,3);
- _Pv.setZero();
- _xf = MatrixXf(0,0);
- _da = NULL;
-}
-
-Particle::Particle(float w, VectorXf xv, MatrixXf Pv, MatrixXf xf, vector<MatrixXf> Pf, float* da)
-{
- _w = w;
- _xv = xv;
- _Pv = Pv;
- _xf = xf;
- _Pf = vector<MatrixXf>(Pf);
- _da = da;
-}
-
-Particle::~Particle()
-{
-}
-
-//getters
-float Particle::w() const
-{
- return _w;
-}
-
-VectorXf Particle::xv() const
-{
- return _xv;
-}
-
-MatrixXf Particle::Pv() const
-{
- return _Pv;
-}
-
-MatrixXf Particle::xf() const
-{
- return _xf;
-}
-
-vector<MatrixXf> Particle::Pf() const
-{
- return _Pf;
-}
-
-float* Particle::da() const
-{
- return _da;
-}
-
-//setters
-void Particle::setW(float w)
-{
- _w = w;
-}
-
-void Particle::setXv(VectorXf xv)
-{
- for (int s=0; s<xv.size(); s++) {
- _xv(s) = xv(s); //copy over the values
- }
-}
-
-void Particle::setPv(MatrixXf Pv)
-{
- for (int r=0; r<Pv.rows(); r++){
- for (int j=0; j<Pv.cols(); j++){
- _Pv(r,j) = Pv(r,j); //copy over the values
- }
- }
-}
-
-void Particle::setXf(MatrixXf xf)
-{
- _xf = xf;
-}
-
-void Particle::setPf(vector<MatrixXf> Pf)
-{
- _Pf = vector<MatrixXf>(Pf);
-}
-
-void Particle::setDa(float* da)
-{
- _da = da;
-}
-
View
41 testcode/particle.h
@@ -1,41 +0,0 @@
-#ifndef PARTICLES_H
-#define PARTICLES_H
-
-#include <Eigen/Dense>
-#include <vector>
-
-using namespace Eigen;
-using namespace std;
-
-class Particle{
-public:
- Particle();
- Particle(float w, VectorXf xv, MatrixXf Pv, MatrixXf xf, vector<MatrixXf> Pf, float* da);
- ~Particle();
-
- //getters
- float w() const;
- VectorXf xv() const;
- MatrixXf Pv() const;
- MatrixXf xf() const;
- vector<MatrixXf> Pf() const;
- float* da() const;
-
- //setters
- void setW(float w);
- void setXv(VectorXf xv);
- void setPv(MatrixXf Pv);
- void setXf(MatrixXf xf);
- void setPf(vector<MatrixXf> Pf);
- void setDa(float* da);
-
-private:
- float _w;
- VectorXf _xv;
- MatrixXf _Pv;
- MatrixXf _xf;
- vector<MatrixXf> _Pf;
- float* _da;
-};
-
-#endif //PARTICLES_H
View
38 testcode/pi_to_pi.cpp
@@ -1,38 +0,0 @@
-#include "pi_to_pi.h"
-#include <iostream>
-
-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)
-{
- 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;
-}
-
View
16 testcode/pi_to_pi.h
@@ -1,16 +0,0 @@
-#ifndef PI_TO_PI_H
-#define PI_TO_PI_H
-
-#include <Eigen/Dense>
-#include <vector>
-#include <math.h>
-#define pi 3.1416
-
-using namespace Eigen;
-using namespace std;
-
-void pi_to_pi(VectorXf &angle); //takes in array of floats, returna array
-float pi_to_pi(float ang);
-//vector<int> find1(VectorXf input);
-
-#endif //PI_TO_PI_H
View
74 testcode/predict.cpp
@@ -1,74 +0,0 @@
-#include "predict.h"
-#include <math.h>
-#include <iostream>
-
-
-#define pi 3.1416
-//checked all values. everything works
-
-using namespace std;
-
-void predict(Particle &particle,float V,float G,Matrix2f Q, float WB,float dt, int addrandom)
-{
- VectorXf xv = particle.xv();
- MatrixXf Pv = particle.Pv();
- #if 0
- cout<<"particle.xv() in predict"<<endl;
- cout<<particle.xv()<<endl;
- cout<<"particle.Pv() in predict"<<endl;
- cout<<particle.Pv()<<endl;
- #endif
- //Jacobians
- float phi = xv(2);
- MatrixXf Gv(3,3);
-
- Gv << 1,0,-V*dt*sin(G+phi),
- 0,1,V*dt*cos(G+phi),
- 0,0,1;
- MatrixXf Gu(3,2);
- Gu << dt*cos(G+phi), -V*dt*sin(G+phi),
- dt*sin(G+phi), V*dt*cos(G+phi),
- dt*sin(G)/WB, V*dt*cos(G)/WB;
-
- //predict covariance
- MatrixXf newPv;//(Pv.rows(),Pv.cols());
-
- //TODO: Pv here is somehow corrupted. Probably in sample_proposal
- #if 0
- cout<<"Pv in predict"<<endl;
- cout<<Pv<<endl;
- #endif
-
- newPv = Gv*Pv*Gv.transpose() + Gu*Q*Gu.transpose();
- particle.setPv(newPv);
-
- //optional: add random noise to predicted state
- if (addrandom ==1) {
- VectorXf A(2);
- A(0) = V;
- A(1) = G;
- VectorXf VG(2);
- VG = multivariate_gauss(A,Q,1);
- V = VG(0);
- G = VG(1);
- }
-
- //predict state
- VectorXf xv_temp(3);
- xv_temp << xv(0) + V*dt*cos(G+xv(2)),
- xv(1) + V*dt*sin(G+xv(2)),
- 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
14 testcode/predict.h
@@ -1,14 +0,0 @@
-#ifndef PREDICT_H
-#define PREDICT_H
-
-#include <Eigen/Dense>
-#include "particle.h"
-#include "multivariate_gauss.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
9 testcode/predict_true.cpp
@@ -1,9 +0,0 @@
-#include "predict_true.h"
-#include "pi_to_pi.h"
-
-void predict_true(VectorXf &xv, float V, float G, int WB, float dt)
-{
- xv(0) = xv(0) + V*dt*cos(G+xv(2));
- xv(1) = xv(1) + V*dt*sin(G+xv(2));
- xv(2) = pi_to_pi(xv(2) + V*dt*sin(G)/WB);
-}
View
13 testcode/predict_true.h
@@ -1,13 +0,0 @@
-#ifndef PREDICT_TRUE_H
-#define PREDICT_TRUE_H
-
-#include <Eigen/Dense>
-
-using namespace Eigen;
-
-void predict_true(VectorXf &xv,float V,float G,int WB,float dt);
-//V is m/s
-// G is steering angle
-//WB = WHEELBASE
-
-#endif //PREDICT_TRUE_H
View
15 testcode/printMat.h
@@ -1,15 +0,0 @@
-#include <boost/numeric/ublas/vector.hpp>
-#include <boost/numeric/ublas/matrix.hpp>
-
-using namespace boost::numeric::ublas;
-
-void printMat(matrix A)
-{
- for (int r = 0; r< A.size1(); A++) {
- for (int c = 0; c< A.size2(); A++) {
- printf("%f ", A(r,c));
- }
- printf("\n");
- }
- printf("\n");
-}
View
44 testcode/resample_particles.cpp
@@ -1,44 +0,0 @@
-#include "resample_particles.h"
-#include "configfile.h"
-#include "stratified_resample.h"
-#include <iostream>
-
-using namespace std;
-
-void resample_particles(vector<Particle> &particles, int Nmin, int doresample)
-{
- unsigned int N = particles.size();
- //assert(particles.size() == config::NPARTICLES);
- VectorXf w(N);
- w.setZero();
-
- int i;
- for (i=0; i<N; i++) {
- w(i) = particles[i].w();
- //cout<<"w at "<<i<<" "<<w(i)<<endl;
- }
-
- float ws = w.sum();
-
- for (i=0; i<N; i++) {
- w(i) = w(i)/ws;
- }
-
- float Neff=0;
- vector<int> keep;
- stratified_resample(w,keep,Neff);
-
- vector<Particle> old_particles = vector<Particle>(particles);
- particles.resize(keep.size());
-
- 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;
- particles[i].setW(1.0/(float)N);
- }
- }
-}
View
14 testcode/resample_particles.h
@@ -1,14 +0,0 @@
-#ifndef RESAMPLE_PARTICLES_H
-#define RESAMPLE_PARTICLES_H
-
-#include <Eigen/Dense>
-#include <vector>
-#include "particle.h"
-
-
-using namespace Eigen;
-using namespace std;
-
-void resample_particles(vector<Particle> &particles, int Nmin, int doresample);
-
-#endif //RESAMPLE_PARTICLES_H
View
164 testcode/sample_proposal.cpp
@@ -1,164 +0,0 @@
-#include "sample_proposal.h"
-#include <iostream>
-#include <Eigen/SVD>
-#include <iomanip>
-#include "assert.h"
-
-//compute proposal distribution, then sample from it, and compute new particle weight
-void sample_proposal(Particle &particle, MatrixXf z, vector<int> idf, MatrixXf R)
-{
- VectorXf xv(particle.xv());
- MatrixXf Pv(particle.Pv());
-
- VectorXf xv0(xv);
- MatrixXf Pv0(Pv);
-
- vector<MatrixXf> Hv;
- vector<MatrixXf> Hf;
- vector<MatrixXf> Sf;
-
- MatrixXf zpi(z.rows(),1);
- zpi.setZero();
- MatrixXf Hvi;
- MatrixXf Hfi;
- MatrixXf Sfi;
-
- VectorXf vi(z.rows());
-
- //process each feature, incrementally refine proposal distribution
- unsigned i,r;
- for (i =0; i<idf.size(); i++) {
- vector<int> j;
- j.push_back(idf[i]);
-
- compute_jacobians(particle,j,R,zpi,&Hv,&Hf,&Sf);
- assert(zpi.cols() == 1);
-
- Hvi = Hv[i];
- Hfi = Hf[i];
- Sfi = Sf[i].inverse();
-#if 0
- cout<<"i in SAMPLE_PROPOSAL"<<endl;
- cout<<i<<endl;
- cout<<"should be 1"<<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++) {
- //cout<<"z("<<r<<","<<i<<") "<<z(r,i)<<endl;;
- //cout<<"zpi("<<r<<","<<"0) "<<zpi(r,0)<<endl;;
- vi[r] = z(r,i) - zpi(r,0);
- //cout<<"vi["<<r<<"] "<<vi[r]<<endl;
- }
-
-#if 0
- cout<<"in sample proposal"<<endl;
- cout<<"z"<<endl;
- cout<<z<<endl;
- cout<<endl;
- cout<<"zpi"<<endl;
- cout<<zpi<<endl;
- cout<<endl;
- cout<<"vi"<<endl;
- cout<<vi<<endl;
- cout<<endl;
-#endif
- vi[1] = pi_to_pi(vi[1]);
-
- //add a little bias so I won't get NaNs when I do an inverse
-
- #if 0
- cout<<"Pv"<<endl;
- cout<<Pv<<endl;
-
- for (r=0; r<Pv.rows(); r++) {
- for (c=0; c<Pv.cols(); c++) {
- if (r==c) {
- Pv(r,c) += 1.0e-6;
- }
- }
- }
- #endif
-
- //proposal covariance
- MatrixXf Pv_inv = Pv.llt().solve(MatrixXf::Identity(Pv.rows(), Pv.cols()));
- Pv = Hvi.transpose() * Sfi * Hvi + Pv_inv;//Pv.inverse();
- Pv = Pv.llt().solve(MatrixXf::Identity(Pv.rows(), Pv.cols()));//Pv.inverse();
-
- //proposal mean
- xv = xv + Pv * Hvi.transpose() * Sfi *vi;
- //cout<<"xv at the end"<<endl;
- //cout<<xv<<endl;
- particle.setXv(xv);
- particle.setPv(Pv);
- }
-
- //sample from proposal distribution
- VectorXf xvs = multivariate_gauss(xv,Pv,1);
- 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
- float like = likelihood_given_xv(particle, z, idf, R);
- float prior = gauss_evaluate(delta_xv(xv0,xvs), Pv0,0);
- float prop = gauss_evaluate(delta_xv(xv,xvs),Pv,0);
- #if 0
- cout<<"in compute_jacob: particle.w() = "<<particle.w()<<endl;
- cout<<"like = "<<like<<endl;
- cout<<"prior = "<<prior<<endl;
- cout<<"prop = "<<prop<<endl;
- #endif
- particle.setW(particle.w() * like * prior / prop);
- #if 0
- cout<<"particle.w()"<<endl;
- cout<<particle.w()<<endl;
- #endif
-}
-
-float likelihood_given_xv(Particle particle, MatrixXf z, vector<int>idf, MatrixXf R)
-{
- float w = 1;
- vector<int> idfi;
-
- vector<MatrixXf> Hv;
- vector<MatrixXf> Hf;
- vector<MatrixXf> Sf;
-
- MatrixXf zp;
- MatrixXf Sfi;
- VectorXf v(z.rows());
-
- unsigned i,k;
-
- for (i=0; i<idf.size(); i++){
- idfi.push_back(idf[i]);
- zp.resize(z.rows(), idfi.size());
- compute_jacobians(particle,idfi,R,zp,&Hv,&Hf,&Sf);
-
- for (k=0; k<z.rows(); k++) {
- v(k) = z(k,i)-zp(k,i);
- }
- v(1) = pi_to_pi(v(1));
-
- w = w*gauss_evaluate(v,Sf[0],0);
- //cout<<"new w: "<<w<<endl;
- }
- //cout<<"w returned "<<w<<endl;
- return w;
-}
-
-VectorXf delta_xv(VectorXf xv1, VectorXf xv2)
-{
- //compute innovation between two xv estimates, normalising the heading component
- VectorXf dx = xv1-xv2;
- dx(2) = pi_to_pi(dx(2));
- return dx;
-}
-
View
27 testcode/sample_proposal.h
@@ -1,27 +0,0 @@
-#ifndef SAMPLE_PROPOSAL_H
-#define SAMPLE_PROPOSAL_H
-
-#include <vector>
-#include <Eigen/Dense>
-
-#include "particle.h"
-#include "compute_jacobians.h"
-#include "multivariate_gauss.h"
-#include "gauss_evaluate.h"
-#include "pi_to_pi.h"
-
-using namespace Eigen;
-using namespace std;
-
-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);
-VectorXf delta_xv(VectorXf xv1, VectorXf xv2);
-
-#if 0
-template<typename _Matrix_Type_>
-bool pseudoInverse(const _Matrix_Type_ &a, _Matrix_Type_ &result, double
-epsilon = numeric_limits<typename _Matrix_Type_::Scalar>::epsilon());
-#endif
-
-#endif
View
33 testcode/stratified_random.cpp
@@ -1,33 +0,0 @@
-#include "stratified_random.h"
-#include <assert.h>
-#include <iostream>
-
-using namespace std;
-
-void stratified_random(int N, vector<float> &di)
-{
- float k = 1.0/(float)N;
-
- //deterministic intervals
- float temp = k/2;
- while (t