test_discrete_filter.cpp
Go to the documentation of this file.
00001 // $Id: test_discrete_filter.cpp tdelaet $
00002 // Copyright (C) 2007 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 // Include file with properties
00023 #include "../mobile_robot_wall_cts.h"
00024 
00025 #include <filter/histogramfilter.h>
00026 
00027 #include <model/discretesystemmodel.h>
00028 
00029 #include <pdf/discretepdf.h>
00030 #include "conditionalUniformMeasPdf1d.h"
00031 #include "../mobile_robot.h"
00032 
00033 #include <iostream>
00034 #include <fstream>
00035 
00036 using namespace MatrixWrapper;
00037 using namespace BFL;
00038 using namespace std;
00039 
00040 
00041 
00042 /* The purpose of this program is to construct a histogram filter for a very
00043    simple example, which is a 1d mobile robot localisation.
00044    Furthermore the mobile robot is equipped with an ultrasonic sensor which
00045    directly measures the robot's position.
00046 
00047   The necessary SYSTEM MODEL is discrete.
00048 
00049   The MEASUREMENT MODEL is a home made one which takes into account the discrete nature
00050   of the state under consideration and some extra gaussian measurement noise
00051 
00052 */
00053 
00054 
00055 int main(int argc, char** argv)
00056 {
00057   cerr << "==================================================" << endl
00058        << "Test of histogram filter" << endl
00059        << "1D Mobile robot localisation example" << endl
00060        << "==================================================" << endl;
00061 
00062 
00063   int num_states = 20;
00064   int num_cond_args = 1;
00065   /****************************
00066   * Discrete system model      *
00067   ***************************/
00068   int cond_arg_dims[num_cond_args];
00069   cond_arg_dims[0] = num_states;
00070   DiscreteConditionalPdf sys_pdf(num_states,num_cond_args,cond_arg_dims);  // no  inputs
00071   std::vector<int> cond_args(num_cond_args);
00072   double prob_diag = 0.9;
00073   double prob_nondiag = (1-prob_diag)/(num_states-1);
00074   for (int state_kMinusOne = 0 ; state_kMinusOne < num_states ;  state_kMinusOne++)
00075     {
00076        cond_args[0] = state_kMinusOne;
00077        for (int state_k = 0 ; state_k < num_states ;  state_k++)
00078          {
00079            if (state_kMinusOne == state_k) sys_pdf.ProbabilitySet(prob_diag,state_k,cond_args);
00080            else sys_pdf.ProbabilitySet(prob_nondiag,state_k,cond_args);
00081          }
00082     }
00083   DiscreteSystemModel sys_model(&sys_pdf);
00084   std::cerr << "discrete system model created" << std::endl;
00085 
00086   /*********************************
00087    * Initialise measurement model *
00088    ********************************/
00089   // Construct the measurement noise (a scalar in this case)
00090   ColumnVector measNoise_Mu(MEAS_SIZE);
00091   measNoise_Mu(1) = 0.0;
00092 
00093   SymmetricMatrix meas_noise_Cov(MEAS_SIZE);
00094   meas_noise_Cov(1,1) = SIGMA_MEAS_NOISE;
00095   ColumnVector meas_noise_Mu(1);
00096   meas_noise_Mu(1) = MU_MEAS_NOISE;
00097   Gaussian measurement_Uncertainty(meas_noise_Mu, meas_noise_Cov);
00098 
00099   // create the model
00100   ConditionalUniformMeasPdf1d meas_pdf(measurement_Uncertainty);
00101   MeasurementModel<MatrixWrapper::ColumnVector,int> meas_model(&meas_pdf);
00102   std::cout << "measurement model created" << std::endl;
00103 
00104   /****************************
00105   * Uniform prior DENSITY     *
00106   ***************************/
00107   DiscretePdf prior(num_states); //equal probability is set for all states
00108   std::cout << "uniform prior density created" << std::endl;
00109 
00110   /******************************
00111    * Construction of the Filter *
00112    ******************************/
00113   HistogramFilter<ColumnVector> filter(&prior);
00114   DiscretePdf * prior_test = filter.PostGet();
00115   std::cout << "filter created" << std::endl;
00116 
00117   /***************************
00118    * Initialise MOBILE ROBOT *
00119    **************************/
00120   // Model of mobile robot in world with one wall
00121   // The model is used to simultate the distance measurements.
00122   MobileRobot mobile_robot;
00123   ColumnVector input(2);
00124   input(1) = 0.1;
00125   input(2) = 0.0;
00126 
00127 
00128   /*******************
00129    * ESTIMATION LOOP *
00130    *******************/
00131   cout << "MAIN: Starting estimation" << endl;
00132   unsigned int time_step;
00133   for (time_step = 0; time_step < NUM_TIME_STEPS; time_step++)
00134     {
00135       // DO ONE STEP WITH MOBILE ROBOT
00136       mobile_robot.Move(input);
00137 
00138       // DO ONE MEASUREMENT
00139       // TODO: mobile_robot returns negative measurements
00140       ColumnVector measurement = mobile_robot.Measure();
00141       measurement(1) = -measurement(1);
00142 
00143       // UPDATE FILTER
00144       filter.Update(&sys_model,&meas_model,measurement);
00145       //filter.Update(&sys_model);
00146       //filter.Update(&meas_model,measurement);
00147 
00148     } // estimation loop
00149 
00150 
00151 
00152   DiscretePdf *  posterior = filter.PostGet();
00153   cout << "After " << time_step+1 << " timesteps " << endl;
00154   cout << " Posterior probabilities = " << endl;
00155   for (int state = 0 ;  state< posterior->NumStatesGet() ; state++) cout << state << ": " << (double)(posterior->ProbabilityGet(state)) << endl;
00156 
00157 
00158   cout << "======================================================" << endl
00159        << "End of the Histogram filter for 1D mobile robot localisation" << endl
00160        << "======================================================"
00161        << endl;
00162 
00163 
00164   return 0;
00165 }


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