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