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/extendedkalmanfilter.h>
00024
00025 #include <model/linearanalyticsystemmodel_gaussianuncertainty.h>
00026 #include <model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
00027
00028 #include <pdf/analyticconditionalgaussian.h>
00029 #include <pdf/analyticconditionalgaussian.h>
00030 #include <pdf/linearanalyticconditionalgaussian.h>
00031
00032 #include "../compare_filters/nonlinearanalyticconditionalgaussianmobile.h"
00033 #include "../mobile_robot.h"
00034
00035 #include <smoother/rauchtungstriebel.h>
00036 #include <smoother/particlesmoother.h>
00037
00038 #include <iostream>
00039 #include <fstream>
00040
00041
00042 #include "../examples/mobile_robot_wall_cts.h"
00043
00044 using namespace MatrixWrapper;
00045 using namespace BFL;
00046 using namespace std;
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
00073
00074
00075 int main(int argc, char** argv)
00076 {
00077 cerr << "==================================================" << endl
00078 << "Test of different smooters" << endl
00079 << "Mobile robot localisation example" << endl
00080 << "==================================================" << endl;
00081
00082
00083
00084
00085 ofstream fout_time, fout_E, fout_cov, fout_meas, fout_states, fout_particles, fout_numparticles, fout_E_smooth, fout_cov_smooth, fout_time_smooth;
00086
00087 fout_time.open("time.out");
00088 fout_E.open("E.out");
00089 fout_cov.open("cov.out");
00090 fout_meas.open("meas.out");
00091 fout_states.open("states.out");
00092 fout_E_smooth.open("Esmooth.out");
00093 fout_cov_smooth.open("covsmooth.out");
00094 fout_time_smooth.open("timesmooth.out");
00095
00096
00097
00098
00099
00100
00101 ColumnVector sys_noise_Mu(STATE_SIZE);
00102 sys_noise_Mu = 0.0;
00103 sys_noise_Mu(1) = MU_SYSTEM_NOISE_X;
00104 sys_noise_Mu(2) = MU_SYSTEM_NOISE_Y;
00105 sys_noise_Mu(3) = MU_SYSTEM_NOISE_THETA;
00106
00107 SymmetricMatrix sys_noise_Cov(STATE_SIZE);
00108 sys_noise_Cov = 0.0;
00109 sys_noise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
00110 sys_noise_Cov(1,2) = 0.0;
00111 sys_noise_Cov(1,3) = 0.0;
00112 sys_noise_Cov(2,1) = 0.0;
00113 sys_noise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
00114 sys_noise_Cov(2,3) = 0.0;
00115 sys_noise_Cov(3,1) = 0.0;
00116 sys_noise_Cov(3,2) = 0.0;
00117 sys_noise_Cov(3,3) = SIGMA_SYSTEM_NOISE_THETA;
00118
00119 Gaussian system_Uncertainty(sys_noise_Mu, sys_noise_Cov);
00120
00121
00122 NonLinearAnalyticConditionalGaussianMobile sys_pdf(system_Uncertainty);
00123 AnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);
00124
00125
00126
00127
00128
00129 double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
00130 Matrix H(MEAS_SIZE,STATE_SIZE);
00131 H = 0.0;
00132 H(1,1) = wall_ct * RICO_WALL;
00133 H(1,2) = 0 - wall_ct;
00134
00135
00136 ColumnVector MeasNoise_Mu(MEAS_SIZE);
00137 SymmetricMatrix MeasNoise_Cov(MEAS_SIZE);
00138 MeasNoise_Mu(1) = MU_MEAS_NOISE;
00139 MeasNoise_Cov(1,1) = SIGMA_MEAS_NOISE;
00140
00141 Gaussian measurement_Uncertainty(MeasNoise_Mu,MeasNoise_Cov);
00142
00143
00144 LinearAnalyticConditionalGaussian meas_pdf(H, measurement_Uncertainty);
00145 LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);
00146
00147
00148
00149
00150
00151
00152 ColumnVector prior_mu(STATE_SIZE);
00153 SymmetricMatrix prior_sigma(STATE_SIZE);
00154 prior_mu(1) = PRIOR_MU_X;
00155 prior_mu(2) = PRIOR_MU_Y;
00156 prior_sigma = 0.0;
00157 prior_sigma(1,1) = PRIOR_COV_X;
00158 prior_sigma(2,2) = PRIOR_COV_Y;
00159 prior_sigma(3,3) = PRIOR_COV_THETA;
00160 Gaussian prior_cont(prior_mu,prior_sigma);
00161
00162
00163 cout<< "Prior initialised"<< "" << endl;
00164 cout << "Prior Mean = " << endl << prior_cont.ExpectedValueGet() << endl
00165 << "Covariance = " << endl << prior_cont.CovarianceGet() << endl;
00166
00167
00168
00169
00170 ExtendedKalmanFilter filter(&prior_cont);
00171
00172
00173
00174
00175
00176
00177
00178
00179 MobileRobot mobile_robot;
00180 ColumnVector input(2);
00181 input(1) = 0.1;
00182 input(2) = 0.0;
00183
00184
00185
00186
00187 vector<Gaussian> posteriors(NUM_TIME_STEPS);
00188 vector<Gaussian>::iterator posteriors_it = posteriors.begin();
00189
00190
00191
00192
00193 cout << "MAIN: Starting estimation" << endl;
00194 unsigned int time_step;
00195 for (time_step = 0; time_step < NUM_TIME_STEPS; time_step++)
00196 {
00197
00198 fout_time << time_step << ";" << endl;
00199 fout_meas << mobile_robot.Measure()(1) << ";" << endl;
00200 fout_states << mobile_robot.GetState()(1) << "," << mobile_robot.GetState()(2) << ","
00201 << mobile_robot.GetState()(3) << ";" << endl;
00202
00203
00204 Gaussian * posterior = (Gaussian*)(filter.PostGet());
00205 fout_E << posterior->ExpectedValueGet()(1) << "," << posterior->ExpectedValueGet()(2)<< ","
00206 << posterior->ExpectedValueGet()(3) << ";" << endl;
00207 fout_cov << posterior->CovarianceGet()(1,1) << "," << posterior->CovarianceGet()(1,2) << ","
00208 << posterior->CovarianceGet()(1,3) << "," << posterior->CovarianceGet()(2,2) << ","
00209 << posterior->CovarianceGet()(2,3) << "," << posterior->CovarianceGet()(3,3) << ";" << endl;
00210
00211
00212 mobile_robot.Move(input);
00213
00214 if ((time_step+1)%10 == 0){
00215
00216 ColumnVector measurement = mobile_robot.Measure();
00217
00218
00219 filter.Update(&sys_model,input,&meas_model,measurement);
00220 }
00221 else{
00222 filter.Update(&sys_model,input);
00223 }
00224
00225
00226
00227 *posteriors_it = *posterior;
00228
00229 posteriors_it++;
00230 }
00231
00232
00233 Pdf<ColumnVector> * posterior = filter.PostGet();
00234
00235 fout_time << time_step << ";" << endl;
00236 fout_meas << mobile_robot.Measure()(1) << ";" << endl;
00237 fout_states << mobile_robot.GetState()(1) << "," << mobile_robot.GetState()(2) << ","
00238 << mobile_robot.GetState()(3) << ";" << endl;
00239
00240
00241 fout_E << posterior->ExpectedValueGet()(1) << "," << posterior->ExpectedValueGet()(2)<< ","
00242 << posterior->ExpectedValueGet()(3) << ";" << endl;
00243 fout_cov << posterior->CovarianceGet()(1,1) << "," << posterior->CovarianceGet()(1,2) << ","
00244 << posterior->CovarianceGet()(1,3) << "," << posterior->CovarianceGet()(2,2) << ","
00245 << posterior->CovarianceGet()(2,3) << "," << posterior->CovarianceGet()(3,3) << ";" << endl;
00246
00247 cout << "After " << time_step+1 << " timesteps " << endl;
00248 cout << " Posterior Mean = " << endl << posterior->ExpectedValueGet() << endl
00249 << " Covariance = " << endl << posterior->CovarianceGet() << "" << endl;
00250
00251
00252 cout << "======================================================" << endl
00253 << "End of the filter for mobile robot localisation" << endl
00254 << "======================================================"
00255 << endl;
00256
00257
00258
00259
00260
00261 RauchTungStriebel backwardfilter((Gaussian*)posterior);
00262
00263 fout_time_smooth << time_step << ";" << endl;
00264
00265 fout_E_smooth << posterior->ExpectedValueGet()(1) << "," << posterior->ExpectedValueGet()(2)<< ","
00266 << posterior->ExpectedValueGet()(3) << ";" << endl;
00267 fout_cov_smooth << posterior->CovarianceGet()(1,1) << "," << posterior->CovarianceGet()(1,2) << ","
00268 << posterior->CovarianceGet()(1,3) << "," << posterior->CovarianceGet()(2,2) << ","
00269 << posterior->CovarianceGet()(2,3) << "," << posterior->CovarianceGet()(3,3) << ";" << endl;
00270
00271
00272
00273
00274 cout << "======================================================" << endl
00275 << "Start of the smoother for mobile robot localisation" << endl
00276 << "======================================================"
00277 << endl;
00278 for (time_step = NUM_TIME_STEPS-1; time_step+1 > 0 ; time_step--)
00279 {
00280 posteriors_it--;
00281
00282 Gaussian filtered(posteriors_it->ExpectedValueGet(),posteriors_it->CovarianceGet());
00283 backwardfilter.Update(&sys_model,input, &filtered);
00284 Pdf<ColumnVector> * posterior = backwardfilter.PostGet();
00285
00286 fout_time_smooth << time_step << ";" << endl;
00287
00288 fout_E_smooth << posterior->ExpectedValueGet()(1) << "," << posterior->ExpectedValueGet()(2)<< ","
00289 << posterior->ExpectedValueGet()(3) << ";" << endl;
00290 fout_cov_smooth << posterior->CovarianceGet()(1,1) << "," << posterior->CovarianceGet()(1,2) << ","
00291 << posterior->CovarianceGet()(1,3) << "," << posterior->CovarianceGet()(2,2) << ","
00292 << posterior->CovarianceGet()(2,3) << "," << posterior->CovarianceGet()(3,3) << ";" << endl;
00293
00294
00295 posteriors_it->ExpectedValueSet(posterior->ExpectedValueGet());
00296 posteriors_it->CovarianceSet(posterior->CovarianceGet());
00297
00298 }
00299
00300 cout << "After Smoothing first timestep " << endl;
00301 cout << " Posterior Mean = " << endl << posteriors_it->ExpectedValueGet() << endl
00302 << " Covariance = " << endl << posteriors_it->CovarianceGet() << "" << endl;
00303
00304
00305
00306 cout << "======================================================" << endl
00307 << "End of the Kalman smoother for mobile robot localisation" << endl
00308 << "======================================================"
00309 << endl;
00310 return 0;
00311
00312
00313
00314
00315 fout_time.close();
00316 fout_E.close();
00317 fout_cov.close();
00318 fout_meas.close();
00319 fout_states.close();
00320 fout_time_smooth.close();
00321 fout_E_smooth.close();
00322 fout_cov_smooth.close();
00323 }