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 "../mobile_robot_wall_cts.h"
00024
00025 #include <filter/histogramfilter.h>
00026
00027 #include <model/discretesystemmodel.h>
00028
00029 #include <pdf/discretepdf.h>
00030 #include "conditionalUniformMeasPdf1d.h"
00031 #include "../mobile_robot.h"
00032
00033 #include <iostream>
00034 #include <fstream>
00035
00036 using namespace MatrixWrapper;
00037 using namespace BFL;
00038 using namespace std;
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055 int main(int argc, char** argv)
00056 {
00057 cerr << "==================================================" << endl
00058 << "Test of histogram filter" << endl
00059 << "1D Mobile robot localisation example" << endl
00060 << "==================================================" << endl;
00061
00062
00063 int num_states = 20;
00064 int num_cond_args = 1;
00065
00066
00067
00068 int cond_arg_dims[num_cond_args];
00069 cond_arg_dims[0] = num_states;
00070 DiscreteConditionalPdf sys_pdf(num_states,num_cond_args,cond_arg_dims);
00071 std::vector<int> cond_args(num_cond_args);
00072 double prob_diag = 0.9;
00073 double prob_nondiag = (1-prob_diag)/(num_states-1);
00074 for (int state_kMinusOne = 0 ; state_kMinusOne < num_states ; state_kMinusOne++)
00075 {
00076 cond_args[0] = state_kMinusOne;
00077 for (int state_k = 0 ; state_k < num_states ; state_k++)
00078 {
00079 if (state_kMinusOne == state_k) sys_pdf.ProbabilitySet(prob_diag,state_k,cond_args);
00080 else sys_pdf.ProbabilitySet(prob_nondiag,state_k,cond_args);
00081 }
00082 }
00083 DiscreteSystemModel sys_model(&sys_pdf);
00084 std::cerr << "discrete system model created" << std::endl;
00085
00086
00087
00088
00089
00090 ColumnVector measNoise_Mu(MEAS_SIZE);
00091 measNoise_Mu(1) = 0.0;
00092
00093 SymmetricMatrix meas_noise_Cov(MEAS_SIZE);
00094 meas_noise_Cov(1,1) = SIGMA_MEAS_NOISE;
00095 ColumnVector meas_noise_Mu(1);
00096 meas_noise_Mu(1) = MU_MEAS_NOISE;
00097 Gaussian measurement_Uncertainty(meas_noise_Mu, meas_noise_Cov);
00098
00099
00100 ConditionalUniformMeasPdf1d meas_pdf(measurement_Uncertainty);
00101 MeasurementModel<MatrixWrapper::ColumnVector,int> meas_model(&meas_pdf);
00102 std::cout << "measurement model created" << std::endl;
00103
00104
00105
00106
00107 DiscretePdf prior(num_states);
00108 std::cout << "uniform prior density created" << std::endl;
00109
00110
00111
00112
00113 HistogramFilter<ColumnVector> filter(&prior);
00114 DiscretePdf * prior_test = filter.PostGet();
00115 std::cout << "filter created" << std::endl;
00116
00117
00118
00119
00120
00121
00122 MobileRobot mobile_robot;
00123 ColumnVector input(2);
00124 input(1) = 0.1;
00125 input(2) = 0.0;
00126
00127
00128
00129
00130
00131 cout << "MAIN: Starting estimation" << endl;
00132 unsigned int time_step;
00133 for (time_step = 0; time_step < NUM_TIME_STEPS; time_step++)
00134 {
00135
00136 mobile_robot.Move(input);
00137
00138
00139
00140 ColumnVector measurement = mobile_robot.Measure();
00141 measurement(1) = -measurement(1);
00142
00143
00144 filter.Update(&sys_model,&meas_model,measurement);
00145
00146
00147
00148 }
00149
00150
00151
00152 DiscretePdf * posterior = filter.PostGet();
00153 cout << "After " << time_step+1 << " timesteps " << endl;
00154 cout << " Posterior probabilities = " << endl;
00155 for (int state = 0 ; state< posterior->NumStatesGet() ; state++) cout << state << ": " << (double)(posterior->ProbabilityGet(state)) << endl;
00156
00157
00158 cout << "======================================================" << endl
00159 << "End of the Histogram filter for 1D mobile robot localisation" << endl
00160 << "======================================================"
00161 << endl;
00162
00163
00164 return 0;
00165 }
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 Fri Aug 28 2015 10:10:21