test_nonlinear_particle.cpp
Go to the documentation of this file.
00001 // $Id: test_nonlinear_particle.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/bootstrapfilter.h>
00024 
00025 #include <model/systemmodel.h>
00026 #include <model/measurementmodel.h>
00027 
00028 #include "nonlinearSystemPdf.h"
00029 #include "nonlinearMeasurementPdf.h"
00030 
00031 #include "../mobile_robot.h"
00032 
00033 #include <iostream>
00034 #include <fstream>
00035 
00036 // Include file with properties
00037 #include "../mobile_robot_wall_cts.h"
00038 
00039 using namespace MatrixWrapper;
00040 using namespace BFL;
00041 using namespace std;
00042 
00043 
00044 
00045 /* The purpose of this program is to construct a kalman filter for the problem
00046   of localisation of a mobile robot equipped with an ultrasonic sensor.
00047   In this case the orientation is known, which simplifies the model considerably:
00048   The system model will become linear.
00049   The ultrasonic measures the distance to the wall (it can be switched off:
00050   see mobile_robot_wall_cts.h)
00051 
00052   The necessary SYSTEM MODEL is:
00053 
00054   x_k      = x_{k-1} + v_{k-1} * cos(theta) * delta_t
00055   y_k      = y_{k-1} + v_{k-1} * sin(theta) * delta_t
00056 
00057   The used MEASUREMENT MODEL:
00058   measuring the (perpendicular) distance z to the wall y = ax + b
00059 
00060   set WALL_CT = 1/sqrt(pow(a,2) + 1)
00061   z = WALL_CT * a * x - WALL_CT * y + WALL_CT * b + GAUSSIAN_NOISE
00062   or Z = H * X_k + J * U_k
00063 
00064   where
00065 
00066   H = [ WALL_CT * a       - WALL_CT      0 ]
00067   and GAUSSIAN_NOISE = N((WALL_CT * b), SIGMA_MEAS_NOISE)
00068 
00069 */
00070 
00071 
00072 int main(int argc, char** argv)
00073 {
00074   cerr << "==================================================" << endl
00075        << "Test of bootstrap filter" << endl
00076        << "Mobile robot localisation example" << endl
00077        << "==================================================" << endl;
00078 
00079 
00080   /****************************
00081    * NonLinear system model      *
00082    ***************************/
00083 
00084   // create gaussian
00085   ColumnVector sys_noise_Mu(STATE_SIZE);
00086   sys_noise_Mu(1) = MU_SYSTEM_NOISE_X;
00087   sys_noise_Mu(2) = MU_SYSTEM_NOISE_Y;
00088   sys_noise_Mu(3) = MU_SYSTEM_NOISE_THETA;
00089 
00090   SymmetricMatrix sys_noise_Cov(STATE_SIZE);
00091   sys_noise_Cov = 0.0;
00092   sys_noise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
00093   sys_noise_Cov(1,2) = 0.0;
00094   sys_noise_Cov(1,3) = 0.0;
00095   sys_noise_Cov(2,1) = 0.0;
00096   sys_noise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
00097   sys_noise_Cov(2,3) = 0.0;
00098   sys_noise_Cov(3,1) = 0.0;
00099   sys_noise_Cov(3,2) = 0.0;
00100   sys_noise_Cov(3,3) = SIGMA_SYSTEM_NOISE_THETA;
00101 
00102   Gaussian system_Uncertainty(sys_noise_Mu, sys_noise_Cov);
00103 
00104   // create the nonlinear system model
00105   NonlinearSystemPdf sys_pdf(system_Uncertainty);
00106   SystemModel<ColumnVector> sys_model(&sys_pdf);
00107 
00108 
00109   /*********************************
00110    * NonLinear Measurement model   *
00111    ********************************/
00112 
00113   // create matrix H for linear measurement model
00114   double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
00115   Matrix H(MEAS_SIZE,STATE_SIZE);
00116   H = 0.0;
00117   H(1,1) = wall_ct * RICO_WALL;
00118   H(1,2) = 0 - wall_ct;
00119   H(1,3) = 0.0;
00120   // Construct the measurement noise (a scalar in this case)
00121   ColumnVector meas_noise_Mu(MEAS_SIZE);
00122   meas_noise_Mu(1) = MU_MEAS_NOISE;
00123   SymmetricMatrix meas_noise_Cov(MEAS_SIZE);
00124   meas_noise_Cov(1,1) = SIGMA_MEAS_NOISE;
00125   Gaussian measurement_Uncertainty(meas_noise_Mu, meas_noise_Cov);
00126 
00127   // create the measurement model
00128   LinearAnalyticConditionalGaussian meas_pdf(H, measurement_Uncertainty);
00129   LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);
00130 
00131   /****************************
00132    * Linear prior DENSITY     *
00133    ***************************/
00134   // Continuous Gaussian prior (for Kalman filters)
00135   ColumnVector prior_Mu(STATE_SIZE);
00136   prior_Mu(1) = PRIOR_MU_X;
00137   prior_Mu(2) = PRIOR_MU_Y;
00138   prior_Mu(3) = PRIOR_MU_THETA;
00139   SymmetricMatrix prior_Cov(STATE_SIZE);
00140   prior_Cov(1,1) = PRIOR_COV_X;
00141   prior_Cov(1,2) = 0.0;
00142   prior_Cov(1,3) = 0.0;
00143   prior_Cov(2,1) = 0.0;
00144   prior_Cov(2,2) = PRIOR_COV_Y;
00145   prior_Cov(2,3) = 0.0;
00146   prior_Cov(3,1) = 0.0;
00147   prior_Cov(3,2) = 0.0;
00148   prior_Cov(3,3) = PRIOR_COV_THETA;
00149   Gaussian prior_cont(prior_Mu,prior_Cov);
00150 
00151   // Discrete prior for Particle filter (using the continuous Gaussian prior)
00152   vector<Sample<ColumnVector> > prior_samples(NUM_SAMPLES);
00153   MCPdf<ColumnVector> prior_discr(NUM_SAMPLES,STATE_SIZE);
00154   prior_cont.SampleFrom(prior_samples,NUM_SAMPLES,CHOLESKY,NULL);
00155   prior_discr.ListOfSamplesSet(prior_samples);
00156 
00157   /******************************
00158    * Construction of the Filter *
00159    ******************************/
00160   BootstrapFilter<ColumnVector,ColumnVector> filter(&prior_discr, 0, NUM_SAMPLES/4.0);
00161 
00162   /***************************
00163    * initialise MOBILE ROBOT *
00164    **************************/
00165   // Model of mobile robot in world with one wall
00166   // The model is used to simultate the distance measurements.
00167   MobileRobot mobile_robot;
00168   ColumnVector input(2);
00169   input(1) = 0.1;
00170   input(2) = 0.0;
00171 
00172 
00173 
00174 
00175   /*******************
00176    * ESTIMATION LOOP *
00177    *******************/
00178   cout << "MAIN: Starting estimation" << endl;
00179   unsigned int time_step;
00180   for (time_step = 0; time_step < NUM_TIME_STEPS-1; time_step++)
00181     {
00182       // DO ONE STEP WITH MOBILE ROBOT
00183       mobile_robot.Move(input);
00184 
00185       // DO ONE MEASUREMENT
00186       ColumnVector measurement = mobile_robot.Measure();
00187 
00188       // UPDATE FILTER
00189       filter.Update(&sys_model,input,&meas_model,measurement);
00190 
00191     } // estimation loop
00192 
00193 
00194 
00195   Pdf<ColumnVector> * posterior = filter.PostGet();
00196   cout << "After " << time_step+1 << " timesteps " << endl;
00197   cout << " Posterior Mean = " << endl << posterior->ExpectedValueGet() << endl
00198        << " Covariance = " << endl << posterior->CovarianceGet() << "" << endl;
00199 
00200 
00201   cout << "======================================================" << endl
00202        << "End of the Bootstrap filter for mobile robot localisation" << endl
00203        << "======================================================"
00204        << endl;
00205 
00206 
00207   return 0;
00208 }


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