complete_filter_test.cpp
Go to the documentation of this file.
00001 // Copyright (C) 2007 Wim Meeussen <wim DOT meeussen AT mech DOT kuleuven DOT be>
00002 // Copyright (C) 2008 Tinne De Laet <tinne DOT delaet 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 General Public License as published by
00006 // the Free Software Foundation; either version 2 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 General Public License for more details.
00013 //
00014 // You should have received a copy of the GNU 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 
00019 
00020 #include "complete_filter_test.hpp"
00021 #include "approxEqual.hpp"
00022 
00023 
00024 
00025 
00026 // Registers the fixture into the 'registry'
00027 CPPUNIT_TEST_SUITE_REGISTRATION( Complete_FilterTest );
00028 
00029 using namespace MatrixWrapper;
00030 using namespace BFL;
00031 
00032 
00033 void
00034 Complete_FilterTest::setUp()
00035 {
00036 }
00037 
00038 
00039 void
00040 Complete_FilterTest::tearDown()
00041 {
00042 }
00043 
00044 void
00045 Complete_FilterTest::testComplete_FilterValue_Cont()
00046 {
00047   double epsilon       = 0.015;
00048   double epsilon_large = 0.5;
00049   double epsilon_huge  = 2.0;
00050 
00051   /****************************
00052    * Initialise system model *
00053    ***************************/
00054   ColumnVector SysNoise_Mu(STATE_SIZE);
00055   SysNoise_Mu = 0.0;
00056   SysNoise_Mu(1) = MU_SYSTEM_NOISE_X;
00057   SysNoise_Mu(2) = MU_SYSTEM_NOISE_Y;
00058   SysNoise_Mu(3) = MU_SYSTEM_NOISE_THETA;
00059 
00060   SymmetricMatrix SysNoise_Cov(STATE_SIZE);
00061   SysNoise_Cov = 0.0;
00062   // Uncertainty or Noice (Additive) and Matrix A
00063   SysNoise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
00064   SysNoise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
00065   SysNoise_Cov(3,3) = SIGMA_SYSTEM_NOISE_THETA;
00066 
00067   Gaussian System_Uncertainty(SysNoise_Mu, SysNoise_Cov);
00068   NonLinearAnalyticConditionalGaussianMobile sys_pdf(System_Uncertainty);
00069   AnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);
00070 
00071 
00072   /*********************************
00073    * Initialise measurement model *
00074    ********************************/
00075   // Fill up H
00076   double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
00077   Matrix H(MEAS_SIZE,STATE_SIZE);
00078   H = 0.0;
00079   H(1,1) = wall_ct * RICO_WALL;
00080   H(1,2) = 0 - wall_ct;
00081 
00082   // Construct the measurement noise (a scalar in this case)
00083   ColumnVector MeasNoise_Mu(MEAS_SIZE);
00084   SymmetricMatrix MeasNoise_Cov(MEAS_SIZE);
00085   MeasNoise_Mu(1) = MU_MEAS_NOISE;
00086   MeasNoise_Cov(1,1) = SIGMA_MEAS_NOISE;
00087 
00088   Gaussian Measurement_Uncertainty(MeasNoise_Mu,MeasNoise_Cov);
00089   LinearAnalyticConditionalGaussian meas_pdf(H,Measurement_Uncertainty);
00090   LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);
00091 
00092   /****************************
00093    * Initialise prior DENSITY *
00094    ***************************/
00095   // Continuous Gaussian prior (for Kalman filters)
00096   ColumnVector prior_mu(STATE_SIZE);
00097   SymmetricMatrix prior_sigma(STATE_SIZE);
00098   prior_mu(1) = PRIOR_MU_X;
00099   prior_mu(2) = PRIOR_MU_Y;
00100   prior_mu(STATE_SIZE) = PRIOR_MU_THETA;
00101   prior_sigma = 0.0;
00102   prior_sigma(1,1) = PRIOR_COV_X;
00103   prior_sigma(2,2) = PRIOR_COV_Y;
00104   prior_sigma(3,3) = PRIOR_COV_THETA;
00105   Gaussian prior_cont(prior_mu,prior_sigma);
00106 
00107   // Discrete prior for Particle filter (using the continuous Gaussian prior)
00108   vector<Sample<ColumnVector> > prior_samples(NUM_SAMPLES);
00109   MCPdf<ColumnVector> prior_discr(NUM_SAMPLES,STATE_SIZE);
00110   prior_cont.SampleFrom(prior_samples,NUM_SAMPLES,CHOLESKY,NULL);
00111   prior_discr.ListOfSamplesSet(prior_samples);
00112 
00113   // Mixture prior for the Mixture Boostrap filter
00114   ColumnVector prior_mu1(STATE_SIZE);
00115   SymmetricMatrix prior_sigma1(STATE_SIZE);
00116   prior_mu1(1) = PRIOR_MU_X1;
00117   prior_mu1(2) = PRIOR_MU_Y1;
00118   prior_mu1(STATE_SIZE) = PRIOR_MU_THETA1;
00119   prior_sigma1 = 0.0;
00120   prior_sigma1(1,1) = PRIOR_COV_X1;
00121   prior_sigma1(2,2) = PRIOR_COV_Y1;
00122   prior_sigma1(3,3) = PRIOR_COV_THETA1;
00123   Gaussian prior_cont1(prior_mu1,prior_sigma1);
00124 
00125   MCPdf<ColumnVector> mixcomp1(NUM_SAMPLES,STATE_SIZE);
00126   prior_cont1.SampleFrom(prior_samples,NUM_SAMPLES,CHOLESKY,NULL);
00127   mixcomp1.ListOfSamplesSet(prior_samples);
00128 
00129   ColumnVector prior_mu2(STATE_SIZE);
00130   SymmetricMatrix prior_sigma2(STATE_SIZE);
00131   prior_mu2(1) = PRIOR_MU_X2;
00132   prior_mu2(2) = PRIOR_MU_Y2;
00133   prior_mu2(STATE_SIZE) = PRIOR_MU_THETA2;
00134   prior_sigma2 = 0.0;
00135   prior_sigma2(1,1) = PRIOR_COV_X2;
00136   prior_sigma2(2,2) = PRIOR_COV_Y2;
00137   prior_sigma2(3,3) = PRIOR_COV_THETA2;
00138   Gaussian prior_cont2(prior_mu2,prior_sigma2);
00139 
00140   MCPdf<ColumnVector> mixcomp2(NUM_SAMPLES,STATE_SIZE);
00141   prior_cont2.SampleFrom(prior_samples,NUM_SAMPLES,CHOLESKY,NULL);
00142   mixcomp2.ListOfSamplesSet(prior_samples);
00143 
00144   ColumnVector prior_mu3(STATE_SIZE);
00145   SymmetricMatrix prior_sigma3(STATE_SIZE);
00146   prior_mu3(1) = PRIOR_MU_X3;
00147   prior_mu3(2) = PRIOR_MU_Y3;
00148   prior_mu3(STATE_SIZE) = PRIOR_MU_THETA3;
00149   prior_sigma3 = 0.0;
00150   prior_sigma3(1,1) = PRIOR_COV_X3;
00151   prior_sigma3(2,2) = PRIOR_COV_Y3;
00152   prior_sigma3(3,3) = PRIOR_COV_THETA3;
00153   Gaussian prior_cont3(prior_mu3,prior_sigma3);
00154 
00155   MCPdf<ColumnVector> mixcomp3(NUM_SAMPLES,STATE_SIZE);
00156   prior_cont3.SampleFrom(prior_samples,NUM_SAMPLES,CHOLESKY,NULL);
00157   mixcomp3.ListOfSamplesSet(prior_samples);
00158 
00159   ColumnVector prior_mu4(STATE_SIZE);
00160   SymmetricMatrix prior_sigma4(STATE_SIZE);
00161   prior_mu4(1) = PRIOR_MU_X4;
00162   prior_mu4(2) = PRIOR_MU_Y4;
00163   prior_mu4(STATE_SIZE) = PRIOR_MU_THETA3;
00164   prior_sigma4 = 0.0;
00165   prior_sigma4(1,1) = PRIOR_COV_X4;
00166   prior_sigma4(2,2) = PRIOR_COV_Y4;
00167   prior_sigma4(3,3) = PRIOR_COV_THETA4;
00168   Gaussian prior_cont4(prior_mu4,prior_sigma4);
00169 
00170   MCPdf<ColumnVector> mixcomp4(NUM_SAMPLES,STATE_SIZE);
00171   prior_cont4.SampleFrom(prior_samples,NUM_SAMPLES,CHOLESKY,NULL);
00172   mixcomp4.ListOfSamplesSet(prior_samples);
00173 
00174   vector<Pdf<ColumnVector>*> mixVec(3);
00175   mixVec[0] = &mixcomp1;
00176   mixVec[1] = &mixcomp2;
00177   mixVec[2] = &mixcomp3;
00178   //mixVec[3] = &mixcomp4;
00179   Mixture<ColumnVector> prior_mix(mixVec);
00180 
00181   // check
00182   ColumnVector mean_check(STATE_SIZE);
00183   mean_check(1) = PRIOR_MU_X; mean_check(2) = PRIOR_MU_Y; mean_check(3) = PRIOR_MU_THETA;
00184   SymmetricMatrix cov_check(STATE_SIZE);
00185   cov_check(1,1) = PRIOR_COV_X; cov_check(1,2) = 0; cov_check(1,3) = 0;
00186   cov_check(2,1) = 0; cov_check(2,2) = PRIOR_COV_Y; cov_check(2,3) = 0;
00187   cov_check(3,1) = 0; cov_check(3,2) = 0; cov_check(3,3) = PRIOR_COV_THETA;
00188   CPPUNIT_ASSERT_EQUAL(approxEqual(prior_cont.ExpectedValueGet(), mean_check, epsilon),true);
00189   CPPUNIT_ASSERT_EQUAL(approxEqual(prior_cont.CovarianceGet(), cov_check, epsilon),true);
00190 
00191 
00192   /***************************
00193    * initialise MOBILE ROBOT *
00194    **************************/
00195   // Model of mobile robot in world
00196   // The model is used to simultate the distance measurements.
00197   MobileRobot mobile_robot;
00198   ColumnVector input(INPUT_SIZE);
00199   input(1) = LIN_SPEED * DELTA_T;
00200   input(2)  = ROT_SPEED * DELTA_T;
00201 
00202 
00203   /******************************
00204    * Construction of the Filter *
00205    ******************************/
00206   Filter<ColumnVector,ColumnVector> *my_filter_extendedkalman, *my_filter_iteratedextendedkalman, *my_filter_bootstrap, *my_filter_ekparticle, *my_filter_mixtureBootstrap;
00207   my_filter_extendedkalman = new ExtendedKalmanFilter(&prior_cont);
00208   my_filter_iteratedextendedkalman = new IteratedExtendedKalmanFilter(&prior_cont,NUM_ITERATIONS);
00209   my_filter_bootstrap = new BootstrapFilter<ColumnVector,ColumnVector> (&prior_discr, RESAMPLE_PERIOD, RESAMPLE_THRESHOLD);
00210   my_filter_ekparticle = new EKParticleFilter(&prior_discr, 0, RESAMPLE_THRESHOLD);
00211   my_filter_mixtureBootstrap = new MixtureBootstrapFilter<ColumnVector,ColumnVector> (&prior_mix, RESAMPLE_PERIOD, RESAMPLE_THRESHOLD);
00212 
00213   /*******************
00214    * ESTIMATION LOOP *
00215    *******************/
00216   ColumnVector measurement ;
00217   ColumnVector mobile_robot_state ;
00218   Pdf<ColumnVector> * posterior_mixtureBootstrap;
00219   ofstream mixtureFile;
00220   if(OUTPUT_MIXTURE)
00221   {
00222     mixtureFile.open("mixtureOutput.txt");
00223   }
00224 
00225   cout << "Running 4 different filters. This may take a few minutes... " << endl;
00226   unsigned int time_step;
00227   for (time_step = 0; time_step < NUM_TIME_STEPS-1; time_step++)
00228     {
00229       // DO ONE STEP WITH MOBILE ROBOT
00230       mobile_robot.Move(input);
00231 
00232       // DO ONE MEASUREMENT
00233       measurement = mobile_robot.Measure();
00234       mobile_robot_state = mobile_robot.GetState();
00235 
00236       if(OUTPUT_MIXTURE)
00237       {
00238         posterior_mixtureBootstrap = my_filter_mixtureBootstrap->PostGet();
00239         vector<WeightedSample<ColumnVector> > los;
00240         vector<WeightedSample<ColumnVector> >::iterator los_it;
00241         int numComp = dynamic_cast<Mixture<ColumnVector> *>(posterior_mixtureBootstrap)->NumComponentsGet();
00242         mixtureFile << time_step << " " << numComp << " ";  
00243         mixtureFile << mobile_robot_state(1) << " " << mobile_robot_state(2) << " " << mobile_robot_state(3) << " ";  
00244         for(int i = 0 ; i<numComp ; i++ )
00245         {   
00246             double componentWeight = ( dynamic_cast<Mixture<ColumnVector> *>(posterior_mixtureBootstrap)->WeightGet(i)) ;
00247             los = dynamic_cast<MCPdf<ColumnVector> *>( dynamic_cast<Mixture<ColumnVector> *>(posterior_mixtureBootstrap)->ComponentGet(i))->ListOfSamplesGet(); 
00248             mixtureFile << i << " " << componentWeight << " " << los.size()<< " " << STATE_SIZE << " ";  
00249             for ( los_it=los.begin(); los_it != los.end() ; los_it++)
00250             {
00251                 for (int j=0; j<STATE_SIZE ; j++)
00252                     mixtureFile << los_it->ValueGet()[j] << " ";
00253                 mixtureFile<< los_it->WeightGet() << " ";  
00254             } 
00255         }
00256         mixtureFile<<endl;
00257        }
00258       // UPDATE FILTER
00259       my_filter_extendedkalman->Update(&sys_model,input,&meas_model, measurement);
00260       my_filter_iteratedextendedkalman->Update(&sys_model,input,&meas_model, measurement);
00261       my_filter_bootstrap->Update(&sys_model,input,&meas_model, measurement);
00262       //my_filter_ekparticle->Update(&sys_model,input,&meas_model, measurement);
00263       my_filter_mixtureBootstrap->Update(&sys_model,input,&meas_model, measurement);
00264     }
00265 
00266     if(OUTPUT_MIXTURE)
00267     {
00268       posterior_mixtureBootstrap = my_filter_mixtureBootstrap->PostGet();
00269       vector<WeightedSample<ColumnVector> > los;
00270       vector<WeightedSample<ColumnVector> >::iterator los_it;
00271       int numComp = dynamic_cast<Mixture<ColumnVector> *>(posterior_mixtureBootstrap)->NumComponentsGet();
00272       mixtureFile << time_step << " " << numComp << " ";  
00273       mixtureFile << mobile_robot_state(1) << " " << mobile_robot_state(2) << " " << mobile_robot_state(3) << " ";  
00274       for(int i = 0 ; i<numComp ; i++ )
00275       {   
00276           double componentWeight = ( dynamic_cast<Mixture<ColumnVector> *>(posterior_mixtureBootstrap)->WeightGet(i)) ;
00277           los = dynamic_cast<MCPdf<ColumnVector> *>( dynamic_cast<Mixture<ColumnVector> *>(posterior_mixtureBootstrap)->ComponentGet(i))->ListOfSamplesGet(); 
00278           mixtureFile << i << " " << componentWeight << " " << los.size()<< " " << STATE_SIZE << " ";  
00279           for ( los_it=los.begin(); los_it != los.end() ; los_it++)
00280           {
00281               for (int j=0; j<STATE_SIZE ; j++)
00282                   mixtureFile << los_it->ValueGet()[j] << " ";
00283               mixtureFile<< los_it->WeightGet() << " ";  
00284           } 
00285       }
00286       mixtureFile<<endl;
00287      }
00288 
00289 
00290   // ek_check
00291   Pdf<ColumnVector> * posterior_extendedkalman = my_filter_extendedkalman->PostGet();
00292   ColumnVector mean_ek_check(STATE_SIZE);
00293   mean_ek_check=mobile_robot.GetState();
00294   //mean_ek_check(1) = mobile_robot_state(1); mean_ek_check(2) = mobile_robot_state(2); mean_ek_check(3) = mobile_robot_state(3);
00295   SymmetricMatrix cov_ek_check(STATE_SIZE);
00296   cov_ek_check(1,1) = 0.0599729;   cov_ek_check(1,2) = 0.000291386; cov_ek_check(1,3) = 0.00223255;
00297   cov_ek_check(2,1) = 0.000291386; cov_ek_check(2,2) = 0.000277528; cov_ek_check(2,3) = 0.000644136;
00298   cov_ek_check(3,1) = 0.00223255;  cov_ek_check(3,2) = 0.000644136; cov_ek_check(3,3) = 0.00766009;
00299   CPPUNIT_ASSERT_EQUAL(approxEqual(posterior_extendedkalman->ExpectedValueGet(), mean_ek_check, epsilon_large),true);
00300   CPPUNIT_ASSERT_EQUAL(approxEqual(posterior_extendedkalman->CovarianceGet(), cov_ek_check, epsilon),true);
00301 
00302   // it_check
00303   Pdf<ColumnVector> * posterior_iteratedextendedkalman = my_filter_iteratedextendedkalman->PostGet();
00304   ColumnVector mean_it_check(STATE_SIZE);
00305   mean_it_check=mobile_robot.GetState();
00306   //mean_it_check(1) = mobile_robot_state(1); mean_it_check(2) = mobile_robot_state(2); mean_it_check(3) = mobile_robot_state(3);
00307   SymmetricMatrix cov_it_check(STATE_SIZE);
00308   cov_it_check = 0.0;
00309   cov_it_check(1,1) = 0.0611143;   cov_it_check(1,2) = 0.000315923; cov_it_check(1,3) = 0.00238938;
00310   cov_it_check(2,1) = 0.000315923; cov_it_check(2,2) = 0.000280736; cov_it_check(2,3) = 0.000665735;
00311   cov_it_check(3,1) = 0.00238938;  cov_it_check(3,2) = 0.000665735; cov_it_check(3,3) = 0.00775776;
00312   CPPUNIT_ASSERT_EQUAL(approxEqual(posterior_iteratedextendedkalman->ExpectedValueGet(), mean_it_check, epsilon_large),true);
00313   CPPUNIT_ASSERT_EQUAL(approxEqual(posterior_iteratedextendedkalman->CovarianceGet(), cov_it_check, epsilon),true);
00314 
00315   // bs_check
00316   Pdf<ColumnVector> * posterior_bootstrap = my_filter_bootstrap->PostGet();
00317   ColumnVector mean_bs_check(STATE_SIZE);
00318   mean_bs_check=mobile_robot.GetState();
00319   //mean_bs_check(1) = mobile_robot_state(1); mean_bs_check(2) = mobile_robot_state(2); mean_bs_check(3) = mobile_robot_state(3);
00320   SymmetricMatrix cov_bs_check(STATE_SIZE);
00321   cov_bs_check = 0.0;
00322   cov_bs_check(1,1) = PRIOR_COV_X;   
00323   CPPUNIT_ASSERT_EQUAL(approxEqual(posterior_bootstrap->ExpectedValueGet(), mean_bs_check, epsilon_large),true);
00324   CPPUNIT_ASSERT_EQUAL(approxEqual(posterior_bootstrap->CovarianceGet(), cov_bs_check, epsilon),true);
00325 
00326   // ep_check
00327   /*
00328   Pdf<ColumnVector> * posterior_ekparticle = my_filter_ekparticle->PostGet();
00329   cout << " Posterior Mean = " << endl << posterior_ekparticle->ExpectedValueGet() << endl
00330        << " Covariance = " << endl << posterior_ekparticle->CovarianceGet() << "" << endl;
00331   ColumnVector mean_ep_check(STATE_SIZE);
00332   //mean_ep_check(1) = 6.64581; mean_ep_check(2) = -7.05499; mean_ep_check(3) = -0.76974;
00333   mean_ep_check=mobile_robot.GetState();
00334   SymmetricMatrix cov_ep_check(STATE_SIZE);
00335   cov_ep_check(1,1) = 0.0160492;   cov_ep_check(1,2) = 0.000193798; cov_ep_check(1,3) = 0.0013101;
00336   cov_ep_check(2,1) = 0.000193798; cov_ep_check(2,2) = 0.000289425; cov_ep_check(2,3) = 0.000701263;
00337   cov_ep_check(3,1) = 0.0013101;   cov_ep_check(3,2) = 0.000701263; cov_ep_check(3,3) = 0.00682061;
00338   cout << "mean_ep_check " << mean_ep_check << endl;
00339   cout << "cov_ep_check " << cov_ep_check << endl;
00340   CPPUNIT_ASSERT_EQUAL(approxEqual(posterior_ekparticle->ExpectedValueGet(), mean_ep_check, epsilon_huge),true);
00341   CPPUNIT_ASSERT_EQUAL(approxEqual(posterior_ekparticle->CovarianceGet(), cov_ep_check, epsilon_large),true);
00342   */
00343   // mixtureBoostrapFilter check
00344   posterior_mixtureBootstrap = my_filter_mixtureBootstrap->PostGet();
00345   ColumnVector mean_mbs_check(STATE_SIZE);
00346   //mean_mbs_check(1) = 6.64581; mean_mbs_check(2) = -7.05499; mean_mbs_check(3) = -0.76974;
00347   mean_mbs_check(1) = mobile_robot_state(1); mean_mbs_check(2) = mobile_robot_state(2); mean_mbs_check(3) = mobile_robot_state(3);
00348   //cout << "mixture weights:" << endl;
00349   vector<Probability> weights= dynamic_cast<Mixture<ColumnVector> *>(posterior_mixtureBootstrap)->WeightsGet();
00350   ColumnVector exp;
00351   for(int i = 0 ; i< dynamic_cast<Mixture<ColumnVector> *>(posterior_mixtureBootstrap)->NumComponentsGet(); i++ )
00352   {
00353         //cout << "weight component " << i << ": " << weights[i] << endl;
00354         exp= dynamic_cast<Mixture<ColumnVector> *>(posterior_mixtureBootstrap)->ComponentGet(i)->ExpectedValueGet();
00355         //cout << "expected value component " << i << ": " << exp << endl;
00356   }
00357   //cout << "expected value total: " << posterior_mixtureBootstrap->ExpectedValueGet() << endl;
00358   //cout << "should be : " << mean_mbs_check << endl;
00359   CPPUNIT_ASSERT_EQUAL(approxEqual(posterior_mixtureBootstrap->ExpectedValueGet(), mean_mbs_check, epsilon_huge),true);
00360 
00361   // closing file stream
00362   if(OUTPUT_MIXTURE)
00363   {
00364     mixtureFile.close();
00365   }
00366 
00367   // delete the filters
00368   delete my_filter_extendedkalman;
00369   delete my_filter_iteratedextendedkalman;
00370   delete my_filter_bootstrap;
00371   delete my_filter_ekparticle;
00372   delete my_filter_mixtureBootstrap;
00373 }
00374 
00375 void
00376 Complete_FilterTest::testComplete_FilterValue_Discr()
00377 {
00378   int num_states = 20;
00379   int num_cond_args = 1;
00380   /****************************
00381   * Discrete system model      *
00382   ***************************/
00383   int cond_arg_dims[num_cond_args];
00384   cond_arg_dims[0] = num_states;
00385   DiscreteConditionalPdf sys_pdf(num_states,num_cond_args,cond_arg_dims);  // no  inputs
00386   std::vector<int> cond_args(num_cond_args);
00387   double prob_diag = 0.9;
00388   double prob_nondiag = (1-prob_diag)/(num_states-1);
00389   for (int state_kMinusOne = 0 ; state_kMinusOne < num_states ;  state_kMinusOne++)
00390     {
00391        cond_args[0] = state_kMinusOne;
00392        for (int state_k = 0 ; state_k < num_states ;  state_k++)
00393          {
00394            if (state_kMinusOne == state_k) sys_pdf.ProbabilitySet(prob_diag,state_k,cond_args);
00395            else sys_pdf.ProbabilitySet(prob_nondiag,state_k,cond_args);
00396          }
00397     }
00398   DiscreteSystemModel sys_model(&sys_pdf);
00399 
00400   /*********************************
00401    * Initialise measurement model *
00402    ********************************/
00403 
00404   // Construct the measurement noise (a scalar in this case)
00405   ColumnVector measNoise_Mu(MEAS_SIZE);
00406   measNoise_Mu(1) = MU_MEAS_NOISE;
00407 
00408   SymmetricMatrix measNoise_Cov(MEAS_SIZE);
00409   measNoise_Cov(1,1) = SIGMA_MEAS_NOISE;
00410   Gaussian measurement_uncertainty(measNoise_Mu, measNoise_Cov);
00411 
00412   // create the model
00413   ConditionalUniformMeasPdf1d meas_pdf(measurement_uncertainty);
00414   MeasurementModel<MatrixWrapper::ColumnVector,int> meas_model(&meas_pdf);
00415 
00416   /****************************
00417   * Uniform prior DENSITY     *
00418   ***************************/
00419   DiscretePdf prior(num_states); //equal probability is set for all classed
00420 
00421   /******************************
00422    * Construction of the Filter *
00423    ******************************/
00424   HistogramFilter<ColumnVector> filter(&prior);
00425   DiscretePdf * prior_test = filter.PostGet();
00426 
00427   /***************************
00428    * initialise MOBILE ROBOT *
00429    **************************/
00430   // Model of mobile robot in world with one wall
00431   // The model is used to simultate the distance measurements.
00432   MobileRobot mobile_robot;
00433   ColumnVector input(2);
00434   input(1) = 0.1;
00435   input(2) = 0.0;
00436 
00437 
00438   /*******************
00439    * ESTIMATION LOOP *
00440    *******************/
00441   unsigned int time_step;
00442   for (time_step = 0; time_step < NUM_TIME_STEPS-1; time_step++)
00443     {
00444       // DO ONE STEP WITH MOBILE ROBOT
00445       mobile_robot.Move(input);
00446       // DO ONE MEASUREMENT
00447       ColumnVector measurement = mobile_robot.Measure();
00448       // change sign of measurement (measurement model returns negative value)
00449       measurement(1) = 0-measurement(1);
00450       // UPDATE FILTER
00451       filter.Update(&sys_model,&meas_model,measurement);
00452     } // estimation loop
00453 
00454   DiscretePdf *  posterior = filter.PostGet();
00455   for (int state=0; state< num_states; state++)
00456   {
00457     //cout << state << ": " << posterior->ProbabilityGet(state) << endl;
00458     if (state== (int)(mobile_robot.GetState()(2)) ) CPPUNIT_ASSERT(posterior->ProbabilityGet(state) >0.9);
00459     else CPPUNIT_ASSERT(posterior->ProbabilityGet(state) <0.1);
00460   }
00461 }


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:52