model_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 "model_test.hpp"
00019 #include <cmath> // For sinus
00020 
00021 #define NUM_DS 5
00022 #define NUM_COND_ARGS 1
00023 
00024 //testLinearAnalyticSystemModelGaussianUncertainty
00025 #define STATE_SIZE 3
00026 #define INPUT_SIZE 2
00027 
00028 #define SIGMA_NOISE 0.0001 // Noise variance (constant for every input here)
00029 #define CORRELATION_NOISE 0.0 // Correlation between different noise (idem)
00030 
00031 #define LINEAR_SPEED 1.0
00032 #define ROT_SPEED 0.0
00033 
00034 //testLinearAnalyticMeasurementModelGaussianUncertainty
00035 #define MEASUREMENT_SIZE 1
00036 // Coordinates of wall
00037 #define RICO_WALL 0.5
00038 #define OFFSET_WALL 30
00039 #define DELTA_T 1
00040 
00041 // Measurement noise
00042 #define WALL_CT 1/(sqrt(pow(RICO_WALL,2) + 1))
00043 #define MU_MEAS_NOISE OFFSET_WALL*WALL_CT
00044 #define SIGMA_MEAS_NOISE 0.5
00045 // Registers the fixture into the 'registry'
00046 CPPUNIT_TEST_SUITE_REGISTRATION( ModelTest );
00047 
00048 using namespace BFL;
00049 
00050 void
00051 ModelTest::setUp()
00052 {
00053 }
00054 
00055 void
00056 ModelTest::tearDown()
00057 {
00058 }
00059 
00060 void
00061 ModelTest::testDiscreteSystemModel()
00062 {
00063   int cond_arg_dims[NUM_COND_ARGS] = { NUM_DS };
00064   int state_k;int state_kMinusOne;
00065 
00066   DiscreteConditionalPdf a_discretecondpdf(NUM_DS,NUM_COND_ARGS,cond_arg_dims);
00067   std::vector<int> cond_args(NUM_COND_ARGS);
00068   /* Set and Get all Probabilities*/
00069   double prob_diag = 0.98;
00070   double prob_nondiag = (1-prob_diag)/(NUM_DS-1);
00071   for (state_kMinusOne = 0 ; state_kMinusOne < NUM_DS ;  state_kMinusOne++)
00072     {
00073        cond_args[0] = state_kMinusOne;
00074        for (state_k = 0 ; state_k < NUM_DS ;  state_k++)
00075          {
00076            if (state_kMinusOne == state_k) a_discretecondpdf.ProbabilitySet(prob_diag,state_k,cond_args);
00077            else a_discretecondpdf.ProbabilitySet(prob_nondiag,state_k,cond_args);
00078          }
00079     }
00080 
00081   /* Construction */
00082   DiscreteSystemModel a_discreteSysModel(&a_discretecondpdf);
00083 
00084   /* Number of discrete states*/
00085   CPPUNIT_ASSERT_EQUAL( NUM_DS , (int)a_discreteSysModel.NumStatesGet());
00086 
00087   /* State size*/
00088   CPPUNIT_ASSERT_EQUAL( 1 , a_discreteSysModel.StateSizeGet());
00089 
00090   /* SystemWithoutInputs*/
00091   CPPUNIT_ASSERT_EQUAL( true , a_discreteSysModel.SystemWithoutInputs());
00092 
00093   /* ProbabilityGet without inputs*/
00094   for (state_kMinusOne = 0 ; state_kMinusOne < NUM_DS ;  state_kMinusOne++)
00095     {
00096        cond_args[0] = state_kMinusOne;
00097        for (state_k = 0 ; state_k < NUM_DS ;  state_k++)
00098          {
00099             CPPUNIT_ASSERT_EQUAL( (double)a_discretecondpdf.ProbabilityGet(state_k),(double)a_discreteSysModel.ProbabilityGet(state_k,state_kMinusOne));
00100          }
00101     }
00102 
00103   /* Simulate*/// ???
00104   //for (state_kMinusOne = 0 ; state_kMinusOne < NUM_DS ;  state_kMinusOne++)
00105   //   {
00106   //      a_discretecondpdf.ConditionalArgumentSet(0,state_kMinusOne);
00107   //      CPPUNIT_ASSERT_EQUAL( a_discretecondpdf.ExpectedValueGet() , a_discreteSysModel.Simulate(state_kMinusOne));
00108   //   }
00109 
00110   /* Copy constructor */
00111   DiscreteSystemModel b_discreteSysModel(a_discreteSysModel);
00112   for (state_kMinusOne = 0 ; state_kMinusOne < NUM_DS ;  state_kMinusOne++)
00113     {
00114        cond_args[0] = state_kMinusOne;
00115        for (state_k = 0 ; state_k < NUM_DS ;  state_k++)
00116          {
00117             CPPUNIT_ASSERT_EQUAL( (double)a_discreteSysModel.ProbabilityGet(state_k,state_kMinusOne),(double)b_discreteSysModel.ProbabilityGet(state_k,state_kMinusOne));
00118          }
00119     }
00120 
00121   // creating system model with input
00122   int num_cond_args_new = 2;
00123   int size_input = 2;
00124   int cond_args_dims_new[num_cond_args_new];
00125   cond_args_dims_new[0] =  NUM_DS; cond_args_dims_new[1] =  size_input;
00126   int input;
00127 
00128   DiscreteConditionalPdf c_discretecondpdf(NUM_DS,num_cond_args_new,cond_args_dims_new);
00129   std::vector<int> cond_args_new(num_cond_args_new);
00130   /* Set and Get all Probabilities*/
00131   for (state_kMinusOne = 0 ; state_kMinusOne < NUM_DS ;  state_kMinusOne++)
00132     {
00133        cond_args_new[0] = state_kMinusOne;
00134        for (input = 0 ; input < size_input ;  input++)
00135          {
00136             cond_args_new[1] = input;
00137             for (state_k = 0 ; state_k < NUM_DS ;  state_k++)
00138               {
00139                 if (state_kMinusOne == state_k) c_discretecondpdf.ProbabilitySet(prob_diag,state_k,cond_args_new);
00140                 else c_discretecondpdf.ProbabilitySet(prob_nondiag,state_k,cond_args_new);
00141               }
00142          }
00143     }
00144 
00145   DiscreteSystemModel c_discreteSysModel;
00146   c_discreteSysModel.SystemPdfSet(&c_discretecondpdf);
00147 
00148   /* SystemWithoutInputs*/
00149   CPPUNIT_ASSERT_EQUAL( false , c_discreteSysModel.SystemWithoutInputs());
00150 
00151   /* ProbabilityGet without inputs*/
00152   for (state_kMinusOne = 0 ; state_kMinusOne < NUM_DS ;  state_kMinusOne++)
00153     {
00154        cond_args[0] = state_kMinusOne;
00155        for (input = 0 ; input < size_input ;  input++)
00156          {
00157             cond_args_new[1] = input;
00158             for (state_k = 0 ; state_k < NUM_DS ;  state_k++)
00159               {
00160                  CPPUNIT_ASSERT_EQUAL( (double)c_discretecondpdf.ProbabilityGet(state_k),(double)c_discreteSysModel.ProbabilityGet(state_k,state_kMinusOne,input));
00161               }
00162          }
00163     }
00164 }
00165 
00166 
00167 void
00168 ModelTest::testLinearAnalyticMeasurementModelGaussianUncertainty()
00169 {
00170   Matrix A(STATE_SIZE,STATE_SIZE);
00171   Matrix B(STATE_SIZE,INPUT_SIZE);
00172 
00173   ColumnVector state(STATE_SIZE);
00174   ColumnVector initial_state(3);
00175   initial_state(1) = 1.0;
00176   initial_state(2) = 1.0;
00177   initial_state(3) = 0.5;
00178   ColumnVector input(INPUT_SIZE);
00179   ColumnVector initial_input(2);
00180   initial_input(1) = 1.0;
00181   initial_input(2) = 1.0;
00182 
00183   for (int i=1; i < STATE_SIZE+1; i++){state(i) = initial_state(i);}
00184   for (int i=1; i < INPUT_SIZE+1; i++){input(i) = initial_input(i);}
00185 
00186   // Uncertainty or Noice (Additive)
00187   ColumnVector Noise_Mu(STATE_SIZE);
00188   for (int row=0; row < STATE_SIZE; row++){Noise_Mu(row+1) = 0;}
00189   SymmetricMatrix Noise_Cov(STATE_SIZE);
00190   for (int row=0; row < STATE_SIZE; row++)
00191     {
00192       for (int column=0; column < STATE_SIZE; column++)
00193         {
00194           if (row == column) {Noise_Cov(row+1,column+1) = SIGMA_NOISE;}
00195           else {Noise_Cov(row+1,column+1) = CORRELATION_NOISE;}
00196         }
00197     }
00198   Gaussian System_Uncertainty(Noise_Mu,Noise_Cov);
00199 
00200   // MATRIX A: unit matrix
00201   for (int row=0; row < STATE_SIZE; row++)
00202     {
00203       for (int column=0; column < STATE_SIZE; column++)
00204         {
00205           if (row == column) A(row+1,column+1)=1;
00206           else A(row+1,column+1)=0;
00207         }
00208     }
00209   // MATRIX B
00210   B(STATE_SIZE,1) = 0.0; B(1,INPUT_SIZE) = 0.0; B(2,INPUT_SIZE) = 0.0;
00211   B(STATE_SIZE,INPUT_SIZE) = 1.0;
00212   B(1,1) = cos(state(STATE_SIZE)) * DELTA_T;
00213   B(2,1) = sin(state(STATE_SIZE)) * DELTA_T;
00214 
00215   vector<Matrix> v(2);
00216   v[0] = A;
00217   v[1] = B;
00218   LinearAnalyticConditionalGaussian pdf(v,System_Uncertainty);
00219   LinearAnalyticSystemModelGaussianUncertainty a_linAnSysModel(&pdf);
00220 
00221   /* State size Get */
00222   CPPUNIT_ASSERT_EQUAL( STATE_SIZE , a_linAnSysModel.StateSizeGet());
00223 
00224   /* System without inputs */
00225   CPPUNIT_ASSERT_EQUAL( false , a_linAnSysModel.SystemWithoutInputs());
00226 
00227   /* Get the system model matrices */
00228   CPPUNIT_ASSERT_EQUAL( A , a_linAnSysModel.AGet());
00229   CPPUNIT_ASSERT_EQUAL( B , a_linAnSysModel.BGet());
00230 
00231   /* df_dxGet */
00232   CPPUNIT_ASSERT_EQUAL( A , a_linAnSysModel.df_dxGet(input,state));
00233   CPPUNIT_ASSERT_EQUAL( B , a_linAnSysModel.BGet());
00234 
00235   /* PredictionGet */
00236   pdf.ConditionalArgumentSet(0,state);
00237   pdf.ConditionalArgumentSet(1,input);
00238   CPPUNIT_ASSERT_EQUAL( pdf.ExpectedValueGet() , a_linAnSysModel.PredictionGet(input,state) );
00239 
00240   /* CovarianceGet */
00241   CPPUNIT_ASSERT_EQUAL( pdf.CovarianceGet() , a_linAnSysModel.CovarianceGet(input,state) );
00242 
00243   /* Simulate */
00244   ColumnVector state_new(STATE_SIZE);
00245   state_new = a_linAnSysModel.Simulate(state, input);
00246   // TODO: can we check this in any way?
00247 
00248   /* ProbabilityGet */
00249   CPPUNIT_ASSERT_EQUAL((double)pdf.ProbabilityGet(state_new) , (double)a_linAnSysModel.ProbabilityGet(state_new,state,input) );
00250 
00251   // build new A and B matrices for new state
00252   // MATRIX B
00253   B(1,1) = cos(state_new(STATE_SIZE)) * DELTA_T;
00254   B(2,1) = sin(state_new(STATE_SIZE)) * DELTA_T;
00255 
00256   /* A and B set and get */
00257   a_linAnSysModel.ASet(A);
00258   a_linAnSysModel.BSet(B);
00259   CPPUNIT_ASSERT_EQUAL(A , a_linAnSysModel.AGet());
00260   CPPUNIT_ASSERT_EQUAL(B , a_linAnSysModel.BGet());
00261 
00262   //TODO: SystemPdfSet
00263 }
00264 
00265 void
00266 ModelTest::testLinearAnalyticSystemModelGaussianUncertainty()
00267 {
00268   Matrix H(MEASUREMENT_SIZE,STATE_SIZE);
00269 
00270   ColumnVector state(STATE_SIZE);
00271   ColumnVector initial_state(3);
00272   initial_state(1) = 1.0;
00273   initial_state(2) = 1.0;
00274   initial_state(3) = 0.5;
00275   for (int i=1; i < STATE_SIZE+1; i++){state(i) = initial_state(i);}
00276   ColumnVector measurement(MEASUREMENT_SIZE);
00277   ColumnVector MeasNoise_Mu(MEASUREMENT_SIZE);
00278   SymmetricMatrix MeasNoise_Cov(MEASUREMENT_SIZE);
00279 
00280   // Fill up H
00281   H(1,1) = WALL_CT * RICO_WALL;
00282   H(1,2) = 0 - WALL_CT;
00283   H(1,STATE_SIZE) = 0;
00284 
00285   // Construct the measurement noise (a scalar in this case)
00286   MeasNoise_Mu(1) = MU_MEAS_NOISE;
00287   MeasNoise_Cov(1,1) = SIGMA_MEAS_NOISE;
00288 
00289   Gaussian Measurement_Uncertainty(MeasNoise_Mu,MeasNoise_Cov);
00290   LinearAnalyticConditionalGaussian pdf(H,Measurement_Uncertainty);
00291   LinearAnalyticMeasurementModelGaussianUncertainty a_linAnMeasModel(&pdf);
00292 
00293   /* Measurement size Get */
00294   CPPUNIT_ASSERT_EQUAL( MEASUREMENT_SIZE , a_linAnMeasModel.MeasurementSizeGet());
00295 
00296   /* System without sensor parameters */
00297   CPPUNIT_ASSERT_EQUAL( true , a_linAnMeasModel.SystemWithoutSensorParams());
00298 
00299   /* Get the measurement model matrices */
00300   CPPUNIT_ASSERT_EQUAL( H , a_linAnMeasModel.HGet());
00301   //CPPUNIT_ASSERT_EQUAL( J , a_linAnMeasModel.JGet());
00302 
00303   /* df_dxGet */
00304   CPPUNIT_ASSERT_EQUAL( H , a_linAnMeasModel.df_dxGet(0,state));
00305 
00306   /* PredictionGet */
00307   pdf.ConditionalArgumentSet(0,state);
00308   CPPUNIT_ASSERT_EQUAL( pdf.ExpectedValueGet() , a_linAnMeasModel.PredictionGet(0,state) );
00309 
00310   /* CovarianceGet */
00311   CPPUNIT_ASSERT_EQUAL( pdf.CovarianceGet() , a_linAnMeasModel.CovarianceGet(0,state) );
00312 
00313   /* Simulate */
00314   ColumnVector meas_new(MEASUREMENT_SIZE);
00315   meas_new = a_linAnMeasModel.Simulate(state);
00316   // TODO: can we check this in any way?
00317 
00318   /* ProbabilityGet */
00319   CPPUNIT_ASSERT_EQUAL((double)pdf.ProbabilityGet(meas_new) , (double)a_linAnMeasModel.ProbabilityGet(meas_new,state) );
00320 
00321   // build new H
00322   H = 2;
00323 
00324   /* H  set and get */
00325   a_linAnMeasModel.HSet(H);
00326   CPPUNIT_ASSERT_EQUAL(H , a_linAnMeasModel.HGet());
00327 
00328   //TODO: SystemPdfSet
00329 
00330 
00331 }


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