test_compare_filters.cpp

一个很全的matlab工具包

// $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 ColumnVector

int 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;
}