ekf_test.cpp
Go to the documentation of this file.
00001 // Copyright (C) 2008 Willow Garage inc.
00002 //
00003 // This program is free software; you can redistribute it and/or modify
00004 // it under the terms of the GNU General Public License as published by
00005 // the Free Software Foundation; either version 2 of the License, or
00006 // (at your option) any later version.
00007 //
00008 // This program is distributed in the hope that it will be useful,
00009 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00010 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00011 // GNU General Public License for more details.
00012 //
00013 // You should have received a copy of the GNU General Public License
00014 // along with this program; if not, write to the Free Software
00015 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00016 //
00017 
00018 #include "ekf_test.hpp"
00019 #include "approxEqual.hpp"
00020 
00021 using namespace BFL;
00022 using namespace MatrixWrapper;
00023 
00024 
00025 // Registers the fixture into the 'registry'
00026 CPPUNIT_TEST_SUITE_REGISTRATION( EKFTest );
00027 
00028 static const unsigned int state_size = 6;
00029 static const unsigned int meas_size = 4;
00030 static const double epsilon = 0.0001;
00031 
00032 
00033 
00034 
00035 void EKFTest::setUp()
00036 {
00037   assert(meas_size <= state_size);
00038 
00039   // create MEASUREMENT MODEL
00040   ColumnVector measNoise_Mu(meas_size);      measNoise_Mu = 0;
00041   SymmetricMatrix measNoise_Cov(meas_size);  measNoise_Cov = 0;
00042   Matrix meas_matrix(meas_size, state_size); meas_matrix = 0;
00043   for (unsigned int i=1; i<=meas_size; i++){
00044     measNoise_Mu(i) = 9.2-i;
00045     measNoise_Cov(i,i) = pow(2.8, 2);
00046     meas_matrix(i,i) = 2.8-3*i;
00047   }
00048   Gaussian measurement_Uncertainty(measNoise_Mu, measNoise_Cov);
00049   meas_pdf_   = new LinearAnalyticConditionalGaussian(meas_matrix, measurement_Uncertainty);
00050   meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(meas_pdf_);
00051 
00052   // create EKF filter
00053   ColumnVector prior_Mu(state_size);      prior_Mu = 0;
00054   SymmetricMatrix prior_Cov(state_size);  prior_Cov = 0;
00055   for (unsigned int i=1; i<=state_size; i++) {
00056     prior_Mu(i) = 29+i*i*i;
00057     prior_Cov(i,i) = pow(29.34*i+23.23, 2);
00058   }
00059   prior_  = new Gaussian(prior_Mu,prior_Cov);
00060   filter_ = new ExtendedKalmanFilter(prior_);
00061 }
00062 
00063 
00064 
00065 void EKFTest::tearDown()
00066 {
00067   delete filter_;
00068   delete meas_model_;
00069   delete meas_pdf_;
00070   delete prior_;
00071 }
00072 
00073 
00074 
00075 void EKFTest::testMeasUpdate()
00076 {
00077   ColumnVector meas(meas_size);
00078   for (unsigned int i=1; i<= meas_size; i++)
00079     meas(i) = 2.4*i;
00080   filter_->Update(meas_model_, meas);
00081 
00082 
00083   // expected value should be like this
00084   ColumnVector expectedvalue(state_size);
00085   expectedvalue(1) = 29.066225;
00086   expectedvalue(2) = 0.754136;
00087   expectedvalue(3) = -0.160365;
00088   expectedvalue(4) = -0.477823;
00089   expectedvalue(5) = 154.000000;
00090   expectedvalue(6) = 245.000000;
00091   CPPUNIT_ASSERT_EQUAL(approxEqual(expectedvalue, filter_->PostGet()->ExpectedValueGet(), epsilon), true);
00092 
00093   // covariance should be like this
00094   SymmetricMatrix covariance(state_size);  covariance = 0;
00095   covariance(1,1) = 183.019889;
00096   covariance(2,2) = 0.765538;
00097   covariance(3,3) = 0.203951;
00098   covariance(4,4) = 0.092627;
00099   covariance(5,5) = 28876.204900;
00100   covariance(6,6) = 39708.532900;
00101   CPPUNIT_ASSERT_EQUAL(approxEqual(covariance, filter_->PostGet()->CovarianceGet(), epsilon), true);
00102 }


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 11 2019 03:45:12