00001 // Copyright (C) 2008 Wim Meeussen <meeussen at willowgarage com> 00002 // 00003 // This program is free software; you can redistribute it and/or modify 00004 // it under the terms of the GNU Lesser General Public License as published by 00005 // the Free Software Foundation; either version 2.1 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 Lesser General Public License for more details. 00012 // 00013 // You should have received a copy of the GNU Lesser 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 <robot_pose_ekf/nonlinearanalyticconditionalgaussianodo.h> 00019 #include <bfl/wrappers/rng/rng.h> // Wrapper around several rng libraries 00020 #define NUMCONDARGUMENTS_MOBILE 2 00021 00022 namespace BFL 00023 { 00024 using namespace MatrixWrapper; 00025 00026 00027 NonLinearAnalyticConditionalGaussianOdo::NonLinearAnalyticConditionalGaussianOdo(const Gaussian& additiveNoise) 00028 : AnalyticConditionalGaussianAdditiveNoise(additiveNoise,NUMCONDARGUMENTS_MOBILE), 00029 df(6,6) 00030 { 00031 // initialize df matrix 00032 for (unsigned int i=1; i<=6; i++){ 00033 for (unsigned int j=1; j<=6; j++){ 00034 if (i==j) df(i,j) = 1; 00035 else df(i,j) = 0; 00036 } 00037 } 00038 } 00039 00040 00041 NonLinearAnalyticConditionalGaussianOdo::~NonLinearAnalyticConditionalGaussianOdo(){} 00042 00043 ColumnVector NonLinearAnalyticConditionalGaussianOdo::ExpectedValueGet() const 00044 { 00045 ColumnVector state = ConditionalArgumentGet(0); 00046 ColumnVector vel = ConditionalArgumentGet(1); 00047 state(1) += cos(state(6)) * vel(1); 00048 state(2) += sin(state(6)) * vel(1); 00049 state(6) += vel(2); 00050 return state + AdditiveNoiseMuGet(); 00051 } 00052 00053 Matrix NonLinearAnalyticConditionalGaussianOdo::dfGet(unsigned int i) const 00054 { 00055 if (i==0)//derivative to the first conditional argument (x) 00056 { 00057 double vel_trans = ConditionalArgumentGet(1)(1); 00058 double yaw = ConditionalArgumentGet(0)(6); 00059 00060 df(1,3)=-vel_trans*sin(yaw); 00061 df(2,3)= vel_trans*cos(yaw); 00062 00063 return df; 00064 } 00065 else 00066 { 00067 if (i >= NumConditionalArgumentsGet()) 00068 { 00069 cerr << "This pdf Only has " << NumConditionalArgumentsGet() << " conditional arguments\n"; 00070 exit(-BFL_ERRMISUSE); 00071 } 00072 else{ 00073 cerr << "The df is not implemented for the" <<i << "th conditional argument\n"; 00074 exit(-BFL_ERRMISUSE); 00075 } 00076 } 00077 } 00078 00079 }//namespace BFL 00080