test_compare_filters.cpp
Go to the documentation of this file.
00001 // $Id: test_compare_filters.cpp 5925 2006-03-14 21:23:49Z tdelaet $
00002 // Copyright (C) 2006 Klaas Gadeyne <first dot last at gmail dot com>
00003 //                    Tinne De Laet <first dot last at mech dot kuleuven dot be>
00004 //
00005 // This program is free software; you can redistribute it and/or modify
00006 // it under the terms of the GNU Lesser General Public License as published by
00007 // the Free Software Foundation; either version 2.1 of the License, or
00008 // (at your option) any later version.
00009 //
00010 // This program is distributed in the hope that it will be useful,
00011 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013 // GNU Lesser General Public License for more details.
00014 //
00015 // You should have received a copy of the GNU Lesser General Public License
00016 // along with this program; if not, write to the Free Software
00017 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00018 
00019 /* Demonstration program for the Bayesian Filtering Library.
00020    Mobile robot localization with respect to wall with different possibilities for filter
00021 */
00022 
00023 #include <filter/bootstrapfilter.h>
00024 #include <filter/extendedkalmanfilter.h>
00025 #include <filter/iteratedextendedkalmanfilter.h>
00026 #include <filter/asirfilter.h>
00027 #include <filter/EKparticlefilter.h>
00028 
00029 #include "nonlinearanalyticconditionalgaussianmobile.h"//added
00030 #include <model/analyticsystemmodel_gaussianuncertainty.h>
00031 #include <model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
00032 #include <pdf/analyticconditionalgaussian.h>
00033 #include <pdf/gaussian.h>
00034 
00035 #include <wrappers/matrix/matrix_wrapper.h>
00036 #include <wrappers/matrix/vector_wrapper.h>
00037 
00038 #include <iostream>
00039 #include <fstream>
00040 #include <string>
00041 
00042 // Include file with properties
00043 #include "../mobile_robot_wall_cts.h"
00044 // Include pdf specific for this example
00045 #include "nonlinearanalyticconditionalgaussianmobile.h"
00046 // Include the mobile_robot simulator
00047 #include "../mobile_robot.h"
00048 
00049 /* The purpose of this program is to test different filter for the problem
00050   of localisation of a mobile robot equipped with an ultrasonic sensor.
00051   The ultrasonic measures the distance to the wall (it can be switched off:
00052   see mobile_robot_wall_cts.h)
00053 
00054   The necessary SYSTEM MODEL is:
00055 
00056   x_k      = x_{k-1} + v_{k-1} * cos(theta_{k-1} * delta_t
00057   y_k      = y_{k-1} + v_{k-1} * sin(theta_{k-1} * delta_t
00058   theta_k  = theta_{k-1} + omega_{k-1} * delta_t
00059 
00060   The used MEASUREMENT MODEL:
00061   measuring the (perpendicular) distance z to the wall y = ax + b
00062 
00063   set WALL_CT = 1/sqrt(pow(a,2) + 1)
00064   z = WALL_CT * a * x - WALL_CT * y + WALL_CT * b + GAUSSIAN_NOISE
00065   or Z = H * X_k + J * U_k
00066 
00067   where
00068 
00069   H = [ WALL_CT * a       - WALL_CT      0 ]
00070   and GAUSSIAN_NOISE = N((WALL_CT * b), SIGMA_MEAS_NOISE)
00071 
00072 */
00073 
00074 using namespace MatrixWrapper;
00075 using namespace BFL;
00076 using namespace std;
00077 
00078 #define KALMAN      1
00079 #define IE_KALMAN   2
00080 #define BOOTSTRAP   3
00081 #define EK_PARTICLE 4
00082 #define ASIR        5
00083 
00084 int main(int argc, char** argv)
00085 {
00086   cerr << "==================================================" << endl
00087        << "Test of switching between different filters" << endl
00088        << "Mobile robot localisation example" << endl
00089        << "==================================================" << endl;
00090 
00091 
00092   int filter_name;
00093   if (!(argc== 2 ))
00094   {
00095     cout << "Please provide one argument. Possible arguments are:" << endl
00096          << "  kalman_filter" << endl
00097          << "  bootstrap_filter" << endl
00098          << "  EK_particle_filter" << endl
00099          << "  ASIR_filter" << endl
00100          << "  IE_kalman_filter" << endl;
00101     return 0;
00102   }
00103   else {
00104     string argument = argv[1];
00105     if (argument == "kalman_filter")             filter_name = KALMAN;
00106     else if (argument == "IE_kalman_filter")     filter_name = IE_KALMAN;
00107     else if (argument == "bootstrap_filter")     filter_name = BOOTSTRAP;
00108     else if (argument == "EK_particle_filter")   filter_name = EK_PARTICLE;
00109     else if (argument == "ASIR_filter")          filter_name = ASIR;
00110     else
00111       {
00112         cout << "Please provide another argument. Possible arguments are:" << endl
00113              << "  kalman_filter" << endl
00114              << "  bootstrap_filter" << endl
00115              << "  EK_particle_filter" << endl
00116              << "  ASIR_filter" << endl
00117              << "  IE_kalman_filter" << endl;
00118         return 0 ;
00119       }
00120   }
00121 
00122   /***********************
00123    * PREPARE FILESTREAMS *
00124    **********************/
00125   ofstream fout_time, fout_E, fout_cov, fout_meas, fout_states, fout_particles, fout_numparticles;
00126 
00127   fout_time.open("time.out");
00128   fout_E.open("E.out");
00129   fout_cov.open("cov.out");
00130   fout_meas.open("meas.out");
00131   fout_states.open("states.out");
00132 
00133   if ((filter_name == BOOTSTRAP) || (filter_name == EK_PARTICLE) ||(filter_name == ASIR))
00134     {
00135       fout_particles.open("particles.out");
00136       fout_numparticles.open("numparticles.out");
00137     }
00138 
00139 
00140   /****************************
00141    * Initialise system model *
00142    ***************************/
00143   ColumnVector sys_noise_Mu(STATE_SIZE);
00144   sys_noise_Mu = 0.0;
00145   sys_noise_Mu(1) = MU_SYSTEM_NOISE_X;
00146   sys_noise_Mu(2) = MU_SYSTEM_NOISE_Y;
00147   sys_noise_Mu(3) = MU_SYSTEM_NOISE_THETA;
00148 
00149   SymmetricMatrix sys_noise_Cov(STATE_SIZE);
00150   sys_noise_Cov = 0.0;
00151   sys_noise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
00152   sys_noise_Cov(1,2) = 0.0;
00153   sys_noise_Cov(1,3) = 0.0;
00154   sys_noise_Cov(2,1) = 0.0;
00155   sys_noise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
00156   sys_noise_Cov(2,3) = 0.0;
00157   sys_noise_Cov(3,1) = 0.0;
00158   sys_noise_Cov(3,2) = 0.0;
00159   sys_noise_Cov(3,3) = SIGMA_SYSTEM_NOISE_THETA;
00160 
00161   Gaussian system_Uncertainty(sys_noise_Mu, sys_noise_Cov);
00162   NonLinearAnalyticConditionalGaussianMobile sys_pdf(system_Uncertainty);
00163   AnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);
00164 
00165 
00166   /*********************************
00167    * Initialise measurement model *
00168    ********************************/
00169   // Fill up H
00170   double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
00171   Matrix H(MEAS_SIZE,STATE_SIZE);
00172   H = 0.0;
00173   H(1,1) = wall_ct * RICO_WALL;
00174   H(1,2) = 0 - wall_ct;
00175   cout<< "Measurment model H = " << H << endl;
00176 
00177   // Construct the measurement noise (a scalar in this case)
00178   ColumnVector MeasNoise_Mu(MEAS_SIZE);
00179   SymmetricMatrix MeasNoise_Cov(MEAS_SIZE);
00180   MeasNoise_Mu(1) = MU_MEAS_NOISE;
00181   MeasNoise_Cov(1,1) = SIGMA_MEAS_NOISE;
00182 
00183   Gaussian Measurement_Uncertainty(MeasNoise_Mu,MeasNoise_Cov);
00184   LinearAnalyticConditionalGaussian meas_pdf(H,Measurement_Uncertainty);
00185   LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);
00186 
00187 
00188   /****************************
00189    * Initialise prior DENSITY *
00190    ***************************/
00191    // Continuous Gaussian prior (for Kalman filters)
00192    ColumnVector prior_mu(STATE_SIZE);
00193    SymmetricMatrix prior_sigma(STATE_SIZE);
00194    prior_mu(1) = PRIOR_MU_X;
00195    prior_mu(2) = PRIOR_MU_Y;
00196    prior_mu(STATE_SIZE) = PRIOR_MU_THETA;
00197    prior_sigma = 0.0;
00198    prior_sigma(1,1) = PRIOR_COV_X;
00199    prior_sigma(2,2) = PRIOR_COV_Y;
00200    prior_sigma(3,3) = PRIOR_COV_THETA;
00201    Gaussian prior_cont(prior_mu,prior_sigma);
00202 
00203    // Discrete prior for Particle filter (using the continuous Gaussian prior)
00204    vector<Sample<ColumnVector> > prior_samples(NUM_SAMPLES);
00205    MCPdf<ColumnVector> prior_discr(NUM_SAMPLES,STATE_SIZE);
00206    prior_cont.SampleFrom(prior_samples,NUM_SAMPLES,CHOLESKY,NULL);
00207    prior_discr.ListOfSamplesSet(prior_samples);
00208 
00209    cout<< "Prior initialised"<< "" << endl;
00210    cout << "Prior Mean = " << endl << prior_cont.ExpectedValueGet() << endl
00211         << "Covariance = " << endl << prior_cont.CovarianceGet() << endl;
00212 
00213 
00214 
00215 
00216   /***************************
00217    * initialise MOBILE ROBOT *
00218    **************************/
00219   // Model of mobile robot in world with one wall
00220   // The model is used to simultate the distance measurements.
00221   MobileRobot mobile_robot;
00222   ColumnVector input(INPUT_SIZE);
00223   input(1) = LIN_SPEED * DELTA_T;
00224   input(2)  = ROT_SPEED * DELTA_T;
00225   cout << "Mobile robot initialised"<< "" << endl;
00226 
00227 
00228 
00229   /******************************
00230    * Construction of the Filter *
00231    ******************************/
00232   Filter<ColumnVector,ColumnVector> * my_filter = NULL;
00233 
00234   switch (filter_name){
00235   case KALMAN:{
00236     cout << "Using the Extended Kalman Filter" << endl;
00237     my_filter = new ExtendedKalmanFilter(&prior_cont);
00238     break;}
00239   case IE_KALMAN:{
00240     cout << "Using the Iterated Extended Kalman Filter, " << NUM_ITERATIONS << " iterations" << endl;
00241     my_filter = new IteratedExtendedKalmanFilter(&prior_cont,NUM_ITERATIONS);
00242     break;}
00243   case BOOTSTRAP:{
00244     cout << "Using the bootstrapfilter, " << NUM_SAMPLES << " samples, dynamic resampling" << endl;
00245     my_filter = new BootstrapFilter<ColumnVector,ColumnVector> (&prior_discr, RESAMPLE_PERIOD, RESAMPLE_THRESHOLD);
00246     break;}
00247   case EK_PARTICLE:{
00248     cout << "Using the Extended Particle Kalman Filter, " << NUM_SAMPLES << " samples, dynamic resampling" << endl;
00249     my_filter = new EKParticleFilter(&prior_discr, 0, RESAMPLE_THRESHOLD);
00250     break;}
00251     //case ASIR:{
00252     //cout << "Using the ASIR-filter, " << NUM_SAMPLES << " samples, fixed period resampling" << endl;
00253     //my_filter = new ASIRFilter<ColumnVector,ColumnVector> (prior_discr, RESAMPLE_PERIOD, RESAMPLE_THRESHOLD);
00254     //break;}
00255   default:{
00256     cout << "Type if filter not recognised on construction" <<endl;
00257     return 0 ;}
00258   }
00259 
00260 
00261 
00262   /*******************
00263    * ESTIMATION LOOP *
00264    *******************/
00265   cout << "MAIN: Starting estimation" << endl;
00266   unsigned int time_step;
00267   for (time_step = 0; time_step < NUM_TIME_STEPS-1; time_step++)
00268     {
00269       // write date in files
00270       fout_time << time_step << ";" << endl;
00271       fout_meas << mobile_robot.Measure()(1) << ";" << endl;
00272       fout_states << mobile_robot.GetState()(1) << "," << mobile_robot.GetState()(2) << ","
00273                   << mobile_robot.GetState()(3) << ";" << endl;
00274 
00275       // write posterior to file
00276       Pdf<ColumnVector> * posterior = my_filter->PostGet();
00277       fout_E << posterior->ExpectedValueGet()(1) << "," << posterior->ExpectedValueGet()(2)<< ","
00278              << posterior->ExpectedValueGet()(3) << ";"  << endl;
00279       fout_cov << posterior->CovarianceGet()(1,1) << "," << posterior->CovarianceGet()(1,2) << ","
00280                << posterior->CovarianceGet()(1,3) << "," << posterior->CovarianceGet()(2,2) << ","
00281                << posterior->CovarianceGet()(2,3) << "," << posterior->CovarianceGet()(3,3) << ";" << endl;
00282 
00283 
00284       // write particles to file
00285       if ((filter_name == BOOTSTRAP) || (filter_name == EK_PARTICLE) ||(filter_name == ASIR))
00286         {
00287           fout_numparticles << ((MCPdf<ColumnVector>*)posterior)->NumSamplesGet() << ";" << endl;
00288           vector<WeightedSample<ColumnVector> > samples_list = ((MCPdf<ColumnVector>*)posterior)->ListOfSamplesGet() ;
00289           vector<WeightedSample<ColumnVector> >::iterator sample;
00290           for ( sample = samples_list.begin() ; sample != samples_list.end() ; sample++ )
00291               fout_particles << sample->ValueGet()(1) << "," << sample->ValueGet()(2) << ","
00292                              << sample->ValueGet()(3) << "," <<  sample->WeightGet() <<";"<<endl;
00293         }
00294 
00295 
00296       // DO ONE STEP WITH MOBILE ROBOT
00297       mobile_robot.Move(input);
00298 
00299       // DO ONE MEASUREMENT
00300       ColumnVector measurement = mobile_robot.Measure();
00301 
00302       // UPDATE FILTER
00303       if (USE_MEASUREMENTS)
00304           my_filter->Update(&sys_model,input,&meas_model, measurement);
00305       else
00306           my_filter->Update(&sys_model, input);
00307     } // estimation loop
00308 
00309 
00310 
00311   Pdf<ColumnVector> * posterior = my_filter->PostGet();
00312   cout << "After " << time_step+1 << " timesteps " << endl;
00313   cout << " Posterior Mean = " << endl << posterior->ExpectedValueGet() << endl
00314        << " Covariance = " << endl << posterior->CovarianceGet() << "" << endl;
00315   cout << "=============================================" << endl;
00316 
00317 
00318   // delete the filter
00319   delete my_filter;
00320 
00321   cout << "==================================================" << endl
00322        << "End of the Comparing filters test" << endl
00323        << "=================================================="
00324        << endl;
00325 
00326 
00327 
00328   /****************************
00329    * CLOSE FILESTREAMS
00330    ***************************/
00331   fout_time.close();
00332   fout_E.close();
00333   fout_cov.close();
00334   fout_meas.close();
00335   fout_states.close();
00336   if ((filter_name == BOOTSTRAP) || (filter_name == EK_PARTICLE) ||(filter_name == ASIR))
00337     {
00338       fout_particles.close();
00339       fout_numparticles.close();
00340     }
00341   return 0;
00342 }


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Sun Oct 5 2014 22:29:53