00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include "model_test.hpp"
00019 #include <cmath>
00020
00021 #define NUM_DS 5
00022 #define NUM_COND_ARGS 1
00023
00024
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
00035 #define MEASUREMENT_SIZE 1
00036
00037 #define RICO_WALL 0.5
00038 #define OFFSET_WALL 30
00039 #define DELTA_T 1
00040
00041
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
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
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
00082 DiscreteSystemModel a_discreteSysModel(&a_discretecondpdf);
00083
00084
00085 CPPUNIT_ASSERT_EQUAL( NUM_DS , (int)a_discreteSysModel.NumStatesGet());
00086
00087
00088 CPPUNIT_ASSERT_EQUAL( 1 , a_discreteSysModel.StateSizeGet());
00089
00090
00091 CPPUNIT_ASSERT_EQUAL( true , a_discreteSysModel.SystemWithoutInputs());
00092
00093
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
00104
00105
00106
00107
00108
00109
00110
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
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
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
00149 CPPUNIT_ASSERT_EQUAL( false , c_discreteSysModel.SystemWithoutInputs());
00150
00151
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
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
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
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
00222 CPPUNIT_ASSERT_EQUAL( STATE_SIZE , a_linAnSysModel.StateSizeGet());
00223
00224
00225 CPPUNIT_ASSERT_EQUAL( false , a_linAnSysModel.SystemWithoutInputs());
00226
00227
00228 CPPUNIT_ASSERT_EQUAL( A , a_linAnSysModel.AGet());
00229 CPPUNIT_ASSERT_EQUAL( B , a_linAnSysModel.BGet());
00230
00231
00232 CPPUNIT_ASSERT_EQUAL( A , a_linAnSysModel.df_dxGet(input,state));
00233 CPPUNIT_ASSERT_EQUAL( B , a_linAnSysModel.BGet());
00234
00235
00236 pdf.ConditionalArgumentSet(0,state);
00237 pdf.ConditionalArgumentSet(1,input);
00238 CPPUNIT_ASSERT_EQUAL( pdf.ExpectedValueGet() , a_linAnSysModel.PredictionGet(input,state) );
00239
00240
00241 CPPUNIT_ASSERT_EQUAL( pdf.CovarianceGet() , a_linAnSysModel.CovarianceGet(input,state) );
00242
00243
00244 ColumnVector state_new(STATE_SIZE);
00245 state_new = a_linAnSysModel.Simulate(state, input);
00246
00247
00248
00249 CPPUNIT_ASSERT_EQUAL((double)pdf.ProbabilityGet(state_new) , (double)a_linAnSysModel.ProbabilityGet(state_new,state,input) );
00250
00251
00252
00253 B(1,1) = cos(state_new(STATE_SIZE)) * DELTA_T;
00254 B(2,1) = sin(state_new(STATE_SIZE)) * DELTA_T;
00255
00256
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
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
00281 H(1,1) = WALL_CT * RICO_WALL;
00282 H(1,2) = 0 - WALL_CT;
00283 H(1,STATE_SIZE) = 0;
00284
00285
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
00294 CPPUNIT_ASSERT_EQUAL( MEASUREMENT_SIZE , a_linAnMeasModel.MeasurementSizeGet());
00295
00296
00297 CPPUNIT_ASSERT_EQUAL( true , a_linAnMeasModel.SystemWithoutSensorParams());
00298
00299
00300 CPPUNIT_ASSERT_EQUAL( H , a_linAnMeasModel.HGet());
00301
00302
00303
00304 CPPUNIT_ASSERT_EQUAL( H , a_linAnMeasModel.df_dxGet(0,state));
00305
00306
00307 pdf.ConditionalArgumentSet(0,state);
00308 CPPUNIT_ASSERT_EQUAL( pdf.ExpectedValueGet() , a_linAnMeasModel.PredictionGet(0,state) );
00309
00310
00311 CPPUNIT_ASSERT_EQUAL( pdf.CovarianceGet() , a_linAnMeasModel.CovarianceGet(0,state) );
00312
00313
00314 ColumnVector meas_new(MEASUREMENT_SIZE);
00315 meas_new = a_linAnMeasModel.Simulate(state);
00316
00317
00318
00319 CPPUNIT_ASSERT_EQUAL((double)pdf.ProbabilityGet(meas_new) , (double)a_linAnMeasModel.ProbabilityGet(meas_new,state) );
00320
00321
00322 H = 2;
00323
00324
00325 a_linAnMeasModel.HSet(H);
00326 CPPUNIT_ASSERT_EQUAL(H , a_linAnMeasModel.HGet());
00327
00328
00329
00330
00331 }