test_kalman_smoother.cpp
Go to the documentation of this file.
00001 // $Id: test_kalman_smoother.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/extendedkalmanfilter.h>
00024 
00025 #include <model/linearanalyticsystemmodel_gaussianuncertainty.h>
00026 #include <model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
00027 
00028 #include <pdf/analyticconditionalgaussian.h>
00029 #include <pdf/analyticconditionalgaussian.h>
00030 #include <pdf/linearanalyticconditionalgaussian.h>
00031 
00032 #include "../compare_filters/nonlinearanalyticconditionalgaussianmobile.h"
00033 #include "../mobile_robot.h"
00034 
00035 #include <smoother/rauchtungstriebel.h>
00036 #include <smoother/particlesmoother.h>
00037 
00038 #include <iostream>
00039 #include <fstream>
00040 
00041 // Include file with properties
00042 #include "../examples/mobile_robot_wall_cts.h"
00043 
00044 using namespace MatrixWrapper;
00045 using namespace BFL;
00046 using namespace std;
00047 
00048 /* The purpose of this program is to construct a filter for the problem
00049   of localisation of a mobile robot equipped with an ultrasonic sensor.
00050   In this case the orientation is known, which simplifies the model considerably:
00051   The system model will become linear.
00052   The ultrasonic measures the distance to the wall (it can be switched off:
00053   see mobile_robot_wall_cts.h)
00054 
00055   The necessary SYSTEM MODEL is:
00056 
00057   x_k      = x_{k-1} + v_{k-1} * cos(theta) * delta_t
00058   y_k      = y_{k-1} + v_{k-1} * sin(theta) * delta_t
00059 
00060   The used MEASUREMENT MODEL:
00061   measuring the (perpendicular) distance z to the wall y = ax + b
00062 
00063   set WALL_CT = 1/sqrt(pow(a,2) + 1)
00064   z = WALL_CT * a * x - WALL_CT * y + WALL_CT * b + GAUSSIAN_NOISE
00065   or Z = H * X_k + J * U_k
00066 
00067   where
00068 
00069   H = [ WALL_CT * a       - WALL_CT      0 ]
00070   and GAUSSIAN_NOISE = N((WALL_CT * b), SIGMA_MEAS_NOISE)
00071 
00072 */
00073 
00074 
00075 int main(int argc, char** argv)
00076 {
00077   cerr << "==================================================" << endl
00078        << "Test of different smooters" << endl
00079        << "Mobile robot localisation example" << endl
00080        << "==================================================" << endl;
00081 
00082   /***********************
00083    * PREPARE FILESTREAMS *
00084    **********************/
00085   ofstream fout_time, fout_E, fout_cov, fout_meas, fout_states, fout_particles, fout_numparticles, fout_E_smooth, fout_cov_smooth, fout_time_smooth;
00086 
00087   fout_time.open("time.out");
00088   fout_E.open("E.out");
00089   fout_cov.open("cov.out");
00090   fout_meas.open("meas.out");
00091   fout_states.open("states.out");
00092   fout_E_smooth.open("Esmooth.out");
00093   fout_cov_smooth.open("covsmooth.out");
00094   fout_time_smooth.open("timesmooth.out");
00095 
00096   /****************************
00097    * NonLinear system model      *
00098    ***************************/
00099 
00100   // create gaussian
00101   ColumnVector sys_noise_Mu(STATE_SIZE);
00102   sys_noise_Mu = 0.0;
00103   sys_noise_Mu(1) = MU_SYSTEM_NOISE_X;
00104   sys_noise_Mu(2) = MU_SYSTEM_NOISE_Y;
00105   sys_noise_Mu(3) = MU_SYSTEM_NOISE_THETA;
00106 
00107   SymmetricMatrix sys_noise_Cov(STATE_SIZE);
00108   sys_noise_Cov = 0.0;
00109   sys_noise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
00110   sys_noise_Cov(1,2) = 0.0;
00111   sys_noise_Cov(1,3) = 0.0;
00112   sys_noise_Cov(2,1) = 0.0;
00113   sys_noise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
00114   sys_noise_Cov(2,3) = 0.0;
00115   sys_noise_Cov(3,1) = 0.0;
00116   sys_noise_Cov(3,2) = 0.0;
00117   sys_noise_Cov(3,3) = SIGMA_SYSTEM_NOISE_THETA;
00118 
00119   Gaussian system_Uncertainty(sys_noise_Mu, sys_noise_Cov);
00120 
00121   // create the model
00122   NonLinearAnalyticConditionalGaussianMobile sys_pdf(system_Uncertainty);
00123   AnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);
00124 
00125   /*********************************
00126    * Initialise measurement model *
00127    ********************************/
00128   // Fill up H
00129   double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
00130   Matrix H(MEAS_SIZE,STATE_SIZE);
00131   H = 0.0;
00132   H(1,1) = wall_ct * RICO_WALL;
00133   H(1,2) = 0 - wall_ct;
00134 
00135    // Construct the measurement noise (a scalar in this case)
00136   ColumnVector MeasNoise_Mu(MEAS_SIZE);
00137   SymmetricMatrix MeasNoise_Cov(MEAS_SIZE);
00138   MeasNoise_Mu(1) = MU_MEAS_NOISE;
00139   MeasNoise_Cov(1,1) = SIGMA_MEAS_NOISE;
00140 
00141   Gaussian measurement_Uncertainty(MeasNoise_Mu,MeasNoise_Cov);
00142 
00143   // create the model
00144   LinearAnalyticConditionalGaussian meas_pdf(H, measurement_Uncertainty);
00145   LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);
00146 
00147 
00148   /****************************
00149    * Linear prior DENSITY     *
00150    ***************************/
00151   // Continuous Gaussian prior (for Kalman filters)
00152   ColumnVector prior_mu(STATE_SIZE);
00153   SymmetricMatrix prior_sigma(STATE_SIZE);
00154   prior_mu(1) = PRIOR_MU_X;
00155   prior_mu(2) = PRIOR_MU_Y;
00156   prior_sigma = 0.0;
00157   prior_sigma(1,1) = PRIOR_COV_X;
00158   prior_sigma(2,2) = PRIOR_COV_Y;
00159   prior_sigma(3,3) = PRIOR_COV_THETA;
00160   Gaussian prior_cont(prior_mu,prior_sigma);
00161 
00162 
00163    cout<< "Prior initialised"<< "" << endl;
00164    cout << "Prior Mean = " << endl << prior_cont.ExpectedValueGet() << endl
00165         << "Covariance = " << endl << prior_cont.CovarianceGet() << endl;
00166 
00167   /******************************
00168    * Construction of the Filter *
00169    ******************************/
00170   ExtendedKalmanFilter filter(&prior_cont);
00171 
00172 
00173 
00174   /***************************
00175    * initialise MOBILE ROBOT *
00176    **************************/
00177   // Model of mobile robot in world with one wall
00178   // The model is used to simultate the distance measurements.
00179   MobileRobot mobile_robot;
00180   ColumnVector input(2);
00181   input(1) = 0.1;
00182   input(2) = 0.0;
00183 
00184   /***************************
00185    * vector in which all posteriors will be stored*
00186    **************************/
00187   vector<Gaussian> posteriors(NUM_TIME_STEPS);
00188   vector<Gaussian>::iterator posteriors_it  = posteriors.begin();
00189 
00190   /*******************
00191    * ESTIMATION LOOP *
00192    *******************/
00193   cout << "MAIN: Starting estimation" << endl;
00194   unsigned int time_step;
00195   for (time_step = 0; time_step < NUM_TIME_STEPS; time_step++)
00196     {
00197       // write date in files
00198       fout_time << time_step << ";" << endl;
00199       fout_meas << mobile_robot.Measure()(1) << ";" << endl;
00200       fout_states << mobile_robot.GetState()(1) << "," << mobile_robot.GetState()(2) << ","
00201                   << mobile_robot.GetState()(3) << ";" << endl;
00202 
00203       // write posterior to file
00204       Gaussian * posterior = (Gaussian*)(filter.PostGet());
00205       fout_E << posterior->ExpectedValueGet()(1) << "," << posterior->ExpectedValueGet()(2)<< ","
00206              << posterior->ExpectedValueGet()(3) << ";"  << endl;
00207       fout_cov << posterior->CovarianceGet()(1,1) << "," << posterior->CovarianceGet()(1,2) << ","
00208                << posterior->CovarianceGet()(1,3) << "," << posterior->CovarianceGet()(2,2) << ","
00209                << posterior->CovarianceGet()(2,3) << "," << posterior->CovarianceGet()(3,3) << ";" << endl;
00210 
00211       // DO ONE STEP WITH MOBILE ROBOT
00212       mobile_robot.Move(input);
00213 
00214       if ((time_step+1)%10 == 0){
00215         // DO ONE MEASUREMENT
00216         ColumnVector measurement = mobile_robot.Measure();
00217 
00218         // UPDATE FILTER
00219         filter.Update(&sys_model,input,&meas_model,measurement);
00220       }
00221       else{
00222         filter.Update(&sys_model,input);
00223       }
00224 
00225       //Pdf<ColumnVector> * posterior = filter.PostGet();
00226       // make copy of posterior
00227       *posteriors_it = *posterior;
00228 
00229       posteriors_it++;
00230     } // estimation loop
00231 
00232 
00233       Pdf<ColumnVector> * posterior = filter.PostGet();
00234       // write data in files
00235       fout_time << time_step << ";" << endl;
00236       fout_meas << mobile_robot.Measure()(1) << ";" << endl;
00237       fout_states << mobile_robot.GetState()(1) << "," << mobile_robot.GetState()(2) << ","
00238                   << mobile_robot.GetState()(3) << ";" << endl;
00239 
00240       // write posterior to file
00241       fout_E << posterior->ExpectedValueGet()(1) << "," << posterior->ExpectedValueGet()(2)<< ","
00242              << posterior->ExpectedValueGet()(3) << ";"  << endl;
00243       fout_cov << posterior->CovarianceGet()(1,1) << "," << posterior->CovarianceGet()(1,2) << ","
00244                << posterior->CovarianceGet()(1,3) << "," << posterior->CovarianceGet()(2,2) << ","
00245                << posterior->CovarianceGet()(2,3) << "," << posterior->CovarianceGet()(3,3) << ";" << endl;
00246 
00247   cout << "After " << time_step+1 << " timesteps " << endl;
00248   cout << " Posterior Mean = " << endl << posterior->ExpectedValueGet() << endl
00249        << " Covariance = " << endl << posterior->CovarianceGet() << "" << endl;
00250 
00251 
00252   cout << "======================================================" << endl
00253        << "End of the filter for mobile robot localisation" << endl
00254        << "======================================================"
00255        << endl;
00256 
00257 
00258   /***************************************
00259    * Construction of the Backward Filter *
00260    **************************************/
00261 RauchTungStriebel backwardfilter((Gaussian*)posterior);
00262 
00263   fout_time_smooth << time_step << ";" << endl;
00264   // write posterior to file
00265   fout_E_smooth << posterior->ExpectedValueGet()(1) << "," << posterior->ExpectedValueGet()(2)<< ","
00266          << posterior->ExpectedValueGet()(3) << ";"  << endl;
00267   fout_cov_smooth << posterior->CovarianceGet()(1,1) << "," << posterior->CovarianceGet()(1,2) << ","
00268            << posterior->CovarianceGet()(1,3) << "," << posterior->CovarianceGet()(2,2) << ","
00269            << posterior->CovarianceGet()(2,3) << "," << posterior->CovarianceGet()(3,3) << ";" << endl;
00270 
00271   /*******************
00272    * ESTIMATION LOOP *
00273    *******************/
00274   cout << "======================================================" << endl
00275        << "Start of the smoother for mobile robot localisation" << endl
00276        << "======================================================"
00277        << endl;
00278   for (time_step = NUM_TIME_STEPS-1; time_step+1 > 0 ; time_step--)
00279     {
00280       posteriors_it--;
00281       // UPDATE  BACKWARDFILTER
00282       Gaussian filtered(posteriors_it->ExpectedValueGet(),posteriors_it->CovarianceGet());
00283       backwardfilter.Update(&sys_model,input, &filtered);
00284       Pdf<ColumnVector> * posterior = backwardfilter.PostGet();
00285 
00286       fout_time_smooth << time_step << ";" << endl;
00287       // write posterior to file
00288       fout_E_smooth << posterior->ExpectedValueGet()(1) << "," << posterior->ExpectedValueGet()(2)<< ","
00289              << posterior->ExpectedValueGet()(3) << ";"  << endl;
00290       fout_cov_smooth << posterior->CovarianceGet()(1,1) << "," << posterior->CovarianceGet()(1,2) << ","
00291                << posterior->CovarianceGet()(1,3) << "," << posterior->CovarianceGet()(2,2) << ","
00292                << posterior->CovarianceGet()(2,3) << "," << posterior->CovarianceGet()(3,3) << ";" << endl;
00293 
00294       // make copy of posterior
00295       posteriors_it->ExpectedValueSet(posterior->ExpectedValueGet());
00296       posteriors_it->CovarianceSet(posterior->CovarianceGet());
00297 
00298     } // estimation loop
00299 
00300   cout << "After Smoothing first timestep " << endl;
00301   cout << " Posterior Mean = " << endl << posteriors_it->ExpectedValueGet() << endl
00302        << " Covariance = " << endl << posteriors_it->CovarianceGet() << "" << endl;
00303 
00304 
00305 
00306   cout << "======================================================" << endl
00307        << "End of the Kalman smoother for mobile robot localisation" << endl
00308        << "======================================================"
00309        << endl;
00310   return 0;
00311 
00312   /****************************
00313    * CLOSE FILESTREAMS
00314    ***************************/
00315   fout_time.close();
00316   fout_E.close();
00317   fout_cov.close();
00318   fout_meas.close();
00319   fout_states.close();
00320   fout_time_smooth.close();
00321   fout_E_smooth.close();
00322   fout_cov_smooth.close();
00323 }


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 Mon Feb 11 2019 03:45:12