43 #include "../mobile_robot_wall_cts.h" 47 #include "../mobile_robot.h" 84 int main(
int argc,
char** argv)
86 cerr <<
"==================================================" << endl
87 <<
"Test of switching between different filters" << endl
88 <<
"Mobile robot localisation example" << endl
89 <<
"==================================================" << endl;
95 cout <<
"Please provide one argument. Possible arguments are:" << endl
96 <<
" kalman_filter" << endl
97 <<
" bootstrap_filter" << endl
98 <<
" EK_particle_filter" << endl
99 <<
" ASIR_filter" << endl
100 <<
" IE_kalman_filter" << endl;
104 string argument = argv[1];
105 if (argument ==
"kalman_filter") filter_name =
KALMAN;
106 else if (argument ==
"IE_kalman_filter") filter_name =
IE_KALMAN;
107 else if (argument ==
"bootstrap_filter") filter_name =
BOOTSTRAP;
108 else if (argument ==
"EK_particle_filter") filter_name =
EK_PARTICLE;
109 else if (argument ==
"ASIR_filter") filter_name =
ASIR;
112 cout <<
"Please provide another argument. Possible arguments are:" << endl
113 <<
" kalman_filter" << endl
114 <<
" bootstrap_filter" << endl
115 <<
" EK_particle_filter" << endl
116 <<
" ASIR_filter" << endl
117 <<
" IE_kalman_filter" << endl;
125 ofstream fout_time, fout_E, fout_cov, fout_meas, fout_states, fout_particles, fout_numparticles;
127 fout_time.open(
"time.out");
128 fout_E.open(
"E.out");
129 fout_cov.open(
"cov.out");
130 fout_meas.open(
"meas.out");
131 fout_states.open(
"states.out");
135 fout_particles.open(
"particles.out");
136 fout_numparticles.open(
"numparticles.out");
152 sys_noise_Cov(1,2) = 0.0;
153 sys_noise_Cov(1,3) = 0.0;
154 sys_noise_Cov(2,1) = 0.0;
156 sys_noise_Cov(2,3) = 0.0;
157 sys_noise_Cov(3,1) = 0.0;
158 sys_noise_Cov(3,2) = 0.0;
161 Gaussian system_Uncertainty(sys_noise_Mu, sys_noise_Cov);
170 double wall_ct = 2/(sqrt(pow(
RICO_WALL,2.0) + 1));
174 H(1,2) = 0 - wall_ct;
175 cout<<
"Measurment model H = " << H << endl;
179 SymmetricMatrix MeasNoise_Cov(
MEAS_SIZE);
183 Gaussian Measurement_Uncertainty(MeasNoise_Mu,MeasNoise_Cov);
201 Gaussian prior_cont(prior_mu,prior_sigma);
204 vector<Sample<ColumnVector> > prior_samples(
NUM_SAMPLES);
209 cout<<
"Prior initialised"<<
"" << endl;
211 <<
"Covariance = " << endl << prior_cont.
CovarianceGet() << endl;
225 cout <<
"Mobile robot initialised"<<
"" << endl;
234 switch (filter_name){
236 cout <<
"Using the Extended Kalman Filter" << endl;
240 cout <<
"Using the Iterated Extended Kalman Filter, " <<
NUM_ITERATIONS <<
" iterations" << endl;
244 cout <<
"Using the bootstrapfilter, " <<
NUM_SAMPLES <<
" samples, dynamic resampling" << endl;
248 cout <<
"Using the Extended Particle Kalman Filter, " <<
NUM_SAMPLES <<
" samples, dynamic resampling" << endl;
256 cout <<
"Type if filter not recognised on construction" <<endl;
265 cout <<
"MAIN: Starting estimation" << endl;
266 unsigned int time_step;
270 fout_time << time_step <<
";" << endl;
271 fout_meas << mobile_robot.
Measure()(1) <<
";" << endl;
272 fout_states << mobile_robot.
GetState()(1) <<
"," << mobile_robot.
GetState()(2) <<
"," 273 << mobile_robot.
GetState()(3) <<
";" << endl;
287 fout_numparticles << ((MCPdf<ColumnVector>*)posterior)->NumSamplesGet() <<
";" << endl;
288 vector<WeightedSample<ColumnVector> > samples_list = ((
MCPdf<ColumnVector>*)posterior)->ListOfSamplesGet() ;
289 vector<WeightedSample<ColumnVector> >::iterator sample;
290 for ( sample = samples_list.begin() ; sample != samples_list.end() ; sample++ )
291 fout_particles << sample->ValueGet()(1) <<
"," << sample->ValueGet()(2) <<
"," 292 << sample->ValueGet()(3) <<
"," << sample->WeightGet() <<
";"<<endl;
297 mobile_robot.
Move(input);
300 ColumnVector measurement = mobile_robot.
Measure();
304 my_filter->
Update(&sys_model,input,&meas_model, measurement);
306 my_filter->
Update(&sys_model, input);
312 cout <<
"After " << time_step+1 <<
" timesteps " << endl;
313 cout <<
" Posterior Mean = " << endl << posterior->
ExpectedValueGet() << endl
314 <<
" Covariance = " << endl << posterior->
CovarianceGet() <<
"" << endl;
315 cout <<
"=============================================" << endl;
321 cout <<
"==================================================" << endl
322 <<
"End of the Comparing filters test" << endl
323 <<
"==================================================" 338 fout_particles.close();
339 fout_numparticles.close();
virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const
Get the Covariance Matrix E[(x - E[x])^2] of the Analytic pdf.
MatrixWrapper::ColumnVector GetState()
Particular particle filter : Proposal PDF = SystemPDF.
virtual MatrixWrapper::ColumnVector ExpectedValueGet() const
Get the expected value E[x] of the pdf.
Class PDF: Virtual Base class representing Probability Density Functions.
This is a class simulating a mobile robot.
virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const
Get the Covariance Matrix E[(x - E[x])^2] of the Analytic pdf.
bool SampleFrom(vector< Sample< MatrixWrapper::ColumnVector > > &list_samples, const int num_samples, int method=DEFAULT, void *args=NULL) const
MatrixWrapper::ColumnVector Measure()
Class representing Gaussian (or normal density)
#define MU_SYSTEM_NOISE_X
Particle filter using EKF for proposal step.
#define MU_SYSTEM_NOISE_THETA
#define RESAMPLE_THRESHOLD
#define SIGMA_SYSTEM_NOISE_THETA
virtual Pdf< StateVar > * PostGet()
Get Posterior density.
Non Linear Conditional Gaussian.
virtual T ExpectedValueGet() const
Get the expected value E[x] of the pdf.
void Move(MatrixWrapper::ColumnVector inputs)
int main(int argc, char **argv)
Monte Carlo Pdf: Sample based implementation of Pdf.
Class for analytic system models with additive Gauss. uncertainty.
Class for linear analytic measurementmodels with additive gaussian noise.
Linear Conditional Gaussian.
#define SIGMA_SYSTEM_NOISE_X
const unsigned int NUM_SAMPLES
virtual bool Update(SystemModel< StateVar > *const sysmodel, const StateVar &u, MeasurementModel< MeasVar, StateVar > *const measmodel, const MeasVar &z, const StateVar &s)
Full Update (system with inputs/sensing params)
bool ListOfSamplesSet(const vector< WeightedSample< T > > &list_of_samples)
Set the list of weighted samples.
#define MU_SYSTEM_NOISE_Y
#define SIGMA_SYSTEM_NOISE_Y