// $Id: test_compare_filters.cpp,v 1.11 2005/01/28 16:52:32 kgadeyne Exp $// Copyright (C) 2003 Klaas Gadeyne <klaas dot gadeyne at mech dot kuleuven dot ac dot be>//  // This program is free software; you can redistribute it and/or modify// it under the terms of the GNU General Public License as published by// the Free Software Foundation; either version 2 of the License, or// (at your option) any later version.//  // This program is distributed in the hope that it will be useful,// but WITHOUT ANY WARRANTY; without even the implied warranty of// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the// GNU General Public License for more details.//  // You should have received a copy of the GNU General Public License// along with this program; if not, write to the Free Software// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA./* Demonstration program for the Bayesian Filtering Library.   Mobile robot localization with respect to wall with basic bootstrapfilter*/#include <filter/bootstrapfilter.h>#include <filter/extendedkalmanfilter.h>#include <filter/iteratedextendedkalmanfilter.h>#include <filter/asirfilter.h>#include <filter/EKparticlefilter.h>#include <model/linearanalyticsystemmodel_gaussianuncertainty.h>#include <model/linearanalyticmeasurementmodel_gaussianuncertainty.h>#include <pdf/analyticconditionalgaussian.h>#include <pdf/gaussian.h>#include <matrix_wrapper.h>#include <vector_wrapper.h>#include <iostream>#include <fstream>// Include system and measurement models#include "mobile_robot_particlefilter.h"using namespace MatrixWrapper;#define CV ColumnVectorint main(){  cerr << "==================================================\n"       << "Test of switching between different filters\n"       << "Mobile robot localisation example\n"       << "=================================================="       << endl;  // Change this to the filter you want  // #define _BOOTSTRAPFILTER_  #define _KALMANFILTER_  // #define _ASIRFILTER_  // #define _IEKF_  // #define _EK_PARTICLEFILTER_  init_system_model();  init_measurement_model();  /*****************   * prior DENSITY *   *****************/  init_prior();  cout << "Prior Mean = \n" << Prior_cont->ExpectedValueGet()       << "\nCovariance = \n" << Prior_cont->CovarianceGet() << "\n";  // Simulate measurements  simulate_measurements(DEFAULT_NUM_TIME_STEPS,			DEFAULT_LINEAR_SPEED,			DEFAULT_ROT_SPEED);  /******************************   * Construction of the Filter *   ******************************/#ifdef _EK_PARTICLEFILTER_  cerr << "Using the Extended Particle Kalman Filter, " << DEFAULT_ROT_SPEED << " samples, dynamic resampling" << endl;  init_mc_prior(DEFAULT_NUM_SAMPLES);  filter_p = new EKParticleFilter(Prior, 0, DEFAULT_RESAMPLE_THRESHOLD,				  DEFAULT_RS);#endif // _EK_PARTICLEFILTER_#ifdef _BOOTSTRAPFILTER_  cerr << "Using the bootstrapfilter, " << DEFAULT_NUM_SAMPLES << " samples, dynamic resampling" << endl;  init_mc_prior(DEFAULT_NUM_SAMPLES);  filter_p = new BootstrapFilter<CV,CV> (Prior, 0,					 DEFAULT_RESAMPLE_THRESHOLD,					 DEFAULT_RS);  /* Static resampling */  // filter_p = new BootstrapFilter<CV,CV> (Prior, DEFAULT_RESAMPLE_PERIOD, 0, DEFAULT_RS);#endif // _BOOTSTRAPFILTER_#ifdef _ASIRFILTER_  cerr << "Using the ASIR-filter, " << DEFAULT_NUM_SAMPLES << " samples, fixed period resampling" << endl;  init_mc_prior(DEFAULT_NUM_SAMPLES);  filter_p = new ASIRFilter<CV,CV> (Prior, 				    DEFAULT_RESAMPLE_PERIOD, 0, DEFAULT_RS);#endif // _ASIRFILTER_#ifdef _KALMANFILTER_  cerr << "Using the Extended Kalman Filter" << endl;  filter_p = new ExtendedKalmanFilter(Prior_cont);#endif // _KALMANFILTER_#ifdef _IEKF_  cerr << "Using the Iterated Extended Kalman Filter, " << NUM_ITERATIONS << " iterations" << endl;#define NUM_ITERATIONS 3  filter_p = new IteratedExtendedKalmanFilter(Prior_cont,NUM_ITERATIONS);#endif // _IEKF_  /*******************   * ESTIMATION LOOP *   *******************/  cout << "MAIN: Starting estimation" << endl;  for (current_time = 0;        current_time < DEFAULT_NUM_TIME_STEPS-1 ;        current_time++)    {      // UPDATE FILTER      update_matrix_B(current_time);      filter_p->Update(sys_model,Inputs[current_time],meas_model,Measurements[current_time+1]);    }      post_p = filter_p->PostGet();      cout << "After " << DEFAULT_NUM_TIME_STEPS << " timesteps " << endl;      cout << "Posterior Mean = \n" << post_p->ExpectedValueGet()	   << "Covariance = \n" << post_p->CovarianceGet() << "\n";      cout << "=============================================\n";      // Experimental code for now...#define NUM_RUNS 0  for (int i = 0; i < NUM_RUNS; i++)    {#ifdef _BOOTSTRAPFILTER_      // Should init printer since we pass pointer to filter.reset()      // (pointer is altered, should implement a cleaner solution for      // this reset!)      init_prior(); init_mc_prior(DEFAULT_NUM_SAMPLES);      filter_p->Reset(Prior);#endif#ifdef _ASIRFILTER_      init_prior(); init_mc_prior(DEFAULT_NUM_SAMPLES);      filter_p->Reset(Prior);#endif // _ASIRFILTER_#ifdef _KALMANFILTER_      init_prior();      cout << "Prior Mean = \n" << Prior_cont->ExpectedValueGet()	   << "Covariance = \n" << Prior_cont->CovarianceGet() << "\n";      filter_p->Reset(Prior_cont);#endif // _KALMANFILTER_#ifdef _IEKF_      init_prior();      filter_p->Reset(Prior_cont);#endif // _IEKF_      init_system_model();      init_measurement_model();            for (current_time = 0; 	   current_time < DEFAULT_NUM_TIME_STEPS-1 ; 	   current_time++)	{	  // UPDATE FILTER	  update_matrix_B(current_time);	  filter_p->Update(sys_model,Inputs[current_time],meas_model,Measurements[current_time+1]);      post_p = filter_p->PostGet();      cout << "Posterior Mean = \n" << post_p->ExpectedValueGet()	   << "Covariance = \n" << post_p->CovarianceGet() << "\n";	}      cout << "=============================================\n";    }    // Cleanup dynamically allocated memory  cleanup();#ifdef _EK_PARTICLEFILTER_  cleanup_mc_prior();#endif // _EK_PARTICLEFILTER_#ifdef _BOOTSTRAPFILTER_  cleanup_mc_prior();#endif // _BOOTSTRAPFILTER_#ifdef _ASIRFILTER_  cleanup_mc_prior();#endif // _ASIRFILTER_  delete filter_p;    cerr << "==================================================\n"       << "End of the Comparing filters test\n"       << "=================================================="       << endl;  return 0;}