Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
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
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
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
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
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
00101
00102
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
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
00121
00122
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
00136
00137 ExtendedKalmanFilter filter(&prior_cont);
00138
00139
00140
00141
00142
00143
00144
00145 MobileRobot mobile_robot;
00146 ColumnVector input(2);
00147 input(1) = 0.1;
00148 input(2) = 0.0;
00149
00150
00151
00152
00153 vector<Gaussian> posteriors(NUM_TIME_STEPS);
00154 vector<Gaussian>::iterator posteriors_it = posteriors.begin();
00155
00156
00157
00158
00159 unsigned int time_step;
00160 for (time_step = 0; time_step < NUM_TIME_STEPS; time_step++)
00161 {
00162
00163
00164 Gaussian * posterior = (Gaussian*)(filter.PostGet());
00165
00166
00167 mobile_robot.Move(input);
00168
00169 if ((time_step+1)%10 == 0){
00170
00171 ColumnVector measurement = mobile_robot.Measure();
00172
00173
00174 filter.Update(&sys_model,input,&meas_model,measurement);
00175 }
00176 else{
00177 filter.Update(&sys_model,input);
00178 }
00179
00180
00181 *posteriors_it = *posterior;
00182
00183 posteriors_it++;
00184 }
00185
00186
00187 Pdf<ColumnVector> * posterior = filter.PostGet();
00188
00189
00190
00191
00192
00193 RauchTungStriebel backwardfilter((Gaussian*)posterior);
00194
00195
00196
00197
00198
00199 for (time_step = NUM_TIME_STEPS-1; time_step+1 > 0 ; time_step--)
00200 {
00201 posteriors_it--;
00202
00203 Gaussian filtered(posteriors_it->ExpectedValueGet(),posteriors_it->CovarianceGet());
00204 backwardfilter.Update(&sys_model,input, &filtered);
00205 Pdf<ColumnVector> * posterior = backwardfilter.PostGet();
00206
00207
00208 posteriors_it->ExpectedValueSet(posterior->ExpectedValueGet());
00209 posteriors_it->CovarianceSet(posterior->CovarianceGet());
00210
00211 }
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
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 Sun Oct 5 2014 22:29:53