nonlinearanalyticconditionalgaussianodo.cpp
Go to the documentation of this file.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 <wrappers/rng/rng.h> // Wrapper around several rng 00020 // libraries 00021 #define NUMCONDARGUMENTS_MOBILE 2 00022 00023 namespace BFL 00024 { 00025 using namespace MatrixWrapper; 00026 00027 00028 NonLinearAnalyticConditionalGaussianOdo::NonLinearAnalyticConditionalGaussianOdo(const Gaussian& additiveNoise) 00029 : AnalyticConditionalGaussianAdditiveNoise(additiveNoise,NUMCONDARGUMENTS_MOBILE), 00030 df(6,6) 00031 { 00032 // initialize df matrix 00033 for (unsigned int i=1; i<=6; i++){ 00034 for (unsigned int j=1; j<=6; j++){ 00035 if (i==j) df(i,j) = 1; 00036 else df(i,j) = 0; 00037 } 00038 } 00039 } 00040 00041 00042 NonLinearAnalyticConditionalGaussianOdo::~NonLinearAnalyticConditionalGaussianOdo(){} 00043 00044 ColumnVector NonLinearAnalyticConditionalGaussianOdo::ExpectedValueGet() const 00045 { 00046 ColumnVector state = ConditionalArgumentGet(0); 00047 ColumnVector vel = ConditionalArgumentGet(1); 00048 state(1) += cos(state(6)) * vel(1); 00049 state(2) += sin(state(6)) * vel(1); 00050 state(6) += vel(2); 00051 return state + AdditiveNoiseMuGet(); 00052 } 00053 00054 Matrix NonLinearAnalyticConditionalGaussianOdo::dfGet(unsigned int i) const 00055 { 00056 if (i==0)//derivative to the first conditional argument (x) 00057 { 00058 double vel_trans = ConditionalArgumentGet(1)(1); 00059 double yaw = ConditionalArgumentGet(0)(6); 00060 00061 df(1,3)=-vel_trans*sin(yaw); 00062 df(2,3)= vel_trans*cos(yaw); 00063 00064 return df; 00065 } 00066 else 00067 { 00068 if (i >= NumConditionalArgumentsGet()) 00069 { 00070 cerr << "This pdf Only has " << NumConditionalArgumentsGet() << " conditional arguments\n"; 00071 exit(-BFL_ERRMISUSE); 00072 } 00073 else{ 00074 cerr << "The df is not implemented for the" <<i << "th conditional argument\n"; 00075 exit(-BFL_ERRMISUSE); 00076 } 00077 } 00078 } 00079 00080 }//namespace BFL 00081