smoother_test.cpp
Go to the documentation of this file.
00001 // Copyright (C) 2007 Tinne De Laet <first dot last at mech dot kuleuven dot be>
00002 //
00003 // This program is free software; you can redistribute it and/or modify
00004 // it under the terms of the GNU General Public License as published by
00005 // the Free Software Foundation; either version 2 of the License, or
00006 // (at your option) any later version.
00007 //
00008 // This program is distributed in the hope that it will be useful,
00009 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00010 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00011 // GNU General Public License for more details.
00012 //
00013 // You should have received a copy of the GNU General Public License
00014 // along with this program; if not, write to the Free Software
00015 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00016 //
00017 
00018 #include "smoother_test.hpp"
00019 #include "approxEqual.hpp"
00020 #include <filter/extendedkalmanfilter.h>
00021 #include <model/linearanalyticsystemmodel_gaussianuncertainty.h>
00022 #include <model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
00023 #include <pdf/analyticconditionalgaussian.h>
00024 #include <pdf/analyticconditionalgaussian.h>
00025 #include <pdf/linearanalyticconditionalgaussian.h>
00026 #include <smoother/rauchtungstriebel.h>
00027 #include <smoother/particlesmoother.h>
00028 
00029 #include "../examples/mobile_robot_wall_cts.h"
00030 #include "../examples/mobile_robot.h"
00031 
00032 // Registers the fixture into the 'registry'
00033 CPPUNIT_TEST_SUITE_REGISTRATION( SmootherTest );
00034 using namespace BFL;
00035 
00036 
00037 void
00038 SmootherTest::setUp()
00039 {
00040 }
00041 
00042 void
00043 SmootherTest::tearDown()
00044 {
00045 }
00046 
00047 void
00048 SmootherTest::testKalmanSmoother()
00049 {
00050   double epsilon       = 0.01;
00051   double epsilon_large = 0.5;
00052   double epsilon_huge  = 2.0;
00053     /* The purpose of this program is to construct a filter for the problem
00054       of localisation of a mobile robot equipped with an ultrasonic sensor.
00055       In this case the orientation is known, which simplifies the model considerably:
00056       The system model will become linear.
00057       The ultrasonic measures the distance to the wall (it can be switched off:
00058       see mobile_robot_wall_cts.h)
00059 
00060       The necessary SYSTEM MODEL is:
00061 
00062       x_k      = x_{k-1} + v_{k-1} * cos(theta) * delta_t
00063       y_k      = y_{k-1} + v_{k-1} * sin(theta) * delta_t
00064 
00065       The used MEASUREMENT MODEL:
00066       measuring the (perpendicular) distance z to the wall y = ax + b
00067 
00068       set WALL_CT = 1/sqrt(pow(a,2) + 1)
00069       z = WALL_CT * a * x - WALL_CT * y + WALL_CT * b + GAUSSIAN_NOISE
00070       or Z = H * X_k + J * U_k
00071 
00072       where
00073 
00074       H = [ WALL_CT * a       - WALL_CT      0 ]
00075       and GAUSSIAN_NOISE = N((WALL_CT * b), SIGMA_MEAS_NOISE)
00076 
00077     */
00078 
00079      /****************************
00080       * NonLinear system model      *
00081       ***************************/
00082       ColumnVector SysNoise_Mu(STATE_SIZE);
00083       SysNoise_Mu = 0.0;
00084       SysNoise_Mu(1) = MU_SYSTEM_NOISE_X;
00085       SysNoise_Mu(2) = MU_SYSTEM_NOISE_Y;
00086       SysNoise_Mu(3) = MU_SYSTEM_NOISE_THETA;
00087 
00088       SymmetricMatrix SysNoise_Cov(STATE_SIZE);
00089       SysNoise_Cov = 0.0;
00090       // Uncertainty or Noice (Additive) and Matrix A
00091       SysNoise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
00092       SysNoise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
00093       SysNoise_Cov(3,3) = SIGMA_SYSTEM_NOISE_THETA;
00094 
00095       Gaussian System_Uncertainty(SysNoise_Mu, SysNoise_Cov);
00096       NonLinearAnalyticConditionalGaussianMobile sys_pdf(System_Uncertainty);
00097       AnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);
00098 
00099       /*********************************
00100        * Initialise measurement model *
00101        ********************************/
00102       // Fill up H
00103       double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
00104       Matrix H(MEAS_SIZE,STATE_SIZE);
00105       H = 0.0;
00106       H(1,1) = wall_ct * RICO_WALL;
00107       H(1,2) = 0 - wall_ct;
00108 
00109       // Construct the measurement noise (a scalar in this case)
00110       ColumnVector MeasNoise_Mu(MEAS_SIZE);
00111       SymmetricMatrix MeasNoise_Cov(MEAS_SIZE);
00112       MeasNoise_Mu(1) = MU_MEAS_NOISE;
00113       MeasNoise_Cov(1,1) = SIGMA_MEAS_NOISE;
00114 
00115       Gaussian Measurement_Uncertainty(MeasNoise_Mu,MeasNoise_Cov);
00116       LinearAnalyticConditionalGaussian meas_pdf(H,Measurement_Uncertainty);
00117       LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);
00118 
00119       /****************************
00120        * Linear prior DENSITY     *
00121        ***************************/
00122       // Continuous Gaussian prior (for Kalman filters)
00123       ColumnVector prior_mu(STATE_SIZE);
00124       SymmetricMatrix prior_sigma(STATE_SIZE);
00125       prior_mu(1) = PRIOR_MU_X;
00126       prior_mu(2) = PRIOR_MU_Y;
00127       prior_mu(STATE_SIZE) = PRIOR_MU_THETA;
00128       prior_sigma = 0.0;
00129       prior_sigma(1,1) = PRIOR_COV_X;
00130       prior_sigma(2,2) = PRIOR_COV_Y;
00131       prior_sigma(3,3) = PRIOR_COV_THETA;
00132       Gaussian prior_cont(prior_mu,prior_sigma);
00133 
00134       /******************************
00135        * Construction of the Filter *
00136        ******************************/
00137       ExtendedKalmanFilter filter(&prior_cont);
00138 
00139 
00140       /***************************
00141        * initialise MOBILE ROBOT *
00142        **************************/
00143       // Model of mobile robot in world with one wall
00144       // The model is used to simultate the distance measurements.
00145       MobileRobot mobile_robot;
00146       ColumnVector input(2);
00147       input(1) = 0.1;
00148       input(2) = 0.0;
00149 
00150       /***************************
00151        * vector in which all posteriors will be stored*
00152        **************************/
00153       vector<Gaussian> posteriors(NUM_TIME_STEPS);
00154       vector<Gaussian>::iterator posteriors_it  = posteriors.begin();
00155 
00156       /*******************
00157        * ESTIMATION LOOP *
00158        *******************/
00159       unsigned int time_step;
00160       for (time_step = 0; time_step < NUM_TIME_STEPS; time_step++)
00161         {
00162 
00163           // write posterior to file
00164           Gaussian * posterior = (Gaussian*)(filter.PostGet());
00165 
00166           // DO ONE STEP WITH MOBILE ROBOT
00167           mobile_robot.Move(input);
00168 
00169           if ((time_step+1)%10 == 0){
00170             // DO ONE MEASUREMENT
00171             ColumnVector measurement = mobile_robot.Measure();
00172 
00173             // UPDATE FILTER
00174             filter.Update(&sys_model,input,&meas_model,measurement);
00175           }
00176           else{
00177             filter.Update(&sys_model,input);
00178           }
00179 
00180           // make copy of posterior
00181           *posteriors_it = *posterior;
00182 
00183           posteriors_it++;
00184         } // estimation loop
00185 
00186 
00187           Pdf<ColumnVector> * posterior = filter.PostGet();
00188 
00189 
00190       /***************************************
00191        * Construction of the Backward Filter *
00192        **************************************/
00193     RauchTungStriebel backwardfilter((Gaussian*)posterior);
00194 
00195 
00196       /*******************
00197        * ESTIMATION LOOP *
00198        *******************/
00199       for (time_step = NUM_TIME_STEPS-1; time_step+1 > 0 ; time_step--)
00200         {
00201           posteriors_it--;
00202           // UPDATE  BACKWARDFILTER
00203           Gaussian filtered(posteriors_it->ExpectedValueGet(),posteriors_it->CovarianceGet());
00204           backwardfilter.Update(&sys_model,input, &filtered);
00205           Pdf<ColumnVector> * posterior = backwardfilter.PostGet();
00206 
00207           // make copy of posterior
00208           posteriors_it->ExpectedValueSet(posterior->ExpectedValueGet());
00209           posteriors_it->CovarianceSet(posterior->CovarianceGet());
00210 
00211         } // estimation loop
00212 
00213   ColumnVector mean_smoother_check(STATE_SIZE);
00214   mean_smoother_check(1) = PRIOR_MU_X; mean_smoother_check(2) = PRIOR_MU_Y; mean_smoother_check(3) = PRIOR_MU_THETA;
00215   SymmetricMatrix cov_smoother_check(STATE_SIZE);
00216   cov_smoother_check=0.0;
00217   cov_smoother_check(1,1) = PRIOR_COV_X; 
00218   CPPUNIT_ASSERT_EQUAL(approxEqual(posteriors_it->ExpectedValueGet(), mean_smoother_check, epsilon_large),true);
00219   CPPUNIT_ASSERT_EQUAL(approxEqual(posteriors_it->CovarianceGet(), cov_smoother_check, epsilon_large),true);
00220 
00221 }
00222 
00223 void
00224 SmootherTest::testParticleSmoother()
00225 {
00226  // no nice implementation found yet
00227 }


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 Thu Feb 11 2016 22:31:57