Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include "ekf_test.hpp"
00019 #include "approxEqual.hpp"
00020
00021 using namespace BFL;
00022 using namespace MatrixWrapper;
00023
00024
00025
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
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
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
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
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