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
00019
00020
00021
00022
00023 #include <filter/bootstrapfilter.h>
00024
00025 #include <model/systemmodel.h>
00026 #include <model/measurementmodel.h>
00027
00028 #include "nonlinearSystemPdf.h"
00029 #include "nonlinearMeasurementPdf.h"
00030
00031 #include "../mobile_robot.h"
00032
00033 #include <iostream>
00034 #include <fstream>
00035
00036
00037 #include "../mobile_robot_wall_cts.h"
00038
00039 using namespace MatrixWrapper;
00040 using namespace BFL;
00041 using namespace std;
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072 int main(int argc, char** argv)
00073 {
00074 cerr << "==================================================" << endl
00075 << "Test of bootstrap filter" << endl
00076 << "Mobile robot localisation example" << endl
00077 << "==================================================" << endl;
00078
00079
00080
00081
00082
00083
00084
00085 ColumnVector sys_noise_Mu(STATE_SIZE);
00086 sys_noise_Mu(1) = MU_SYSTEM_NOISE_X;
00087 sys_noise_Mu(2) = MU_SYSTEM_NOISE_Y;
00088 sys_noise_Mu(3) = MU_SYSTEM_NOISE_THETA;
00089
00090 SymmetricMatrix sys_noise_Cov(STATE_SIZE);
00091 sys_noise_Cov = 0.0;
00092 sys_noise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
00093 sys_noise_Cov(1,2) = 0.0;
00094 sys_noise_Cov(1,3) = 0.0;
00095 sys_noise_Cov(2,1) = 0.0;
00096 sys_noise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
00097 sys_noise_Cov(2,3) = 0.0;
00098 sys_noise_Cov(3,1) = 0.0;
00099 sys_noise_Cov(3,2) = 0.0;
00100 sys_noise_Cov(3,3) = SIGMA_SYSTEM_NOISE_THETA;
00101
00102 Gaussian system_Uncertainty(sys_noise_Mu, sys_noise_Cov);
00103
00104
00105 NonlinearSystemPdf sys_pdf(system_Uncertainty);
00106 SystemModel<ColumnVector> sys_model(&sys_pdf);
00107
00108
00109
00110
00111
00112
00113
00114 double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
00115 Matrix H(MEAS_SIZE,STATE_SIZE);
00116 H = 0.0;
00117 H(1,1) = wall_ct * RICO_WALL;
00118 H(1,2) = 0 - wall_ct;
00119 H(1,3) = 0.0;
00120
00121 ColumnVector meas_noise_Mu(MEAS_SIZE);
00122 meas_noise_Mu(1) = MU_MEAS_NOISE;
00123 SymmetricMatrix meas_noise_Cov(MEAS_SIZE);
00124 meas_noise_Cov(1,1) = SIGMA_MEAS_NOISE;
00125 Gaussian measurement_Uncertainty(meas_noise_Mu, meas_noise_Cov);
00126
00127
00128 LinearAnalyticConditionalGaussian meas_pdf(H, measurement_Uncertainty);
00129 LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);
00130
00131
00132
00133
00134
00135 ColumnVector prior_Mu(STATE_SIZE);
00136 prior_Mu(1) = PRIOR_MU_X;
00137 prior_Mu(2) = PRIOR_MU_Y;
00138 prior_Mu(3) = PRIOR_MU_THETA;
00139 SymmetricMatrix prior_Cov(STATE_SIZE);
00140 prior_Cov(1,1) = PRIOR_COV_X;
00141 prior_Cov(1,2) = 0.0;
00142 prior_Cov(1,3) = 0.0;
00143 prior_Cov(2,1) = 0.0;
00144 prior_Cov(2,2) = PRIOR_COV_Y;
00145 prior_Cov(2,3) = 0.0;
00146 prior_Cov(3,1) = 0.0;
00147 prior_Cov(3,2) = 0.0;
00148 prior_Cov(3,3) = PRIOR_COV_THETA;
00149 Gaussian prior_cont(prior_Mu,prior_Cov);
00150
00151
00152 vector<Sample<ColumnVector> > prior_samples(NUM_SAMPLES);
00153 MCPdf<ColumnVector> prior_discr(NUM_SAMPLES,STATE_SIZE);
00154 prior_cont.SampleFrom(prior_samples,NUM_SAMPLES,CHOLESKY,NULL);
00155 prior_discr.ListOfSamplesSet(prior_samples);
00156
00157
00158
00159
00160 BootstrapFilter<ColumnVector,ColumnVector> filter(&prior_discr, 0, NUM_SAMPLES/4.0);
00161
00162
00163
00164
00165
00166
00167 MobileRobot mobile_robot;
00168 ColumnVector input(2);
00169 input(1) = 0.1;
00170 input(2) = 0.0;
00171
00172
00173
00174
00175
00176
00177
00178 cout << "MAIN: Starting estimation" << endl;
00179 unsigned int time_step;
00180 for (time_step = 0; time_step < NUM_TIME_STEPS-1; time_step++)
00181 {
00182
00183 mobile_robot.Move(input);
00184
00185
00186 ColumnVector measurement = mobile_robot.Measure();
00187
00188
00189 filter.Update(&sys_model,input,&meas_model,measurement);
00190
00191 }
00192
00193
00194
00195 Pdf<ColumnVector> * posterior = filter.PostGet();
00196 cout << "After " << time_step+1 << " timesteps " << endl;
00197 cout << " Posterior Mean = " << endl << posterior->ExpectedValueGet() << endl
00198 << " Covariance = " << endl << posterior->CovarianceGet() << "" << endl;
00199
00200
00201 cout << "======================================================" << endl
00202 << "End of the Bootstrap filter for mobile robot localisation" << endl
00203 << "======================================================"
00204 << endl;
00205
00206
00207 return 0;
00208 }
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