ekf_test.cpp
Go to the documentation of this file.
1 // Copyright (C) 2008 Willow Garage inc.
2 //
3 // This program is free software; you can redistribute it and/or modify
4 // it under the terms of the GNU General Public License as published by
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 "ekf_test.hpp"
19 #include "approxEqual.hpp"
20 
21 using namespace BFL;
22 using namespace MatrixWrapper;
23 
24 
25 // Registers the fixture into the 'registry'
27 
28 static const unsigned int state_size = 6;
29 static const unsigned int meas_size = 4;
30 static const double epsilon = 0.0001;
31 
32 
33 
34 
36 {
37  assert(meas_size <= state_size);
38 
39  // create MEASUREMENT MODEL
40  ColumnVector measNoise_Mu(meas_size); measNoise_Mu = 0;
41  SymmetricMatrix measNoise_Cov(meas_size); measNoise_Cov = 0;
42  Matrix meas_matrix(meas_size, state_size); meas_matrix = 0;
43  for (unsigned int i=1; i<=meas_size; i++){
44  measNoise_Mu(i) = 9.2-i;
45  measNoise_Cov(i,i) = pow(2.8, 2);
46  meas_matrix(i,i) = 2.8-3*i;
47  }
48  Gaussian measurement_Uncertainty(measNoise_Mu, measNoise_Cov);
49  meas_pdf_ = new LinearAnalyticConditionalGaussian(meas_matrix, measurement_Uncertainty);
50  meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(meas_pdf_);
51 
52  // create EKF filter
53  ColumnVector prior_Mu(state_size); prior_Mu = 0;
54  SymmetricMatrix prior_Cov(state_size); prior_Cov = 0;
55  for (unsigned int i=1; i<=state_size; i++) {
56  prior_Mu(i) = 29+i*i*i;
57  prior_Cov(i,i) = pow(29.34*i+23.23, 2);
58  }
59  prior_ = new Gaussian(prior_Mu,prior_Cov);
60  filter_ = new ExtendedKalmanFilter(prior_);
61 }
62 
63 
64 
66 {
67  delete filter_;
68  delete meas_model_;
69  delete meas_pdf_;
70  delete prior_;
71 }
72 
73 
74 
76 {
77  ColumnVector meas(meas_size);
78  for (unsigned int i=1; i<= meas_size; i++)
79  meas(i) = 2.4*i;
80  filter_->Update(meas_model_, meas);
81 
82 
83  // expected value should be like this
84  ColumnVector expectedvalue(state_size);
85  expectedvalue(1) = 29.066225;
86  expectedvalue(2) = 0.754136;
87  expectedvalue(3) = -0.160365;
88  expectedvalue(4) = -0.477823;
89  expectedvalue(5) = 154.000000;
90  expectedvalue(6) = 245.000000;
91  CPPUNIT_ASSERT_EQUAL(approxEqual(expectedvalue, filter_->PostGet()->ExpectedValueGet(), epsilon), true);
92 
93  // covariance should be like this
94  SymmetricMatrix covariance(state_size); covariance = 0;
95  covariance(1,1) = 183.019889;
96  covariance(2,2) = 0.765538;
97  covariance(3,3) = 0.203951;
98  covariance(4,4) = 0.092627;
99  covariance(5,5) = 28876.204900;
100  covariance(6,6) = 39708.532900;
101  CPPUNIT_ASSERT_EQUAL(approxEqual(covariance, filter_->PostGet()->CovarianceGet(), epsilon), true);
102 }
static const unsigned int state_size
Definition: ekf_test.cpp:28
Class representing Gaussian (or normal density)
Definition: gaussian.h:27
void setUp()
Definition: ekf_test.cpp:35
static const unsigned int meas_size
Definition: ekf_test.cpp:29
void testMeasUpdate()
Definition: ekf_test.cpp:75
void tearDown()
Definition: ekf_test.cpp:65
CPPUNIT_TEST_SUITE_REGISTRATION(EKFTest)
Class for linear analytic measurementmodels with additive gaussian noise.
static const double epsilon
Definition: ekf_test.cpp:30
bool approxEqual(double a, double b, double epsilon)
Definition: approxEqual.cpp:20


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