test_nonlinear_particle.cpp
Go to the documentation of this file.
1 // $Id: test_nonlinear_particle.cpp 5925 2006-03-14 21:23:49Z tdelaet $
2 // Copyright (C) 2006 Tinne De Laet <first dot last at mech dot kuleuven dot be>
3 //
4 // This program is free software; you can redistribute it and/or modify
5 // it under the terms of the GNU Lesser General Public License as published by
6 // the Free Software Foundation; either version 2.1 of the License, or
7 // (at your option) any later version.
8 //
9 // This program is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // GNU Lesser General Public License for more details.
13 //
14 // You should have received a copy of the GNU Lesser General Public License
15 // along with this program; if not, write to the Free Software
16 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
17 
18 /* Demonstration program for the Bayesian Filtering Library.
19  Mobile robot localization with respect to wall with different possibilities for filter
20 */
21 
22 
23 #include <filter/bootstrapfilter.h>
24 
25 #include <model/systemmodel.h>
26 #include <model/measurementmodel.h>
27 
28 #include "nonlinearSystemPdf.h"
30 
31 #include "../mobile_robot.h"
32 
33 #include <iostream>
34 #include <fstream>
35 
36 // Include file with properties
37 #include "../mobile_robot_wall_cts.h"
38 
39 using namespace MatrixWrapper;
40 using namespace BFL;
41 using namespace std;
42 
43 
44 
45 /* The purpose of this program is to construct a kalman filter for the problem
46  of localisation of a mobile robot equipped with an ultrasonic sensor.
47  In this case the orientation is known, which simplifies the model considerably:
48  The system model will become linear.
49  The ultrasonic measures the distance to the wall (it can be switched off:
50  see mobile_robot_wall_cts.h)
51 
52  The necessary SYSTEM MODEL is:
53 
54  x_k = x_{k-1} + v_{k-1} * cos(theta) * delta_t
55  y_k = y_{k-1} + v_{k-1} * sin(theta) * delta_t
56 
57  The used MEASUREMENT MODEL:
58  measuring the (perpendicular) distance z to the wall y = ax + b
59 
60  set WALL_CT = 1/sqrt(pow(a,2) + 1)
61  z = WALL_CT * a * x - WALL_CT * y + WALL_CT * b + GAUSSIAN_NOISE
62  or Z = H * X_k + J * U_k
63 
64  where
65 
66  H = [ WALL_CT * a - WALL_CT 0 ]
67  and GAUSSIAN_NOISE = N((WALL_CT * b), SIGMA_MEAS_NOISE)
68 
69 */
70 
71 
72 int main(int argc, char** argv)
73 {
74  cerr << "==================================================" << endl
75  << "Test of bootstrap filter" << endl
76  << "Mobile robot localisation example" << endl
77  << "==================================================" << endl;
78 
79 
80  /****************************
81  * NonLinear system model *
82  ***************************/
83 
84  // create gaussian
85  ColumnVector sys_noise_Mu(STATE_SIZE);
86  sys_noise_Mu(1) = MU_SYSTEM_NOISE_X;
87  sys_noise_Mu(2) = MU_SYSTEM_NOISE_Y;
88  sys_noise_Mu(3) = MU_SYSTEM_NOISE_THETA;
89 
90  SymmetricMatrix sys_noise_Cov(STATE_SIZE);
91  sys_noise_Cov = 0.0;
92  sys_noise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
93  sys_noise_Cov(1,2) = 0.0;
94  sys_noise_Cov(1,3) = 0.0;
95  sys_noise_Cov(2,1) = 0.0;
96  sys_noise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
97  sys_noise_Cov(2,3) = 0.0;
98  sys_noise_Cov(3,1) = 0.0;
99  sys_noise_Cov(3,2) = 0.0;
100  sys_noise_Cov(3,3) = SIGMA_SYSTEM_NOISE_THETA;
101 
102  Gaussian system_Uncertainty(sys_noise_Mu, sys_noise_Cov);
103 
104  // create the nonlinear system model
105  NonlinearSystemPdf sys_pdf(system_Uncertainty);
106  SystemModel<ColumnVector> sys_model(&sys_pdf);
107 
108 
109  /*********************************
110  * NonLinear Measurement model *
111  ********************************/
112 
113  // create matrix H for linear measurement model
114  double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
115  Matrix H(MEAS_SIZE,STATE_SIZE);
116  H = 0.0;
117  H(1,1) = wall_ct * RICO_WALL;
118  H(1,2) = 0 - wall_ct;
119  H(1,3) = 0.0;
120  // Construct the measurement noise (a scalar in this case)
121  ColumnVector meas_noise_Mu(MEAS_SIZE);
122  meas_noise_Mu(1) = MU_MEAS_NOISE;
123  SymmetricMatrix meas_noise_Cov(MEAS_SIZE);
124  meas_noise_Cov(1,1) = SIGMA_MEAS_NOISE;
125  Gaussian measurement_Uncertainty(meas_noise_Mu, meas_noise_Cov);
126 
127  // create the measurement model
128  LinearAnalyticConditionalGaussian meas_pdf(H, measurement_Uncertainty);
130 
131  /****************************
132  * Linear prior DENSITY *
133  ***************************/
134  // Continuous Gaussian prior (for Kalman filters)
135  ColumnVector prior_Mu(STATE_SIZE);
136  prior_Mu(1) = PRIOR_MU_X;
137  prior_Mu(2) = PRIOR_MU_Y;
138  prior_Mu(3) = PRIOR_MU_THETA;
139  SymmetricMatrix prior_Cov(STATE_SIZE);
140  prior_Cov(1,1) = PRIOR_COV_X;
141  prior_Cov(1,2) = 0.0;
142  prior_Cov(1,3) = 0.0;
143  prior_Cov(2,1) = 0.0;
144  prior_Cov(2,2) = PRIOR_COV_Y;
145  prior_Cov(2,3) = 0.0;
146  prior_Cov(3,1) = 0.0;
147  prior_Cov(3,2) = 0.0;
148  prior_Cov(3,3) = PRIOR_COV_THETA;
149  Gaussian prior_cont(prior_Mu,prior_Cov);
150 
151  // Discrete prior for Particle filter (using the continuous Gaussian prior)
152  vector<Sample<ColumnVector> > prior_samples(NUM_SAMPLES);
154  prior_cont.SampleFrom(prior_samples,NUM_SAMPLES,CHOLESKY,NULL);
155  prior_discr.ListOfSamplesSet(prior_samples);
156 
157  /******************************
158  * Construction of the Filter *
159  ******************************/
160  BootstrapFilter<ColumnVector,ColumnVector> filter(&prior_discr, 0, NUM_SAMPLES/4.0);
161 
162  /***************************
163  * initialise MOBILE ROBOT *
164  **************************/
165  // Model of mobile robot in world with one wall
166  // The model is used to simultate the distance measurements.
167  MobileRobot mobile_robot;
168  ColumnVector input(2);
169  input(1) = 0.1;
170  input(2) = 0.0;
171 
172 
173 
174 
175  /*******************
176  * ESTIMATION LOOP *
177  *******************/
178  cout << "MAIN: Starting estimation" << endl;
179  unsigned int time_step;
180  for (time_step = 0; time_step < NUM_TIME_STEPS-1; time_step++)
181  {
182  // DO ONE STEP WITH MOBILE ROBOT
183  mobile_robot.Move(input);
184 
185  // DO ONE MEASUREMENT
186  ColumnVector measurement = mobile_robot.Measure();
187 
188  // UPDATE FILTER
189  filter.Update(&sys_model,input,&meas_model,measurement);
190 
191  } // estimation loop
192 
193 
194 
195  Pdf<ColumnVector> * posterior = filter.PostGet();
196  cout << "After " << time_step+1 << " timesteps " << endl;
197  cout << " Posterior Mean = " << endl << posterior->ExpectedValueGet() << endl
198  << " Covariance = " << endl << posterior->CovarianceGet() << "" << endl;
199 
200 
201  cout << "======================================================" << endl
202  << "End of the Bootstrap filter for mobile robot localisation" << endl
203  << "======================================================"
204  << endl;
205 
206 
207  return 0;
208 }
Particular particle filter : Proposal PDF = SystemPDF.
#define MU_MEAS_NOISE
#define SIGMA_MEAS_NOISE
Class PDF: Virtual Base class representing Probability Density Functions.
Definition: pdf.h:53
This is a class simulating a mobile robot.
Definition: mobile_robot.h:47
virtual MCPdf< StateVar > * PostGet()
Get Posterior density.
MatrixWrapper::ColumnVector Measure()
Non Linear Conditional Gaussian.
#define MEAS_SIZE
virtual T ExpectedValueGet() const
Get the expected value E[x] of the pdf.
Class representing Gaussian (or normal density)
Definition: gaussian.h:27
#define MU_SYSTEM_NOISE_X
bool SampleFrom(vector< Sample< MatrixWrapper::ColumnVector > > &list_samples, const int num_samples, int method=DEFAULT, void *args=NULL) const
#define RICO_WALL
#define MU_SYSTEM_NOISE_THETA
#define SIGMA_SYSTEM_NOISE_THETA
int main(int argc, char **argv)
#define STATE_SIZE
#define PRIOR_COV_THETA
#define PRIOR_COV_Y
void Move(MatrixWrapper::ColumnVector inputs)
Monte Carlo Pdf: Sample based implementation of Pdf.
Definition: mcpdf.h:49
#define PRIOR_MU_X
#define PRIOR_MU_THETA
#define PRIOR_COV_X
Class for linear analytic measurementmodels with additive gaussian noise.
#define CHOLESKY
#define SIGMA_SYSTEM_NOISE_X
const unsigned int NUM_SAMPLES
Definition: pdf_test.cpp:37
#define PRIOR_MU_Y
virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const
Get the Covariance Matrix E[(x - E[x])^2] of the Analytic pdf.
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)
Definition: filter.h:56
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
#define NUM_TIME_STEPS


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 Mon Feb 28 2022 21:56:33