smoother_test.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Tinne De Laet <first dot last at mech dot kuleuven dot be>
2 //
3 // This program is free software; you can redistribute it and/or modify
5 // the Free Software Foundation; either version 2 of the License, or
6 // (at your option) any later version.
7 //
8 // This program is distributed in the hope that it will be useful,
9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 // GNU General Public License for more details.
12 //
13 // You should have received a copy of the GNU General Public License
14 // along with this program; if not, write to the Free Software
15 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
16 //
17
18 #include "smoother_test.hpp"
19 #include "approxEqual.hpp"
28
29 #include "../examples/mobile_robot_wall_cts.h"
30 #include "../examples/mobile_robot.h"
31
32 // Registers the fixture into the 'registry'
34 using namespace BFL;
35
36
37 void
39 {
40 }
41
42 void
44 {
45 }
46
47 void
49 {
50  double epsilon = 0.01;
51  double epsilon_large = 0.5;
52  double epsilon_huge = 2.0;
53  /* The purpose of this program is to construct a filter for the problem
54  of localisation of a mobile robot equipped with an ultrasonic sensor.
55  In this case the orientation is known, which simplifies the model considerably:
56  The system model will become linear.
57  The ultrasonic measures the distance to the wall (it can be switched off:
58  see mobile_robot_wall_cts.h)
59
60  The necessary SYSTEM MODEL is:
61
62  x_k = x_{k-1} + v_{k-1} * cos(theta) * delta_t
63  y_k = y_{k-1} + v_{k-1} * sin(theta) * delta_t
64
65  The used MEASUREMENT MODEL:
66  measuring the (perpendicular) distance z to the wall y = ax + b
67
68  set WALL_CT = 1/sqrt(pow(a,2) + 1)
69  z = WALL_CT * a * x - WALL_CT * y + WALL_CT * b + GAUSSIAN_NOISE
70  or Z = H * X_k + J * U_k
71
72  where
73
74  H = [ WALL_CT * a - WALL_CT 0 ]
75  and GAUSSIAN_NOISE = N((WALL_CT * b), SIGMA_MEAS_NOISE)
76
77  */
78
79  /****************************
80  * NonLinear system model *
81  ***************************/
82  ColumnVector SysNoise_Mu(STATE_SIZE);
83  SysNoise_Mu = 0.0;
84  SysNoise_Mu(1) = MU_SYSTEM_NOISE_X;
85  SysNoise_Mu(2) = MU_SYSTEM_NOISE_Y;
86  SysNoise_Mu(3) = MU_SYSTEM_NOISE_THETA;
87
88  SymmetricMatrix SysNoise_Cov(STATE_SIZE);
89  SysNoise_Cov = 0.0;
90  // Uncertainty or Noice (Additive) and Matrix A
91  SysNoise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
92  SysNoise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
93  SysNoise_Cov(3,3) = SIGMA_SYSTEM_NOISE_THETA;
94
95  Gaussian System_Uncertainty(SysNoise_Mu, SysNoise_Cov);
96  NonLinearAnalyticConditionalGaussianMobile sys_pdf(System_Uncertainty);
97  AnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);
98
99  /*********************************
100  * Initialise measurement model *
101  ********************************/
102  // Fill up H
103  double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
104  Matrix H(MEAS_SIZE,STATE_SIZE);
105  H = 0.0;
106  H(1,1) = wall_ct * RICO_WALL;
107  H(1,2) = 0 - wall_ct;
108
109  // Construct the measurement noise (a scalar in this case)
110  ColumnVector MeasNoise_Mu(MEAS_SIZE);
111  SymmetricMatrix MeasNoise_Cov(MEAS_SIZE);
112  MeasNoise_Mu(1) = MU_MEAS_NOISE;
113  MeasNoise_Cov(1,1) = SIGMA_MEAS_NOISE;
114
115  Gaussian Measurement_Uncertainty(MeasNoise_Mu,MeasNoise_Cov);
116  LinearAnalyticConditionalGaussian meas_pdf(H,Measurement_Uncertainty);
118
119  /****************************
120  * Linear prior DENSITY *
121  ***************************/
122  // Continuous Gaussian prior (for Kalman filters)
123  ColumnVector prior_mu(STATE_SIZE);
124  SymmetricMatrix prior_sigma(STATE_SIZE);
125  prior_mu(1) = PRIOR_MU_X;
126  prior_mu(2) = PRIOR_MU_Y;
127  prior_mu(STATE_SIZE) = PRIOR_MU_THETA;
128  prior_sigma = 0.0;
129  prior_sigma(1,1) = PRIOR_COV_X;
130  prior_sigma(2,2) = PRIOR_COV_Y;
131  prior_sigma(3,3) = PRIOR_COV_THETA;
132  Gaussian prior_cont(prior_mu,prior_sigma);
133
134  /******************************
135  * Construction of the Filter *
136  ******************************/
137  ExtendedKalmanFilter filter(&prior_cont);
138
139
140  /***************************
141  * initialise MOBILE ROBOT *
142  **************************/
143  // Model of mobile robot in world with one wall
144  // The model is used to simultate the distance measurements.
145  MobileRobot mobile_robot;
146  ColumnVector input(2);
147  input(1) = 0.1;
148  input(2) = 0.0;
149
150  /***************************
151  * vector in which all posteriors will be stored*
152  **************************/
153  vector<Gaussian> posteriors(NUM_TIME_STEPS);
154  vector<Gaussian>::iterator posteriors_it = posteriors.begin();
155
156  /*******************
157  * ESTIMATION LOOP *
158  *******************/
159  unsigned int time_step;
160  for (time_step = 0; time_step < NUM_TIME_STEPS; time_step++)
161  {
162
163  // write posterior to file
164  Gaussian * posterior = (Gaussian*)(filter.PostGet());
165
166  // DO ONE STEP WITH MOBILE ROBOT
167  mobile_robot.Move(input);
168
169  if ((time_step+1)%10 == 0){
170  // DO ONE MEASUREMENT
171  ColumnVector measurement = mobile_robot.Measure();
172
173  // UPDATE FILTER
174  filter.Update(&sys_model,input,&meas_model,measurement);
175  }
176  else{
177  filter.Update(&sys_model,input);
178  }
179
180  // make copy of posterior
181  *posteriors_it = *posterior;
182
183  posteriors_it++;
184  } // estimation loop
185
186
187  Pdf<ColumnVector> * posterior = filter.PostGet();
188
189
190  /***************************************
191  * Construction of the Backward Filter *
192  **************************************/
193  RauchTungStriebel backwardfilter((Gaussian*)posterior);
194
195
196  /*******************
197  * ESTIMATION LOOP *
198  *******************/
199  for (time_step = NUM_TIME_STEPS-1; time_step+1 > 0 ; time_step--)
200  {
201  posteriors_it--;
202  // UPDATE BACKWARDFILTER
203  Gaussian filtered(posteriors_it->ExpectedValueGet(),posteriors_it->CovarianceGet());
204  backwardfilter.Update(&sys_model,input, &filtered);
205  Pdf<ColumnVector> * posterior = backwardfilter.PostGet();
206
207  // make copy of posterior
208  posteriors_it->ExpectedValueSet(posterior->ExpectedValueGet());
209  posteriors_it->CovarianceSet(posterior->CovarianceGet());
210
211  } // estimation loop
212
213  ColumnVector mean_smoother_check(STATE_SIZE);
214  mean_smoother_check(1) = PRIOR_MU_X; mean_smoother_check(2) = PRIOR_MU_Y; mean_smoother_check(3) = PRIOR_MU_THETA;
215  SymmetricMatrix cov_smoother_check(STATE_SIZE);
216  cov_smoother_check=0.0;
217  cov_smoother_check(1,1) = PRIOR_COV_X;
218  CPPUNIT_ASSERT_EQUAL(approxEqual(posteriors_it->ExpectedValueGet(), mean_smoother_check, epsilon_large),true);
219  CPPUNIT_ASSERT_EQUAL(approxEqual(posteriors_it->CovarianceGet(), cov_smoother_check, epsilon_large),true);
220
221 }
222
223 void
225 {
226  // no nice implementation found yet
227 }
#define MU_MEAS_NOISE
double epsilon
#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 MatrixWrapper::SymmetricMatrix CovarianceGet() const
Get the Covariance Matrix E[(x - E[x])^2] of the Analytic pdf.
MatrixWrapper::ColumnVector Measure()
Class representing all Rauch-Tung-Striebel backward filters.
#define MEAS_SIZE
Class representing Gaussian (or normal density)
Definition: gaussian.h:27
virtual Pdf< StateVar > * PostGet()
Get Posterior density.
#define MU_SYSTEM_NOISE_X
void testKalmanSmoother()
#define RICO_WALL
#define MU_SYSTEM_NOISE_THETA
#define SIGMA_SYSTEM_NOISE_THETA
#define STATE_SIZE
Non Linear Conditional Gaussian.
#define PRIOR_COV_THETA
#define PRIOR_COV_Y
virtual bool Update(SystemModel< StateVar > *const sysmodel, const StateVar &u, Pdf< StateVar > *const filtered_post)
Full Update (system with inputs)
virtual T ExpectedValueGet() const
Get the expected value E[x] of the pdf.
void Move(MatrixWrapper::ColumnVector inputs)
Class for analytic system models with additive Gauss. uncertainty.
#define PRIOR_MU_X
virtual Gaussian * PostGet()
Get Posterior density.
#define PRIOR_MU_THETA
#define PRIOR_COV_X
void testParticleSmoother()
Class for linear analytic measurementmodels with additive gaussian noise.
CPPUNIT_TEST_SUITE_REGISTRATION(SmootherTest)
#define SIGMA_SYSTEM_NOISE_X
#define PRIOR_MU_Y
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 approxEqual(double a, double b, double epsilon)
Definition: approxEqual.cpp:20
#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 Jun 10 2019 12:47:59