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 <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 


robot_pose_ekf
Author(s): Wim Meeussen, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:38