Permalink
Browse files

start writing fastslam1

  • Loading branch information...
1 parent 783ed1e commit 6b3118feb896d318e4c474ff108a28bb7fb36e2e @yglee committed Aug 27, 2012
View
@@ -1,3 +1,9 @@
+++++++++++++++++++++++
+Function Description
+++++++++++++++++++++++
+
+
+
+++++++++++++++++++++
Algorithm
+++++++++++++++++++++
@@ -20,11 +26,12 @@ xv = robot pose? (x, y, theta) - aka path estimate
Pv = (either control or measurement??)
xf = (either control or measurement??)
Pf = set of covariance matrix for kalman filter gaussian reprenting nth feature location
+ length should equal number of landmarks the particle sees at time t
da = set of mean for kalman filter
set of kalman filters with mean and covariance (one for each feature in the map)
-w.lm = landmark positions
+w.lm = landmark positions (range, bearing)
w.wp = way points defining robots path (bcs this is known, cond'l independence is possible
@@ -38,23 +45,6 @@ Kalman filter is used to estimate the map feature locations.
Each particle posses its own set of EKF since each EKF is conditioned on a robot path.
=====Fast SLAM Notes=======
-Features are conditionally indpendent given the robot path
-
-
-
-8/23/12
-Went through and did unity tests on all files and they seem to generate correct result for one given set of inputs. There is a matrix dimensionality error and at this point, I need to read the papers and identify each variables and rename them and think about what they do...
-
-
-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
+Features are conditionally indpendent given the robot path
-7/25/12
-fix the multivariate gauss's use of Random() (from eigen). It doesn't return diff. random numbers at each turn
+
View
@@ -0,0 +1,13 @@
+FastSLAM
+========
+
+Retrieval
+
+Prediction
+
+Measurement Update
+
+Importance Weight
+
+Resampling
+
@@ -0,0 +1,225 @@
+#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 is the range and bearing of the observed landmark
+ 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]<<" ";
+ }
+ }
+
+ MatrixXf zf(z.rows(),ftag_visible.size());
+ zf.setZero();
+ MatrixXf zn(z.rows(),ftag_visible.size());
+ zn.setZero();
+
+ bool testflag= false;
+
+ data_associate_known(z,ftag_visible,da_table,Nf,zf,idf,zn);
+ //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);
+}
@@ -0,0 +1,22 @@
+#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
@@ -0,0 +1,79 @@
+#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;
+}
Oops, something went wrong.

0 comments on commit 6b3118f

Please sign in to comment.