test_nonlinear_smoother.cpp
Go to the documentation of this file.
00001 // $Id: test_nonlinear_smoother.cpp 5925 2006-03-14 21:23:49Z tdelaet $
00002 // Copyright (C) 2006 Tinne De Laet <first dot last at mech dot kuleuven dot be>
00003 //
00004 // This program is free software; you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published by
00006 // the Free Software Foundation; either version 2.1 of the License, or
00007 // (at your option) any later version.
00008 //
00009 // This program is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 //
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program; if not, write to the Free Software
00016 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00017 
00018 /* Demonstration program for the Bayesian Filtering Library.
00019    Mobile robot localization with respect to wall with different possibilities for filter
00020 */
00021 
00022 
00023 #include <filter/extendedkalmanfilter.h>
00024 #include <filter/bootstrapfilter.h>
00025 
00026 #include <model/linearanalyticsystemmodel_gaussianuncertainty.h>
00027 #include <model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
00028 
00029 #include <pdf/analyticconditionalgaussian.h>
00030 #include <pdf/analyticconditionalgaussian.h>
00031 #include <pdf/linearanalyticconditionalgaussian.h>
00032 #include <pdf/mcpdf.h>
00033 #include <pdf/pdf.h>
00034 
00035 #include "nonlinearanalyticconditionalgaussianmobile.h"
00036 #include "mobile_robot.h"
00037 
00038 #include <smoother/rauchtungstriebel.h>
00039 #include <smoother/particlesmoother.h>
00040 
00041 #include <iostream>
00042 #include <fstream>
00043 
00044 using namespace MatrixWrapper;
00045 using namespace BFL;
00046 using namespace std;
00047 
00048 
00049 
00050 /* The purpose of this program is to construct a smoother (consisting of a
00051   forward and a backward filter) for the problem
00052   of localisation of a mobile robot equipped with an ultrasonic sensor.
00053   The ultrasonic measures the distance to the wall (it can be switched off:
00054   see mobile_robot_wall_cts.h)
00055 
00056   The necessary SYSTEM MODEL is:
00057 
00058   x_k      = x_{k-1} + v_{k-1} * cos(theta) * delta_t
00059   y_k      = y_{k-1} + v_{k-1} * sin(theta) * delta_t
00060 
00061   The used MEASUREMENT MODEL:
00062   measuring the (perpendicular) distance z to the wall y = ax + b
00063 
00064   set WALL_CT = 1/sqrt(pow(a,2) + 1)
00065   z = WALL_CT * a * x - WALL_CT * y + WALL_CT * b + GAUSSIAN_NOISE
00066   or Z = H * X_k + J * U_k
00067 
00068   where
00069 
00070   H = [ WALL_CT * a       - WALL_CT      0 ]
00071   and GAUSSIAN_NOISE = N((WALL_CT * b), SIGMA_MEAS_NOISE)
00072 
00073 */
00074 
00075 #define KALMAN      1
00076 #define BOOTSTRAP   2
00077 
00078 int main(int argc, char** argv)
00079 {
00080   cerr << "==================================================" << endl
00081        << "Test of different smooters" << endl
00082        << "Mobile robot localisation example" << endl
00083        << "==================================================" << endl;
00084 
00085   int smoother_name;
00086   if (!(argc== 2 ))
00087   {
00088     cout << "Please provide one argument. Possible arguments are:" << endl
00089          << "  kalman_smoother" << endl
00090          << "  bootstrap_smoother" << endl;
00091     return 0;
00092   }
00093   else {
00094     string argument = argv[1];
00095     if (argument == "kalman_filter")             smoother_name = KALMAN;
00096     else if (argument == "bootstrap_filter")     smoother_name = BOOTSTRAP;
00097     else
00098       {
00099         cout << "Please provide another argument. Possible arguments are:" << endl
00100              << "  kalman_filter" << endl
00101              << "  bootstrap_filter" << endl;
00102         return 0 ;
00103       }
00104   }
00105   /***********************
00106    * PREPARE FILESTREAMS *
00107    **********************/
00108   ofstream fout_time, fout_E, fout_cov, fout_meas, fout_states, fout_particles, fout_numparticles, fout_E_smooth, fout_cov_smooth, fout_time_smooth, fout_particles_smooth, fout_numparticles_smooth;
00109 
00110   fout_time.open("time.out");
00111   fout_E.open("E.out");
00112   fout_cov.open("cov.out");
00113   fout_meas.open("meas.out");
00114   fout_states.open("states.out");
00115   fout_E_smooth.open("Esmooth.out");
00116   fout_cov_smooth.open("covsmooth.out");
00117   fout_time_smooth.open("timesmooth.out");
00118 
00119   if (smoother_name  == BOOTSTRAP )
00120     {
00121       fout_particles.open("particles.out");
00122       fout_numparticles.open("numparticles.out");
00123       fout_particles_smooth.open("particlessmooth.out");
00124       fout_numparticles_smooth.open("numparticlessmooth.out");
00125     }
00126 
00127 
00128   /****************************
00129    * Linear system model      *
00130    ***************************/
00131 
00132   // create gaussian
00133   ColumnVector sysNoise_Mu(3);
00134   sysNoise_Mu(1) = 0.0;
00135   sysNoise_Mu(2) = 0.0;
00136   sysNoise_Mu(3) = 0.0;
00137 
00138   SymmetricMatrix sysNoise_Cov(3);
00139   sysNoise_Cov(1,1) = pow(0.05,2);
00140   sysNoise_Cov(1,2) = 0.0;
00141   sysNoise_Cov(1,3) = 0.0;
00142   sysNoise_Cov(2,1) = 0.0;
00143   sysNoise_Cov(2,2) = pow(0.05,2);
00144   sysNoise_Cov(2,3) = 0.0;
00145   sysNoise_Cov(3,1) = 0.0;
00146   sysNoise_Cov(3,2) = 0.0;
00147   sysNoise_Cov(3,3) = pow(0.03,2);
00148 
00149   Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
00150 
00151   // create the model
00152   NonLinearAnalyticConditionalGaussianMobile sys_pdf(system_Uncertainty);
00153   AnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);
00154 
00155   /*********************************
00156    * Initialise measurement model *
00157    ********************************/
00158 
00159   // create matrix H for linear measurement model
00160   Matrix H(1,3);
00161   H(1,1) = 0.0;
00162   H(1,2) = 2.0;
00163   H(1,3) = 0;
00164   // Construct the measurement noise (a scalar in this case)
00165   ColumnVector measNoise_Mu(1);
00166   measNoise_Mu(1) = 0.0;
00167 
00168   SymmetricMatrix measNoise_Cov(1);
00169   measNoise_Cov(1,1) = pow(0.05,2);
00170   Gaussian measurement_Uncertainty(measNoise_Mu, measNoise_Cov);
00171 
00172   // create the model
00173   LinearAnalyticConditionalGaussian meas_pdf(H, measurement_Uncertainty);
00174   LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);
00175 
00176 
00177   /****************************
00178    * Linear prior DENSITY     *
00179    ***************************/
00180    // Continuous Gaussian prior (for Kalman filters)
00181   ColumnVector prior_Mu(3);
00182   prior_Mu(1) = 1.0;//-1.0;
00183   prior_Mu(2) = 0.0;//1.0;
00184   prior_Mu(3) = 0.0;
00185   SymmetricMatrix prior_Cov(3);
00186   prior_Cov(1,1) = 0.1;
00187   prior_Cov(1,2) = 0.0;
00188   prior_Cov(1,3) = 0.0;
00189   prior_Cov(2,1) = 0.0;
00190   prior_Cov(2,2) = 1.0;
00191   prior_Cov(2,3) = 0.0;
00192   prior_Cov(3,1) = 0.0;
00193   prior_Cov(3,2) = 0.0;
00194   prior_Cov(3,3) = pow(0.8,2);
00195   Gaussian prior_cont(prior_Mu,prior_Cov);
00196 
00197   int NUM_SAMPLES =  1000;
00198   vector<Sample<ColumnVector> > prior_samples(NUM_SAMPLES);
00199   MCPdf<ColumnVector> prior_discr(NUM_SAMPLES,3);
00200   prior_cont.SampleFrom(prior_samples,NUM_SAMPLES,CHOLESKY,NULL);
00201   prior_discr.ListOfSamplesSet(prior_samples);
00202 
00203    cout<< "Prior initialised"<< "" << endl;
00204    cout << "Prior Mean = " << endl << prior_cont.ExpectedValueGet() << endl
00205         << "Covariance = " << endl << prior_cont.CovarianceGet() << endl;
00206 
00207   /******************************
00208    * Construction of the Filter *
00209    ******************************/
00210   //ExtendedKalmanFilter filter(&prior_cont);
00211   Filter<ColumnVector,ColumnVector> * filter = NULL;
00212 
00213   switch (smoother_name){
00214   case KALMAN:{
00215     cout << "Using the Extended Kalman Filter" << endl;
00216     filter = new ExtendedKalmanFilter(&prior_cont);
00217     break;}
00218   case BOOTSTRAP:{
00219     cout << "Using the bootstrapfilter, " << NUM_SAMPLES << " samples, dynamic resampling" << endl;
00220     filter = new BootstrapFilter<ColumnVector,ColumnVector> (&prior_discr, 0, NUM_SAMPLES/4.0);
00221     break;}
00222   default:{
00223     cout << "Type if filter not recognised on construction" <<endl;
00224     return 0 ;}
00225   }
00226 
00227 
00228   /***************************
00229    * initialise MOBILE ROBOT *
00230    **************************/
00231   // Model of mobile robot in world with one wall
00232   // The model is used to simultate the distance measurements.
00233   MobileRobot mobile_robot;
00234   ColumnVector input(2);
00235   input(1) = 0.1;
00236   input(2) = 0.0;
00237 
00238 
00239   /***************************
00240    * number of timesteps*
00241    **************************/
00242   unsigned int num_timesteps = 3;
00243 
00244   /***************************
00245    * vector in which all posteriors will be stored*
00246    **************************/
00247   // in case of gaussian
00248   vector<Gaussian> posteriors_gauss(num_timesteps);
00249   vector<Gaussian>::iterator posteriors_it_gauss  = posteriors_gauss.begin();
00250   // in case of mcpdf
00251   vector<MCPdf<ColumnVector> > posteriors_mcpdf(num_timesteps);
00252   vector<MCPdf<ColumnVector> >::iterator posteriors_it_mcpdf  = posteriors_mcpdf.begin();
00253 
00254   /*******************
00255    * ESTIMATION LOOP *
00256    *******************/
00257   cout << "MAIN: Starting estimation" << endl;
00258   unsigned int time_step;
00259   for (time_step = 0; time_step < num_timesteps; time_step++)
00260     {
00261       // write date in files
00262       fout_time << time_step << ";" << endl;
00263       fout_meas << mobile_robot.Measure()(1) << ";" << endl;
00264       fout_states << mobile_robot.GetState()(1) << "," << mobile_robot.GetState()(2) << ","
00265                   << mobile_robot.GetState()(3) << ";" << endl;
00266 
00267       // write posterior to file
00268       Pdf<ColumnVector> * posterior = filter->PostGet();
00269       fout_E << posterior->ExpectedValueGet()(1) << "," << posterior->ExpectedValueGet()(2)<< ","
00270              << posterior->ExpectedValueGet()(3) << ";"  << endl;
00271       fout_cov << posterior->CovarianceGet()(1,1) << "," << posterior->CovarianceGet()(1,2) << ","
00272                << posterior->CovarianceGet()(1,3) << "," << posterior->CovarianceGet()(2,2) << ","
00273                << posterior->CovarianceGet()(2,3) << "," << posterior->CovarianceGet()(3,3) << ";" << endl;
00274 
00275       // write particles to file
00276       if (smoother_name == BOOTSTRAP)
00277           {
00278             fout_numparticles << ((MCPdf<ColumnVector>*)posterior)->NumSamplesGet() << ";" << endl;
00279             vector<WeightedSample<ColumnVector> > samples_list = ((MCPdf<ColumnVector>*)posterior)->ListOfSamplesGet() ;
00280             vector<WeightedSample<ColumnVector> >::iterator sample;
00281             for ( sample = samples_list.begin() ; sample != samples_list.end() ; sample++ )
00282                 fout_particles << sample->ValueGet()(1) << "," << sample->ValueGet()(2) << ","
00283                                << sample->ValueGet()(3) << "," <<  sample->WeightGet() <<";"<<endl;
00284           }
00285       // DO ONE STEP WITH MOBILE ROBOT
00286       mobile_robot.Move(input);
00287 
00288       if ((time_step+1)%5 == 0){
00289         // DO ONE MEASUREMENT
00290         ColumnVector measurement = mobile_robot.Measure();
00291         // UPDATE FILTER
00292         filter->Update(&sys_model,input,&meas_model,measurement);
00293       }
00294       else{
00295         filter->Update(&sys_model,input);
00296       }
00297 
00298       // make copy of posterior
00299       //*posteriors_it = (Gaussian)(*posterior);
00300       switch (smoother_name){
00301       case KALMAN:{
00302         *posteriors_it_gauss = *(Gaussian*)(posterior);
00303         posteriors_it_gauss++;
00304         break;}
00305       case BOOTSTRAP:{
00306         *posteriors_it_mcpdf = *(MCPdf<ColumnVector>*)(posterior);
00307         posteriors_it_mcpdf++;
00308         break;}
00309       default:{
00310         cout << "Type if filter not recognised on construction" <<endl;
00311         return 0 ;}
00312         }
00313     } // estimation loop
00314 
00315 
00316     Pdf<ColumnVector> * posterior = filter->PostGet();
00317     // write data in files
00318     fout_time << time_step << ";" << endl;
00319     fout_meas << mobile_robot.Measure()(1) << ";" << endl;
00320     fout_states << mobile_robot.GetState()(1) << "," << mobile_robot.GetState()(2) << ","
00321                 << mobile_robot.GetState()(3) << ";" << endl;
00322 
00323     // write posterior to file
00324     fout_E << posterior->ExpectedValueGet()(1) << "," << posterior->ExpectedValueGet()(2)<< ","
00325            << posterior->ExpectedValueGet()(3) << ";"  << endl;
00326     fout_cov << posterior->CovarianceGet()(1,1) << "," << posterior->CovarianceGet()(1,2) << ","
00327              << posterior->CovarianceGet()(1,3) << "," << posterior->CovarianceGet()(2,2) << ","
00328              << posterior->CovarianceGet()(2,3) << "," << posterior->CovarianceGet()(3,3) << ";" << endl;
00329     // write particles to file
00330     if (smoother_name == BOOTSTRAP)
00331         {
00332           fout_numparticles << ((MCPdf<ColumnVector>*)posterior)->NumSamplesGet() << ";" << endl;
00333           vector<WeightedSample<ColumnVector> > samples_list = ((MCPdf<ColumnVector>*)posterior)->ListOfSamplesGet() ;
00334           vector<WeightedSample<ColumnVector> >::iterator sample;
00335           for ( sample = samples_list.begin() ; sample != samples_list.end() ; sample++ )
00336               fout_particles << sample->ValueGet()(1) << "," << sample->ValueGet()(2) << ","
00337                              << sample->ValueGet()(3) << "," <<  sample->WeightGet() <<";"<<endl;
00338         }
00339 
00340   cout << "After " << time_step+1 << " timesteps " << endl;
00341   cout << " Posterior Mean = " << endl << posterior->ExpectedValueGet() << endl
00342        << " Covariance = " << endl << posterior->CovarianceGet() << "" << endl;
00343 
00344 
00345   cout << "======================================================" << endl
00346        << "End of the filter for mobile robot localisation" << endl
00347        << "======================================================"
00348        << endl;
00349 
00350 
00351   /***************************************
00352    * Construction of the Backward Filter *
00353    **************************************/
00354   BackwardFilter<ColumnVector> * backwardfilter = NULL;
00355 
00356   switch (smoother_name){
00357   case KALMAN:{
00358     cout << "Using the RauchTungStriebelSmoother" << endl;
00359     backwardfilter = new RauchTungStriebel((Gaussian*)posterior);
00360     break;}
00361   case BOOTSTRAP:{
00362     cout << "Using the particlesmoother, " << NUM_SAMPLES << " samples" << endl;
00363     backwardfilter = new ParticleSmoother<ColumnVector>((MCPdf<ColumnVector>*)posterior);
00364     break;}
00365   default:{
00366     cout << "Type if filter not recognised on construction" <<endl;
00367     return 0 ;}
00368   }
00369   fout_time_smooth << time_step << ";" << endl;
00370   // write posterior to file
00371   fout_E_smooth << posterior->ExpectedValueGet()(1) << "," << posterior->ExpectedValueGet()(2)<< ","
00372          << posterior->ExpectedValueGet()(3) << ";"  << endl;
00373   fout_cov_smooth << posterior->CovarianceGet()(1,1) << "," << posterior->CovarianceGet()(1,2) << ","
00374            << posterior->CovarianceGet()(1,3) << "," << posterior->CovarianceGet()(2,2) << ","
00375            << posterior->CovarianceGet()(2,3) << "," << posterior->CovarianceGet()(3,3) << ";" << endl;
00376   // write particles to file
00377   if (smoother_name == BOOTSTRAP)
00378   {
00379     fout_numparticles_smooth << ((MCPdf<ColumnVector>*)posterior)->NumSamplesGet() << ";" << endl;
00380     vector<WeightedSample<ColumnVector> > samples_list = ((MCPdf<ColumnVector>*)posterior)->ListOfSamplesGet() ;
00381     vector<WeightedSample<ColumnVector> >::iterator sample;
00382         for ( sample = samples_list.begin() ; sample != samples_list.end() ; sample++ )
00383         fout_particles_smooth << sample->ValueGet()(1) << "," << sample->ValueGet()(2) << ","
00384                            << sample->ValueGet()(3) << "," <<  sample->WeightGet() <<";"<<endl;
00385   }
00386 
00387   /*******************
00388    * ESTIMATION LOOP *
00389    *******************/
00390   cout << "======================================================" << endl
00391        << "Start of the smoother for mobile robot localisation" << endl
00392        << "======================================================"
00393        << endl;
00394   for (time_step = num_timesteps-1; time_step+1 > 0 ; time_step--)
00395     {
00396       //Pdf<ColumnVector> filtered;
00397       switch (smoother_name){
00398       case KALMAN:{
00399         posteriors_it_gauss--;
00400         Gaussian filtered = *posteriors_it_gauss;
00401         backwardfilter->Update(&sys_model,input, &filtered);
00402         break;}
00403       case BOOTSTRAP:{
00404         posteriors_it_mcpdf--;
00405         MCPdf<ColumnVector> filtered = *posteriors_it_mcpdf;
00406         cout << "before update" << endl;
00407         backwardfilter->Update(&sys_model,input, &filtered);
00408         cout << "after update" << endl;
00409         break;}
00410       default:{
00411         cout << "Type if filter not recognised on construction" <<endl;
00412         return 0 ;}
00413         }
00414       // UPDATE  BACKWARDFILTER
00415       Pdf<ColumnVector> * posterior = backwardfilter->PostGet();
00416 
00417       fout_time_smooth << time_step << ";" << endl;
00418       // write posterior to file
00419       fout_E_smooth << posterior->ExpectedValueGet()(1) << "," << posterior->ExpectedValueGet()(2)<< ","
00420              << posterior->ExpectedValueGet()(3) << ";"  << endl;
00421       fout_cov_smooth << posterior->CovarianceGet()(1,1) << "," << posterior->CovarianceGet()(1,2) << ","
00422                << posterior->CovarianceGet()(1,3) << "," << posterior->CovarianceGet()(2,2) << ","
00423                << posterior->CovarianceGet()(2,3) << "," << posterior->CovarianceGet()(3,3) << ";" << endl;
00424       // write particles to file
00425       if (smoother_name == BOOTSTRAP)
00426           {
00427             fout_numparticles_smooth << ((MCPdf<ColumnVector>*)posterior)->NumSamplesGet() << ";" << endl;
00428             vector<WeightedSample<ColumnVector> > samples_list = ((MCPdf<ColumnVector>*)posterior)->ListOfSamplesGet() ;
00429             vector<WeightedSample<ColumnVector> >::iterator sample;
00430             for ( sample = samples_list.begin() ; sample != samples_list.end() ; sample++ )
00431                 fout_particles_smooth << sample->ValueGet()(1) << "," << sample->ValueGet()(2) << ","
00432                                << sample->ValueGet()(3) << "," <<  sample->WeightGet() <<";"<<endl;
00433           }
00434 
00435       // make copy of posterior
00436       //*posteriors_it = (Gaussian)(*posterior);
00437       switch (smoother_name){
00438       case KALMAN:{
00439         *posteriors_it_gauss = *(Gaussian*)(posterior);
00440         break;}
00441       case BOOTSTRAP:{
00442         *posteriors_it_mcpdf = *(MCPdf<ColumnVector>*)(posterior);
00443         break;}
00444       default:{
00445         cout << "Type if filter not recognised on construction" <<endl;
00446         return 0 ;}
00447         }
00448 
00449     } // estimation loop
00450 
00451 
00452   // delete the filter
00453   delete filter;
00454   delete backwardfilter;
00455 
00456   cout << "======================================================" << endl
00457        << "End of the backward filter for mobile robot localisation" << endl
00458        << "======================================================"
00459        << endl;
00460 
00461   /****************************
00462    * CLOSE FILESTREAMS
00463    ***************************/
00464   fout_time.close();
00465   fout_E.close();
00466   fout_cov.close();
00467   fout_meas.close();
00468   fout_states.close();
00469   fout_time_smooth.close();
00470   fout_E_smooth.close();
00471   fout_cov_smooth.close();
00472   if (smoother_name  == BOOTSTRAP )
00473     {
00474       fout_particles.close();
00475       fout_numparticles.close();
00476       fout_particles_smooth.close();
00477       fout_numparticles_smooth.close();
00478     }
00479   return 0;
00480 }


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 Fri Aug 28 2015 10:10:21